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;
@@ -42,16 +48,20 @@ void replayCallback(const std_msgs::String::ConstPtr &msg, ros::NodeHandle &node
       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::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();

src/teaching/pose_storage_service.cpp

@@ -29,8 +29,8 @@ bool deleteCollectedPoses(panda_teaching::DeletePose::Request &req,
 
 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);

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);