Refactored zed_transform to pcl_transform and added a new param

src/sb_gazebo/CMakeLists.txt

@@ -41,13 +41,12 @@ include_directories(
 
 ## Declare a C++ executable
 # add_executable(gazebo_node src/gazebo_node.cpp)
-add_executable(zed_transform src/zed_transform.cpp)
+
 
 ## Specify libraries to link a library or executable target against
 # target_link_libraries(gazebo_node
 #   ${catkin_LIBRARIES}
 # )
-target_link_libraries(zed_transform ${catkin_LIBRARIES} ${sb_utils_LIBRARIES})
 
 
 #############

src/sb_gazebo/launch/elsax_control.launch

@@ -19,7 +19,7 @@
   </include>
 
   <!-- This is for jfrost's depthcamera/kinect/zed plugin -->
-  <node name="zed_transform" pkg="sb_gazebo" type="zed_transform" output="screen">
+  <node name="zed_transform" pkg="sb_pointcloud_processing" type="pcl_transform" output="screen">
     <rosparam param="output_frame"> "zed_pointcloud" </rosparam>
 
     <remap from="/input_pointcloud" to="/zed/camera/point_cloud/uncorrected_cloud_do_not_use"/>

src/sb_gazebo/launch/jfrost_control.launch

@@ -19,7 +19,7 @@
   </include>
 
   <!-- This is for jfrost's depthcamera/kinect/zed plugin -->
-  <node name="zed_transform" pkg="sb_gazebo" type="zed_transform" output="screen">
+  <node name="zed_transform" pkg="sb_pointcloud_processing" type="pcl_transform" output="screen">
     <rosparam param="output_frame"> "zed_pointcloud" </rosparam>
 
     <remap from="/input_pointcloud" to="/zed/camera/point_cloud/uncorrected_cloud_do_not_use"/>

src/sb_pointcloud_processing/CMakeLists.txt

@@ -29,6 +29,11 @@ find_package(catkin REQUIRED COMPONENTS
   pcl_msgs
   sensor_msgs
   mapping_igvc
+  geometry_msgs
+  tf2
+  tf2_ros
+  tf2_msgs
+  tf2_sensor_msgs
   )
 find_package(PCL 1.3 REQUIRED COMPONENTS
   common
@@ -69,47 +74,55 @@ add_executable(line_extractor_node
 )
 
 add_library(sb_pointcloud_processing
-    include/rgb_to_hsv.h
-    src/rgb_to_hsv.cpp
-    include/ColourspaceConverter.h
-    src/ColourspaceConverter.cpp
-)
-
-target_link_libraries(sb_pointcloud_processing
-    ${catkin_LIBRARIES}
-    ${sb_utils_LIBRARIES}
-    ${PCL_LIBRARIES}
+  include/rgb_to_hsv.h
+  src/rgb_to_hsv.cpp
+  include/ColourspaceConverter.h
+  src/ColourspaceConverter.cpp
 )
 
 add_executable(test_pcl_generator_node
   src/test_pcl_generator_node.cpp
   test/TestUtils.h
-  )
+)
+
+add_executable(pcl_transform src/pcl_transform.cpp)
 
 ## Specify libraries to link a library or executable target against
 target_link_libraries(line_extractor_node
-    ${catkin_LIBRARIES}
-    ${PCL_COMMON_LIBRARIES}
-    ${PCL_IO_LIBRARIES}
+  ${catkin_LIBRARIES}
+  ${PCL_COMMON_LIBRARIES}
+  ${PCL_IO_LIBRARIES}
 )
 
 add_dependencies(line_extractor_node
-    ${mapping_igvc_EXPORTED_TARGETS}
+  ${mapping_igvc_EXPORTED_TARGETS}
 )
 
+target_link_libraries(sb_pointcloud_processing
+  ${catkin_LIBRARIES}
+  ${sb_utils_LIBRARIES}
+  ${PCL_LIBRARIES}
+)
+
+
 install(
-    TARGETS
-        sb_pointcloud_processing
-    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  TARGETS
+      sb_pointcloud_processing
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
 target_link_libraries(test_pcl_generator_node
   ${catkin_LIBRARIES}
   ${PCL_COMMON_LIBRARIES}
   ${PCL_IO_LIBRARIES}
-  )
+)
+
+target_link_libraries(pcl_transform
+  ${catkin_LIBRARIES}
+  ${sb_utils_LIBRARIES}
+)
 
 #############
 ## Testing ##

src/sb_gazebo/src/zed_transform.cpp → src/sb_pointcloud_processing/src/pcl_transform.cpp

@@ -14,38 +14,54 @@
 #include <sb_utils.h>
 
 ros::Publisher pub;
-std::string output_frame;
 tf2_ros::Buffer tf_buffer;
 
+std::string output_frame;
+
+// The amount of time (s) for a new pointcloud transform to be published.
+double transform_period;
+
 void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg) {
     try {
         // Create an empty pointcloud
         sensor_msgs::PointCloud2 output;
         // Transform the pointcloud to the requested frame
         geometry_msgs::TransformStamped tf_stamped = tf_buffer.lookupTransform(
-        msg->header.frame_id, output_frame, ros::Time(0), ros::Duration(1.0));
+        msg->header.frame_id, output_frame, ros::Time(0), ros::Duration(0.1));
+
         tf2::doTransform(*msg, output, tf_stamped);
 
         // Publish the transformed pointcloud
         pub.publish(output);
-    } catch (tf2::TransformException ex){
+
+        ros::Duration(transform_period).sleep();
+    } catch (tf2::TransformException ex) {
         ROS_WARN("%s", ex.what());
         ros::Duration(1.0).sleep();
     }
 }
 
-int main(int argc, char** argv){
+int main(int argc, char** argv) {
     ros::init(argc, argv, "pointcloud_transformer");
     ros::NodeHandle private_nh("~");
-    if (!SB_getParam(private_nh, "output_frame", output_frame)){
+    if (!SB_getParam(private_nh, "output_frame", output_frame)) {
         // Error and exit if we didn't get a frame to transform to.
-        // We need this to transform anything, and there is no reasonable default
-        ROS_ERROR("Param 'output_frame' not provided. " \
-                  "Can't  transform anything without a frame to transform it to");
+        // We need this to transform anything, and there is no reasonable
+        // default
+        ROS_ERROR(
+        "Param 'output_frame' not provided. "
+        "Can't  transform anything without a frame to transform it to");
         return 1;
     }
-    ros::Subscriber sub = private_nh.subscribe("/input_pointcloud", 1, pointCloudCallback);
-    pub = private_nh.advertise<sensor_msgs::PointCloud2>("/output_pointcloud", 1);
+
+    double default_transform_period = 0;
+    SB_getParam(
+    private_nh, "transform_period", transform_period, default_transform_period);
+
+    ros::Subscriber sub =
+    private_nh.subscribe("/input_pointcloud", 1, pointCloudCallback);
+    pub =
+    private_nh.advertise<sensor_msgs::PointCloud2>("/output_pointcloud", 1);
     tf2_ros::TransformListener tfListener(tf_buffer);
     ros::spin();
     return 0;