Unify tf frame parameters between transform and cloud nodes (#344) (#453

) * Unify tf frame parameters between transform and cloud nodes At this point there is no need any more for cloud node because transform node includes all features of cloud node. * Reformat code slightly to avoid roslint errors * Use boost bind instead of lambdas to add support for kinetic * Fix some search and replace errors in comments * Add fixed_frame and target_frame params back * Use more common ROS terminology instead of 'htm' * Undo renaming of publisher and subscriber * Just use tf listener when necessary * Migrate package to tf2 * Explicitly store sensor frame in a dedicated variable to make code easier to read * Reformat code slightly to avoid roslint errors * Avoid unnecessary transforms when sensor_frame == target_frame * Fix comments Co-authored-by: anre <andreas.reich@unibw.de> Co-authored-by: AndreasR30 <andreas-reich@live.de> Co-authored-by: anre <andreas.reich@unibw.de>
ros-drivers · Dec 4, 2022 · 76cec0a · 76cec0a
1 parent f149faf
commit 76cec0a
Show file tree

Hide file tree

Showing 40 changed files with 248 additions and 856 deletions.
diff --git a/velodyne/launch/velodyne-all-nodes-VLP16-composed-launch.py b/velodyne/launch/velodyne-all-nodes-VLP16-composed-launch.py
@@ -47,9 +47,9 @@ def generate_launch_description():
         driver_params = yaml.safe_load(f)['velodyne_driver_node']['ros__parameters']
 
     convert_share_dir = ament_index_python.packages.get_package_share_directory('velodyne_pointcloud')
-    convert_params_file = os.path.join(convert_share_dir, 'config', 'VLP16-velodyne_convert_node-params.yaml')
+    convert_params_file = os.path.join(convert_share_dir, 'config', 'VLP16-velodyne_transform_node-params.yaml')
     with open(convert_params_file, 'r') as f:
-        convert_params = yaml.safe_load(f)['velodyne_convert_node']['ros__parameters']
+        convert_params = yaml.safe_load(f)['velodyne_transform_node']['ros__parameters']
     convert_params['calibration'] = os.path.join(convert_share_dir, 'params', 'VLP16db.yaml')
 
     laserscan_share_dir = ament_index_python.packages.get_package_share_directory('velodyne_laserscan')
@@ -70,8 +70,8 @@ def generate_launch_description():
                     parameters=[driver_params]),
                 ComposableNode(
                     package='velodyne_pointcloud',
-                    plugin='velodyne_pointcloud::Convert',
-                    name='velodyne_convert_node',
+                    plugin='velodyne_pointcloud::Transform',
+                    name='velodyne_transform_node',
                     parameters=[convert_params]),
                 ComposableNode(
                     package='velodyne_laserscan',

diff --git a/velodyne/launch/velodyne-all-nodes-VLP16-launch.py b/velodyne/launch/velodyne-all-nodes-VLP16-launch.py
@@ -48,14 +48,14 @@ def generate_launch_description():
                                                    parameters=[driver_params_file])
 
     convert_share_dir = ament_index_python.packages.get_package_share_directory('velodyne_pointcloud')
-    convert_params_file = os.path.join(convert_share_dir, 'config', 'VLP16-velodyne_convert_node-params.yaml')
+    convert_params_file = os.path.join(convert_share_dir, 'config', 'VLP16-velodyne_transform_node-params.yaml')
     with open(convert_params_file, 'r') as f:
-        convert_params = yaml.safe_load(f)['velodyne_convert_node']['ros__parameters']
+        convert_params = yaml.safe_load(f)['velodyne_transform_node']['ros__parameters']
     convert_params['calibration'] = os.path.join(convert_share_dir, 'params', 'VLP16db.yaml')
-    velodyne_convert_node = launch_ros.actions.Node(package='velodyne_pointcloud',
-                                                    executable='velodyne_convert_node',
-                                                    output='both',
-                                                    parameters=[convert_params])
+    velodyne_transform_node = launch_ros.actions.Node(package='velodyne_pointcloud',
+                                                      executable='velodyne_transform_node',
+                                                      output='both',
+                                                      parameters=[convert_params])
 
     laserscan_share_dir = ament_index_python.packages.get_package_share_directory('velodyne_laserscan')
     laserscan_params_file = os.path.join(laserscan_share_dir, 'config', 'default-velodyne_laserscan_node-params.yaml')
@@ -66,7 +66,7 @@ def generate_launch_description():
 
 
     return launch.LaunchDescription([velodyne_driver_node,
-                                     velodyne_convert_node,
+                                     velodyne_transform_node,
                                      velodyne_laserscan_node,
 
                                      launch.actions.RegisterEventHandler(

diff --git a/velodyne/launch/velodyne-all-nodes-VLP32C-composed-launch.py b/velodyne/launch/velodyne-all-nodes-VLP32C-composed-launch.py
@@ -47,9 +47,9 @@ def generate_launch_description():
         driver_params = yaml.safe_load(f)['velodyne_driver_node']['ros__parameters']
 
     convert_share_dir = ament_index_python.packages.get_package_share_directory('velodyne_pointcloud')
-    convert_params_file = os.path.join(convert_share_dir, 'config', 'VLP32C-velodyne_convert_node-params.yaml')
+    convert_params_file = os.path.join(convert_share_dir, 'config', 'VLP32C-velodyne_transform_node-params.yaml')
     with open(convert_params_file, 'r') as f:
-        convert_params = yaml.safe_load(f)['velodyne_convert_node']['ros__parameters']
+        convert_params = yaml.safe_load(f)['velodyne_transform_node']['ros__parameters']
     convert_params['calibration'] = os.path.join(convert_share_dir, 'params', 'VeloView-VLP-32C.yaml')
 
     laserscan_share_dir = ament_index_python.packages.get_package_share_directory('velodyne_laserscan')
@@ -70,8 +70,8 @@ def generate_launch_description():
                     parameters=[driver_params]),
                 ComposableNode(
                     package='velodyne_pointcloud',
-                    plugin='velodyne_pointcloud::Convert',
-                    name='velodyne_convert_node',
+                    plugin='velodyne_pointcloud::Transform',
+                    name='velodyne_transform_node',
                     parameters=[convert_params]),
                 ComposableNode(
                     package='velodyne_laserscan',

diff --git a/velodyne/launch/velodyne-all-nodes-VLP32C-launch.py b/velodyne/launch/velodyne-all-nodes-VLP32C-launch.py
@@ -48,14 +48,14 @@ def generate_launch_description():
                                                    parameters=[driver_params_file])
 
     convert_share_dir = ament_index_python.packages.get_package_share_directory('velodyne_pointcloud')
-    convert_params_file = os.path.join(convert_share_dir, 'config', 'VLP32C-velodyne_convert_node-params.yaml')
+    convert_params_file = os.path.join(convert_share_dir, 'config', 'VLP32C-velodyne_transform_node-params.yaml')
     with open(convert_params_file, 'r') as f:
-        convert_params = yaml.safe_load(f)['velodyne_convert_node']['ros__parameters']
+        convert_params = yaml.safe_load(f)['velodyne_transform_node']['ros__parameters']
     convert_params['calibration'] = os.path.join(convert_share_dir, 'params', 'VeloView-VLP-32C.yaml')
-    velodyne_convert_node = launch_ros.actions.Node(package='velodyne_pointcloud',
-                                                    executable='velodyne_convert_node',
-                                                    output='both',
-                                                    parameters=[convert_params])
+    velodyne_transform_node = launch_ros.actions.Node(package='velodyne_pointcloud',
+                                                      executable='velodyne_transform_node',
+                                                      output='both',
+                                                      parameters=[convert_params])
 
     laserscan_share_dir = ament_index_python.packages.get_package_share_directory('velodyne_laserscan')
     laserscan_params_file = os.path.join(laserscan_share_dir, 'config', 'default-velodyne_laserscan_node-params.yaml')
@@ -66,7 +66,7 @@ def generate_launch_description():
 
 
     return launch.LaunchDescription([velodyne_driver_node,
-                                     velodyne_convert_node,
+                                     velodyne_transform_node,
                                      velodyne_laserscan_node,
 
                                      launch.actions.RegisterEventHandler(

diff --git a/velodyne_driver/tests/pcap_32e_nodelet_hertz.test b/velodyne_driver/tests/pcap_32e_nodelet_hertz.test
@@ -3,7 +3,7 @@
 
 <launch>
 
-  <!-- start nodelet manager, driver and pointcloud nodelets -->
+  <!-- start nodelet manager, driver and transform nodelets -->
   <include file="$(find velodyne_driver)/launch/nodelet_manager.launch">
     <arg name="model" value="32E"/>
     <arg name="pcap" value="$(find velodyne_driver)/tests/32e.pcap"/>

diff --git a/velodyne_driver/tests/pcap_nodelet_hertz.test b/velodyne_driver/tests/pcap_nodelet_hertz.test
@@ -3,7 +3,7 @@
 
 <launch>
 
-  <!-- start nodelet manager, driver and pointcloud nodelets -->
+  <!-- start nodelet manager, driver and transform nodelets -->
   <include file="$(find velodyne_driver)/launch/nodelet_manager.launch">
     <arg name="pcap" value="$(find velodyne_driver)/tests/class.pcap"/>
   </include>

diff --git a/velodyne_driver/tests/pcap_vlp16_nodelet_hertz.test b/velodyne_driver/tests/pcap_vlp16_nodelet_hertz.test
@@ -3,7 +3,7 @@
 
 <launch>
 
-  <!-- start nodelet manager, driver and pointcloud nodelets -->
+  <!-- start nodelet manager, driver and transform nodelets -->
   <include file="$(find velodyne_driver)/launch/nodelet_manager.launch">
     <arg name="model" value="VLP16"/>
     <arg name="pcap" value="$(find velodyne_driver)/tests/vlp16.pcap"/>

diff --git a/velodyne_pointcloud/CMakeLists.txt b/velodyne_pointcloud/CMakeLists.txt
@@ -45,38 +45,6 @@ include_directories(include ${PCL_COMMON_INCLUDE_DIRS})
 
 add_subdirectory(src/lib)
 
-add_library(convert SHARED
-  src/conversions/convert.cpp)
-ament_target_dependencies(convert
-  diagnostic_updater
-  Eigen3
-  rclcpp
-  rclcpp_components
-  tf2
-  velodyne_msgs
-)
-target_link_libraries(convert
-  velodyne_cloud_types
-  velodyne_rawdata
-  ${YAML_CPP_LIBRARIES})
-install(TARGETS convert
-  ARCHIVE DESTINATION lib
-  LIBRARY DESTINATION lib
-  RUNTIME DESTINATION bin
-)
-rclcpp_components_register_nodes(convert
-  "velodyne_pointcloud::Convert")
-
-add_executable(velodyne_convert_node
-  src/conversions/convert_node.cpp)
-ament_target_dependencies(velodyne_convert_node
-  rclcpp
-)
-target_link_libraries(velodyne_convert_node convert)
-install(TARGETS velodyne_convert_node
-  DESTINATION lib/${PROJECT_NAME}
-)
-
 add_library(transform SHARED
   src/conversions/transform.cpp)
 ament_target_dependencies(transform

diff --git a/velodyne_pointcloud/config/VLP16-velodyne_convert_node-params.yaml b/velodyne_pointcloud/config/VLP16-velodyne_convert_node-params.yaml
diff --git a/velodyne_pointcloud/config/VLP16-velodyne_transform_node-params.yaml b/velodyne_pointcloud/config/VLP16-velodyne_transform_node-params.yaml
@@ -5,4 +5,6 @@ velodyne_transform_node:
         min_range: 0.9
         max_range: 130.0
         view_direction: 0.0
+        fixed_frame: ""
+        target_frame: ""
         organize_cloud: true
diff --git a/velodyne_pointcloud/config/VLP32C-velodyne_convert_node-params.yaml b/velodyne_pointcloud/config/VLP32C-velodyne_convert_node-params.yaml
diff --git a/velodyne_pointcloud/config/VLP32C-velodyne_transform_node-params.yaml b/velodyne_pointcloud/config/VLP32C-velodyne_transform_node-params.yaml
@@ -5,4 +5,6 @@ velodyne_transform_node:
         min_range: 0.9
         max_range: 200.0
         view_direction: 0.0
+        fixed_frame: ""
+        target_frame: ""
         organize_cloud: true
diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/convert.hpp b/velodyne_pointcloud/include/velodyne_pointcloud/convert.hpp