thomas/Cobots
2
1
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity

AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA

Browse source
This commit is contained in:
Thomas Muller 2022-03-22 20:38:54 -04:00
parent 25a94b161d
commit a0ee92d53a
2 changed files with 1 additions and 37 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
src/ar3_config/launch/dev.launch.py
View file

@ -134,8 +134,6 @@ def generate_launch_description():
get_yaml(join(config_root, 'trajectory.yaml')), get_yaml(join(config_root, 'trajectory.yaml')),
get_yaml(join(config_root, 'planning_scene.yaml')), get_yaml(join(config_root, 'planning_scene.yaml')),
get_yaml(join(config_root, "ar3_controllers.yaml")), get_yaml(join(config_root, "ar3_controllers.yaml")),
{ "default_planning_pipeline": "ompl" },
{ "planning_pipelines": {"pipeline_names":["ompl"], "ompl": {"planning_plugin": "ompl_interface/OMPLPlanner"} } }
], ],
), ),

36
src/move_group_interface/src/move_group_interface_node.cpp
View file

@ -35,18 +35,16 @@ public:
pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>( pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
"move_pose", "move_pose",
rclcpp::QoS(1), rclcpp::QoS(1),
std::bind(&ScanNPlan::moveCallbackHowItShouldWork, this, std::placeholders::_1), std::bind(&ScanNPlan::moveCallback, this, std::placeholders::_1),
options options
); );
/*
relative_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>( relative_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
"move_relative_pose", "move_relative_pose",
rclcpp::QoS(1), rclcpp::QoS(1),
std::bind(&ScanNPlan::relativeMoveCallback, this, std::placeholders::_1), std::bind(&ScanNPlan::relativeMoveCallback, this, std::placeholders::_1),
options options
); );
*/
} }
void init() { void init() {
@ -58,38 +56,6 @@ private:
void moveCallback(geometry_msgs::msg::PoseStamped::SharedPtr p) { void moveCallback(geometry_msgs::msg::PoseStamped::SharedPtr p) {
RCLCPP_INFO(this->get_logger(), "GOT MSG"); RCLCPP_INFO(this->get_logger(), "GOT MSG");
move_group_->setStartStateToCurrentState();
move_group_->setPlanningTime(20);
move_group_->setNumPlanningAttempts(50);
move_group_->setMaxVelocityScalingFactor(1.0);
move_group_->setMaxAccelerationScalingFactor(1.0);
std::vector<geometry_msgs::msg::Pose> waypoints;
waypoints.push_back(p->pose);
moveit_msgs::msg::RobotTrajectory trajectory;
double fraction = move_group_->computeCartesianPath(waypoints, 0.01, 0.0, trajectory, true);
if(fraction >= 1.0) {
RCLCPP_INFO(this->get_logger(), "Did the carts %f%%", fraction * 100.);
robot_trajectory::RobotTrajectory rt(move_group_->getRobotModel(), move_group_->getName());
rt.setRobotTrajectoryMsg(*move_group_->getCurrentState(10), trajectory);
trajectory_processing::IterativeParabolicTimeParameterization iptp;
bool success =
iptp.computeTimeStamps(rt, 0.1, 0.1);
RCLCPP_INFO(this->get_logger(), "Got stamps %s", success ? "YES" : "NO");
moveit::planning_interface::MoveGroupInterface::Plan plan;
rt.getRobotTrajectoryMsg(plan.trajectory_);
move_group_->execute(plan);
}
//move_group_->move();
}
void moveCallbackHowItShouldWork(geometry_msgs::msg::PoseStamped::SharedPtr p) {
RCLCPP_INFO(this->get_logger(), "GOT MSG");
move_group_->setStartStateToCurrentState(); move_group_->setStartStateToCurrentState();
move_group_->setPoseTarget(p->pose); move_group_->setPoseTarget(p->pose);