merge tf and collector files

.gitignore

@@ -2,6 +2,7 @@
 # Reference: https://intellij-support.jetbrains.com/hc/en-us/articles/206544839
 
 # User-specific stuff
+.idea
 .idea/**/workspace.xml
 .idea/**/tasks.xml
 .idea/**/usage.statistics.xml

CMakeLists.txt

@@ -63,7 +63,6 @@ catkin_package(CATKIN_DEPENDS
 include_directories(${catkin_INCLUDE_DIRS})
 
 add_executable(pose_storage_service src/teaching/pose_storage_service.cpp)
-add_executable(pose_collector src/teaching/pose_collector.cpp)
 add_executable(pose_replayer src/teaching/pose_replayer.cpp)
 add_executable(pose_tf_listener src/teaching/pose_tf_listener.cpp)
 
@@ -73,4 +72,3 @@ add_dependencies(pose_tf_listener panda_teaching_generate_messages_cpp)
 target_link_libraries(pose_storage_service ${catkin_LIBRARIES})
 target_link_libraries(pose_tf_listener ${catkin_LIBRARIES})
 target_link_libraries(pose_replayer ${catkin_LIBRARIES})
-target_link_libraries(pose_collector ${catkin_LIBRARIES})

launch/simulation.launch

@@ -59,7 +59,6 @@
     <node pkg="panda_simulation" type="robot_state_initializer_node" name="robot_state_initializer_node" />
 
     <node pkg="panda_teaching" type="pose_tf_listener" name="pose_tf_listener" output="screen" />
-    <node pkg="panda_teaching" type="pose_collector" name="pose_collector" output="screen" />
     <node pkg="panda_teaching" type="pose_replayer" name="pose_replayer" output="screen" />
     <node pkg="panda_teaching" type="pose_storage_service" name="pose_storage_service" output="screen" />
 </launch>

src/teaching/pose_collector.cpp

-//
-// Created by sebastian on 04.05.20.
-//
-#include <ros/ros.h>
-#include <std_msgs/String.h>
-
-#include "panda_teaching/DeletePose.h"
-
-void collectPose(ros::NodeHandle &node_handle){
-    node_handle.setParam("collect_pose", true);
-}
-
-void cleanUp(ros::NodeHandle &node_handle){
-    ros::ServiceClient client = node_handle.serviceClient<panda_teaching::DeletePose>("DeleteCollectedPoses");
-    panda_teaching::DeletePose srv;
-
-    if (!client.call(srv)){
-        ROS_ERROR("Failed to delete saved poses");
-    }
-}
-
-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...");
-        collectPose(node_handle);
-        return;
-    }
-
-    if(msg->data.compare("clear") == 0){
-        ROS_INFO("Deleting saved poses...");
-        cleanUp(node_handle);
-        return;
-    }
-}
-
-int main(int argc, char** argv) {
-
-    ros::init(argc, argv, "pose_collector");
-    ros::NodeHandle node_handle;
-
-    ros::Subscriber sub = node_handle.subscribe<std_msgs::String>("collectPose", 1000, boost::bind(&collectorCallback, _1, boost::ref(node_handle)));
-    ros::spin();
-
-    return 0;
-}
\ No newline at end of file

src/teaching/pose_tf_listener.cpp

@@ -5,8 +5,10 @@
 #include <tf2_ros/transform_listener.h>
 #include <geometry_msgs/TransformStamped.h>
 #include <geometry_msgs/Twist.h>
+#include <std_msgs/String.h>
 
 #include "panda_teaching/SavePose.h"
+#include "panda_teaching/DeletePose.h"
 
 void collect(ros::NodeHandle node_handle, geometry_msgs::TransformStamped t) {
 
@@ -25,11 +27,44 @@ void collect(ros::NodeHandle node_handle, geometry_msgs::TransformStamped t) {
     }
 }
 
+void collectPose(ros::NodeHandle &node_handle){
+  node_handle.setParam("collect_pose", true);
+}
+
+void cleanUp(ros::NodeHandle &node_handle){
+  ros::ServiceClient client = node_handle.serviceClient<panda_teaching::DeletePose>("DeleteCollectedPoses");
+  panda_teaching::DeletePose srv;
+
+  if (!client.call(srv)){
+    ROS_ERROR("Failed to delete saved poses");
+  }
+}
+
+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...");
+    collectPose(node_handle);
+    return;
+  }
+
+  if(msg->data.compare("clear") == 0){
+    ROS_INFO("Deleting saved poses...");
+    cleanUp(node_handle);
+    return;
+  }
+}
+
 int main(int argc, char **argv) {
     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);
 
@@ -58,6 +93,7 @@ int main(int argc, char **argv) {
             ros::Duration(1.0).sleep();
             continue;
         }
+        ros::spinOnce();
     }
     return 0;
 };
\ No newline at end of file