remove use of parameter

src/teaching/pose_tf_listener.cpp

@@ -12,45 +12,53 @@
 
 void collect(ros::NodeHandle node_handle, geometry_msgs::TransformStamped t) {
 
-    ros::ServiceClient client = node_handle.serviceClient<panda_teaching::SavePose>("SaveCollectedPose");
-    panda_teaching::SavePose srv;
+  ros::ServiceClient client = node_handle.serviceClient<panda_teaching::SavePose>("SaveCollectedPose");
+  panda_teaching::SavePose srv;
 
-    srv.request.pose.position.x = t.transform.translation.x;
-    srv.request.pose.position.y = t.transform.translation.y;
-    srv.request.pose.position.z = t.transform.translation.z;
+  srv.request.pose.position.x = t.transform.translation.x;
+  srv.request.pose.position.y = t.transform.translation.y;
+  srv.request.pose.position.z = t.transform.translation.z;
 
-    srv.request.pose.orientation = t.transform.rotation;
+  srv.request.pose.orientation = t.transform.rotation;
 
-    if (!client.call(srv)) {
-        ROS_ERROR("Failed to call pose_storage_service.");
-        return;
-    }
-}
-
-void collectPose(ros::NodeHandle &node_handle){
-  node_handle.setParam("collect_pose", true);
+  if (!client.call(srv)) {
+    ROS_ERROR("Failed to call pose_storage_service.");
+    return;
+  }
 }
 
-void cleanUp(ros::NodeHandle &node_handle){
+void cleanUp(ros::NodeHandle &node_handle) {
   ros::ServiceClient client = node_handle.serviceClient<panda_teaching::DeletePose>("DeleteCollectedPoses");
   panda_teaching::DeletePose srv;
 
-  if (!client.call(srv)){
+  if (!client.call(srv)) {
     ROS_ERROR("Failed to delete saved poses");
   }
 }
 
-void collectorCallback(const std_msgs::String::ConstPtr& msg, 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){
+  if (msg->data.compare("collect") == 0) {
     ROS_INFO("Collecting new pose for gripper...");
-    collectPose(node_handle);
+    try {
+      geometry_msgs::TransformStamped transformStamped = tfBuffer.lookupTransform("world", "panda_hand", ros::Time(0));
+
+      std::cout << "Collected pose: " << transformStamped << std::endl;
+      collect(node_handle, transformStamped);
+    }
+    catch (tf2::TransformException &ex) {
+      ROS_ERROR_STREAM("transform_error: " << ex.what() << std::endl);
+      ros::Duration(1.0).sleep();
+    }
     return;
   }
 
-  if(msg->data.compare("clear") == 0){
+  if (msg->data.compare("clear") == 0) {
     ROS_INFO("Deleting saved poses...");
     cleanUp(node_handle);
     return;
@@ -58,42 +66,16 @@ void collectorCallback(const std_msgs::String::ConstPtr& msg, ros::NodeHandle &n
 }
 
 int main(int argc, char **argv) {
-    ros::init(argc, argv, "pose_tf_listener");
-
-    ros::NodeHandle node_handle;
+  ros::init(argc, argv, "pose_tf_listener");
 
+  ros::NodeHandle node_handle;
 
   ros::Subscriber sub = node_handle.subscribe<std_msgs::String>("collectPose", 1000, boost::bind(&collectorCallback, _1, boost::ref(node_handle)));
 
-    tf2_ros::Buffer tfBuffer;
-    tf2_ros::TransformListener tfListener(tfBuffer);
-
-    ros::Rate rate(10.0);
-
-    while (node_handle.ok()) {
+  tf2_ros::TransformListener tfListener(tfBuffer);
 
-        geometry_msgs::TransformStamped transformStamped;
+  ros::Rate rate(10.0);
 
-        try {
-            // world == (0, 0, 0, 0)
-            transformStamped = tfBuffer.lookupTransform("world", "panda_hand",
-                                                        ros::Time(0));
-
-            bool collect_pose = false;
-            node_handle.getParam("collect_pose", collect_pose);
-
-            if (collect_pose) {
-                std::cout << "Collected pose: " << transformStamped << std::endl;
-                node_handle.setParam("collect_pose", false);
-                collect(node_handle, transformStamped);
-            }
-        }
-        catch (tf2::TransformException &ex) {
-            std::cout << "transform_error: " << ex.what() << std::endl;
-            ros::Duration(1.0).sleep();
-            continue;
-        }
-        ros::spinOnce();
-    }
-    return 0;
-};
\ No newline at end of file
+  ros::spin();
+  return 0;
+};