#include "std_msgs/String.h" #include <moveit/move_group_interface/move_group_interface.h> void move_toCallback(const std_msgs::String::ConstPtr& msg) { ROS_INFO("INTRO PACKAGE: Recevied message: '%s'", msg->data.c_str()); static const std::string PLANNING_GROUP = "panda_arm"; moveit::planning_interface::MoveGroupInterface group(PLANNING_GROUP); ros::Duration(3.0).sleep(); group.setStartStateToCurrentState(); group.setPlanningTime(5.0); // pose A geometry_msgs::Pose target_pose_1 = group.getCurrentPose().pose; target_pose_1.position.z = 0.4; target_pose_1.position.y = 0.4; target_pose_1.position.x = 0.8; target_pose_1.orientation.x = 0; target_pose_1.orientation.y = 0; target_pose_1.orientation.z = 0; target_pose_1.orientation.w = 1; // pose B geometry_msgs::Pose target_pose_2 = group.getCurrentPose().pose; target_pose_2.position.z = 0.4; target_pose_2.position.y = 0.4; target_pose_2.position.x = 0.8; target_pose_2.orientation.x = 0; target_pose_2.orientation.y = 0; target_pose_2.orientation.z = 0; target_pose_2.orientation.w = 0; std::vector<geometry_msgs::Pose> waypoints; std::string strA ("A"); std::string strB ("B"); if (strA == msg->data) { waypoints.push_back(target_pose_1); } else if (strB == msg->data) { waypoints.push_back(target_pose_2); } else { ROS_WARN("INTRO 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); ROS_INFO("INTRO PACKAGE: Finished movement."); } int main(int argc, char** argv) { ros::init(argc, argv, "intro_package"); ros::NodeHandle node_handle; ros::AsyncSpinner spinner(1); spinner.start(); ros::Subscriber sub = node_handle.subscribe("move_to", 1000, move_toCallback); ROS_INFO("INTRO PACKAGE: Listening to 'move_to'."); ros::waitForShutdown(); return 0; }