Moved PCL Transformation into the sb_pointcloud_processing package (#371)

RobynCastro · web-flow · commit 5133423241c4 · 2019-02-23T13:02:45.000-08:00
* Revert "Revert "Pegged librealsense Version as a Temporary Fix (#359)" (#361)" This reverts commit 863ae21. * Changed setup_realsense realsense version to a working version * Updated realsense version * Fixed install_tools script * Refactored zed_transform to pcl_transform and added a new param * Fixed output frame_id for pcl_transform * Update src/sb_pointcloud_processing/src/pcl_transform.cpp Removed space between time and (s) Co-Authored-By: RobynCastro <robyny3k@gmail.com> * Reordered CMakeLists.txt and changed when pcl_transform will log messages
diff --git a/src/sb_gazebo/CMakeLists.txt b/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})
 
 
 #############
diff --git a/src/sb_gazebo/launch/elsax_control.launch b/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"/>
diff --git a/src/sb_gazebo/launch/jfrost_control.launch b/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"/>
diff --git a/src/sb_pointcloud_processing/CMakeLists.txt b/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
@@ -58,14 +63,24 @@ include_directories(
 ## With catkin_make all packages are built within a single CMake context
 ## The recommended prefix ensures that target names across packages don't collide
 add_executable(line_extractor_node
-  include/LineExtractorNode.h
-  include/DBSCAN.h
-  include/Regression.h
-  src/line_extractor_node.cpp
-  src/LineExtractorNode.cpp
-  src/DBSCAN.cpp
-  src/Regression.cpp
-  include/Regression.h
+    include/LineExtractorNode.h
+    include/DBSCAN.h
+    include/Regression.h
+    src/line_extractor_node.cpp
+    src/LineExtractorNode.cpp
+    src/DBSCAN.cpp
+    src/Regression.cpp
+    include/Regression.h
+)
+
+target_link_libraries(line_extractor_node
+    ${catkin_LIBRARIES}
+    ${PCL_COMMON_LIBRARIES}
+    ${PCL_IO_LIBRARIES}
+)
+
+add_dependencies(line_extractor_node
+    ${mapping_igvc_EXPORTED_TARGETS}
 )
 
 add_library(sb_pointcloud_processing
@@ -81,35 +96,33 @@ target_link_libraries(sb_pointcloud_processing
     ${PCL_LIBRARIES}
 )
 
-add_executable(test_pcl_generator_node
-  src/test_pcl_generator_node.cpp
-  test/TestUtils.h
-  )
-
-## Specify libraries to link a library or executable target against
-target_link_libraries(line_extractor_node
-    ${catkin_LIBRARIES}
-    ${PCL_COMMON_LIBRARIES}
-    ${PCL_IO_LIBRARIES}
-)
-
-add_dependencies(line_extractor_node
-    ${mapping_igvc_EXPORTED_TARGETS}
-)
 
 install(
     TARGETS
-        sb_pointcloud_processing
+    sb_pointcloud_processing
     ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
     LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
     RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
+add_executable(test_pcl_generator_node
+    src/test_pcl_generator_node.cpp
+    test/TestUtils.h
+)
+
 target_link_libraries(test_pcl_generator_node
-  ${catkin_LIBRARIES}
-  ${PCL_COMMON_LIBRARIES}
-  ${PCL_IO_LIBRARIES}
-  )
+    ${catkin_LIBRARIES}
+    ${PCL_COMMON_LIBRARIES}
+    ${PCL_IO_LIBRARIES}
+)
+
+
+add_executable(pcl_transform src/pcl_transform.cpp)
+
+target_link_libraries(pcl_transform
+    ${catkin_LIBRARIES}
+    ${sb_utils_LIBRARIES}
+)
 
 #############
 ## Testing ##
diff --git a/src/sb_pointcloud_processing/src/pcl_transform.cpp b/src/sb_pointcloud_processing/src/pcl_transform.cpp
@@ -14,38 +14,59 @@
 #include <sb_utils.h>
 
 ros::Publisher pub;
-std::string output_frame;
 tf2_ros::Buffer tf_buffer;
 
+std::string output_frame;
+ros::Time last_exception_time;
+
+// 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);
 
+        output.header.frame_id = output_frame;
+
         // Publish the transformed pointcloud
         pub.publish(output);
-    } catch (tf2::TransformException ex){
-        ROS_WARN("%s", ex.what());
-        ros::Duration(1.0).sleep();
+
+        ros::Duration(transform_period).sleep();
+    } catch (tf2::TransformException ex) {
+        if (ros::Time::now().toSec() - last_exception_time.toSec() > 1.0)
+            ROS_WARN("%s", ex.what());
+
+        last_exception_time = ros::Time::now();
     }
 }
 
-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;
