add config file, use params, rename messages

config/robot_information.yaml

+    panda_teaching:
+        planning_group: "panda_arm"
+        pose_element: "panda_hand"

launch/simulation.launch

@@ -27,6 +27,8 @@
     <!-- Load joint controller configurations from YAML file to parameter server -->
     <rosparam file="$(find panda_simulation)/config/panda_control.yaml" command="load" />
 
+    <!-- Load teaching robot information from YAML file to parameter server -->
+    <rosparam file="$(find panda_teaching)/config/robot_information.yaml" command="load" />
 
     <!-- load the controllers -->
     <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller panda_arm_controller" />

src/teaching/pose_replayer.cpp

@@ -13,8 +13,14 @@
 
 void replayCallback(const std_msgs::String::ConstPtr &msg, ros::NodeHandle &node_handle) {
 
-    static const std::string PLANNING_GROUP = "panda_arm";
-    moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
+  if (msg->data.compare("replay") == 0) {
+    std::string planningGroup;
+    if (!node_handle.getParam("planning_group", planningGroup)) {
+      ROS_ERROR("Could not get string value for panda_teaching/planning_group from param server");
+      return;
+    }
+
+    moveit::planning_interface::MoveGroupInterface move_group(planningGroup);
     moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
 
     std::vector<geometry_msgs::Pose> poses;
@@ -23,37 +29,41 @@ void replayCallback(const std_msgs::String::ConstPtr &msg, ros::NodeHandle &node
     panda_teaching::GetCollectedPoses srv;
 
     if (client.call(srv)) {
-        for (int i = 0; i < srv.response.pose_array.poses.size(); i++) {
-            poses.push_back(srv.response.pose_array.poses[i]);
-        }
+      for (int i = 0; i < srv.response.pose_array.poses.size(); i++) {
+        poses.push_back(srv.response.pose_array.poses[i]);
+      }
     } else {
-        ROS_ERROR("Failed to fetch saved poses");
-        return;
+      ROS_ERROR("Failed to fetch saved poses");
+      return;
     }
 
     for (int i = 0; i < srv.response.pose_array.poses.size(); i++) {
 
-        move_group.setStartState(*move_group.getCurrentState());
-        geometry_msgs::Pose another_pose = srv.response.pose_array.poses[i];
-        move_group.setPoseTarget(another_pose);
+      move_group.setStartState(*move_group.getCurrentState());
+      geometry_msgs::Pose another_pose = srv.response.pose_array.poses[i];
+      move_group.setPoseTarget(another_pose);
 
-        moveit::planning_interface::MoveGroupInterface::Plan my_plan;
-        bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
-        ROS_INFO_NAMED("pose_replayer", "Visualizing plan %s", success ? "" : "FAILED");
-        move_group.move();
+      moveit::planning_interface::MoveGroupInterface::Plan my_plan;
+      bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
+      ROS_INFO_NAMED("pose_replayer", "Visualizing plan %s", success ? "" : "FAILED");
+      move_group.move();
     }
+  } else {
+    ROS_ERROR_STREAM("pose_replayer received unknown panda_teaching/poseReplayer message " << msg->data);
+  }
 }
 
 int main(int argc, char **argv) {
 
-    ros::init(argc, argv, "pose_replayer");
-    ros::NodeHandle node_handle;
-    ros::AsyncSpinner spinner(2);
-    spinner.start();
+  ros::init(argc, argv, "pose_replayer");
+  ros::NodeHandle node_handle("panda_teaching");
+
+  ros::AsyncSpinner spinner(2);
+  spinner.start();
 
-    ros::Subscriber sub = node_handle.subscribe<std_msgs::String>("replayPose", 1000, boost::bind(&replayCallback, _1,
+  ros::Subscriber sub = node_handle.subscribe<std_msgs::String>("poseReplayer", 1000, boost::bind(&replayCallback, _1,
                                                                                                   boost::ref(
-                                                                                                          node_handle)));
-    ros::waitForShutdown();
-    return 0;
+                                                                                                      node_handle)));
+  ros::waitForShutdown();
+  return 0;
 }

src/teaching/pose_storage_service.cpp

@@ -9,35 +9,35 @@ std::vector<geometry_msgs::Pose> poses;
 bool getCollectedPoses(panda_teaching::GetCollectedPoses::Request &req,
                        panda_teaching::GetCollectedPoses::Response &res) {
 
-    res.pose_array.poses = poses;
-    return true;
+  res.pose_array.poses = poses;
+  return true;
 }
 
 bool saveCollectedPose(panda_teaching::SavePose::Request &req,
                        panda_teaching::SavePose::Response &res) {
 
-    poses.push_back(req.pose);
-    return true;
+  poses.push_back(req.pose);
+  return true;
 }
 
 bool deleteCollectedPoses(panda_teaching::DeletePose::Request &req,
                           panda_teaching::DeletePose::Response &res) {
 
-    poses.clear();
-    return true;
+  poses.clear();
+  return true;
 }
 
 int main(int argc, char **argv) {
 
-    ros::init(argc, argv, "pose_service_service");
-    ros::NodeHandle n;
+  ros::init(argc, argv, "pose_storage_service");
+  ros::NodeHandle n("panda_teaching");
 
-    ros::ServiceServer retrieve_service = n.advertiseService("GetCollectedPoses", getCollectedPoses);
-    ros::ServiceServer save_service = n.advertiseService("SaveCollectedPose", saveCollectedPose);
-    ros::ServiceServer delete_service = n.advertiseService("DeleteCollectedPoses", deleteCollectedPoses);
+  ros::ServiceServer retrieve_service = n.advertiseService("GetCollectedPoses", getCollectedPoses);
+  ros::ServiceServer save_service = n.advertiseService("SaveCollectedPose", saveCollectedPose);
+  ros::ServiceServer delete_service = n.advertiseService("DeleteCollectedPoses", deleteCollectedPoses);
 
-    ROS_INFO("Ready to collect.");
-    ros::spin();
+  ROS_INFO("Ready to collect.");
+  ros::spin();
 
-    return 0;
+  return 0;
 }

src/teaching/pose_tf_listener.cpp

@@ -9,6 +9,9 @@
 #include "panda_teaching/SavePose.h"
 #include "panda_teaching/DeletePose.h"
 
+// global variable. should be put in class
+tf2_ros::Buffer tfBuffer;
+
 void collect(ros::NodeHandle node_handle, geometry_msgs::TransformStamped t) {
 
   ros::ServiceClient client = node_handle.serviceClient<panda_teaching::SavePose>("SaveCollectedPose");
@@ -35,19 +38,22 @@ void cleanUp(ros::NodeHandle &node_handle) {
   }
 }
 
-// global variable. should be put in class
-tf2_ros::Buffer tfBuffer;
-
 void collectorCallback(const std_msgs::String::ConstPtr &msg, ros::NodeHandle &node_handle) {
 
   ROS_INFO("Retrieved new pose-message...");
 
-  if (msg->data.compare("collect") == 0) {
-    ROS_INFO("Collecting new pose for gripper...");
+  std::string sourceFrame;
+  if (!node_handle.getParam("pose_element", sourceFrame)) {
+    ROS_ERROR("Could not get string value for panda_teaching/pose_element from param server");
+    return;
+  }
+
+  if (msg->data.compare("add") == 0) {
+    ROS_INFO_STREAM("Recording new pose for " << sourceFrame << "...");
     try {
-      geometry_msgs::TransformStamped transformStamped = tfBuffer.lookupTransform("world", "panda_hand", ros::Time(0));
+      geometry_msgs::TransformStamped transformStamped = tfBuffer.lookupTransform("world", sourceFrame, ros::Time(0));
 
-      std::cout << "Collected pose: " << transformStamped << std::endl;
+      std::cout << "Added pose: " << transformStamped << std::endl;
       collect(node_handle, transformStamped);
     }
     catch (tf2::TransformException &ex) {
@@ -55,17 +61,20 @@ void collectorCallback(const std_msgs::String::ConstPtr &msg, ros::NodeHandle &n
       ros::Duration(1.0).sleep();
     }
   } else if (msg->data.compare("clear") == 0) {
-    ROS_INFO("Deleting saved poses...");
+    ROS_INFO_STREAM("Deleting saved poses...");
     cleanUp(node_handle);
+  } else {
+    ROS_ERROR_STREAM("pose_tf_listener received unknown panda_teaching/poseRecorder message " << msg->data);
   }
 }
 
 int main(int argc, char **argv) {
   ros::init(argc, argv, "pose_tf_listener");
+  ros::NodeHandle node_handle("panda_teaching");
 
-  ros::NodeHandle node_handle;
-
-  ros::Subscriber sub = node_handle.subscribe<std_msgs::String>("collectPose", 1000, boost::bind(&collectorCallback, _1, boost::ref(node_handle)));
+  ros::Subscriber sub = node_handle.subscribe<std_msgs::String>("poseRecorder", 1000,
+                                                                boost::bind(&collectorCallback, _1,
+                                                                            boost::ref(node_handle)));
 
   tf2_ros::TransformListener tfListener(tfBuffer);