Move to pose on message

src/IntroPackage.cpp

+#include "std_msgs/String.h"
 #include <moveit/move_group_interface/move_group_interface.h>
 
 
-int main(int argc, char** argv)
+void move_toCallback(const std_msgs::String::ConstPtr& msg)
 {
-    ros::init(argc, argv, "intro_package");
-    ros::NodeHandle node_handle;
-    ros::AsyncSpinner spinner(1);
-    spinner.start();
+    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);
@@ -16,24 +14,60 @@ int main(int argc, char** argv)
     group.setStartStateToCurrentState();
     group.setPlanningTime(5.0);
 
-    std::vector<geometry_msgs::Pose> waypoints;
-
-    geometry_msgs::Pose start_pose = group.getCurrentPose().pose;
+    // 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;
 
-    target_pose_1.position.z = 0.6;
-    target_pose_1.position.y = -0.6;
-    target_pose_1.position.x = 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("FINISHED");
+    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();
 
-    ros::shutdown();
     return 0;
 }