diff --git a/adapter_package/src/adapter_package.cpp b/adapter_package/src/adapter_package.cpp index 4b37a9d419a6230e4eb06ef325fef51469a61559..ac45da75811caaf772f2590d5ec37ff253f431b5 100644 --- a/adapter_package/src/adapter_package.cpp +++ b/adapter_package/src/adapter_package.cpp @@ -38,20 +38,14 @@ void move_toCallback(const std_msgs::String::ConstPtr& msg) std::string strA ("A"); std::string strB ("B"); if (strA == msg->data) { - waypoints.push_back(target_pose_1); + group.setPoseTarget(target_pose_1); } else if (strB == msg->data) { - waypoints.push_back(target_pose_2); + group.setPoseTarget(target_pose_2); } else { ROS_WARN("ADAPTER PACKAGE: Wrong message."); return; } - ///moveit::planning_interface::MoveGroupInterface::Plan plan; - //moveit_msgs::RobotTrajectory trajectory_msg; - //group.computeCartesianPath(waypoints, 0.01, 0.0, trajectory_msg, true); - //plan.trajectory_ = trajectory_msg; - //group.execute(plan); - group.setPoseTarget(target_pose_2); group.move(); ROS_INFO("ADAPTER PACKAGE: Finished movement.");