Set up dynamic reconfiguration for transform_node.

Previously, transform_node has neither read parameters other than frame_id from the command line nor has it exposed these parameters via dynamic reconfigure. As parameters like max_range and view_width have been initialized to zero, the inconfigurable transform_node has returned an empty point cloud. Now, transform_node launches an reconfigure server just as cloud_node. In contrast to cloud_node, transform node exposes another parameter for dynamic reconfiguration: frame_id, i.e. the frame of reference the incoming Velodyne points are transformed to.
CPFL · Feb 19, 2016 · 26fface · 26fface
1 parent 80a1494
commit 26fface
Show file tree

Hide file tree

Showing 8 changed files with 61 additions and 10 deletions.
diff --git a/velodyne_pointcloud/CMakeLists.txt b/velodyne_pointcloud/CMakeLists.txt
@@ -10,7 +10,9 @@ set(${PROJECT_NAME}_CATKIN_DEPS
     sensor_msgs
     tf
     velodyne_driver
-    velodyne_msgs)
+    velodyne_msgs
+    dynamic_reconfigure
+)
 
 find_package(catkin REQUIRED COMPONENTS
              ${${PROJECT_NAME}_CATKIN_DEPS} pcl_conversions)
@@ -28,13 +30,16 @@ find_library(YAML_CPP_LIBRARY
              PATHS ${YAML_CPP_LIBRARY_DIRS})
 
 generate_dynamic_reconfigure_options(
-  cfg/VelodyneConfig.cfg)
+  cfg/CloudNode.cfg cfg/TransformNode.cfg
+)
 
 if(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5")
 add_definitions(-DHAVE_NEW_YAMLCPP)
 endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5")
 
-include_directories(include ${catkin_INCLUDE_DIRS})
+include_directories(include ${catkin_INCLUDE_DIRS} 
+  ${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake
+)
 
 catkin_package(
     CATKIN_DEPENDS ${${PROJECT_NAME}_CATKIN_DEPS}

diff --git a/velodyne_pointcloud/cfg/VelodyneConfig.cfg → velodyne_pointcloud/cfg/CloudNode.cfg b/velodyne_pointcloud/cfg/VelodyneConfig.cfg → velodyne_pointcloud/cfg/CloudNode.cfg
@@ -13,4 +13,4 @@ gen.add("view_direction", double_t, 0, "angle defining the center of view",
 gen.add("view_width", double_t, 0, "angle defining the view width",
         2*pi, 0.0, 2*pi)
 
-exit(gen.generate(PACKAGE, "velodyne_pointcloud", "VelodyneConfig"))
+exit(gen.generate(PACKAGE, "cloud_node", "CloudNode"))
diff --git a/velodyne_pointcloud/cfg/TransformNode.cfg b/velodyne_pointcloud/cfg/TransformNode.cfg
@@ -0,0 +1,17 @@
+#!/usr/bin/env python
+PACKAGE = "velodyne_pointcloud"
+
+from math import pi
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("min_range", double_t, 0, "min range to publish", 0.9, 0.1, 10.0)
+gen.add("max_range", double_t, 0, "max range to publish", 130, 0.1, 200)
+gen.add("view_direction", double_t, 0, "angle defining the center of view",
+        0.0, -pi, pi)
+gen.add("view_width", double_t, 0, "angle defining the view width",
+        2*pi, 0.0, 2*pi)
+gen.add("frame_id", str_t, 0, "new frame of reference for point clouds", "odom")
+
+exit(gen.generate(PACKAGE, "transform_node", "TransformNode"))
diff --git a/velodyne_pointcloud/src/conversions/CMakeLists.txt b/velodyne_pointcloud/src/conversions/CMakeLists.txt
@@ -1,10 +1,12 @@
 add_executable(cloud_node cloud_node.cc convert.cc)
+add_dependencies(cloud_node ${PROJECT_NAME}_gencfg)
 target_link_libraries(cloud_node velodyne_rawdata
                       ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES})
 install(TARGETS cloud_node
         RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
 
 add_library(cloud_nodelet cloud_nodelet.cc convert.cc)
+add_dependencies(cloud_nodelet ${PROJECT_NAME}_gencfg)
 target_link_libraries(cloud_nodelet velodyne_rawdata
                       ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES})
 install(TARGETS cloud_nodelet
@@ -27,12 +29,14 @@ install(TARGETS ringcolors_nodelet
         LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
 
 add_executable(transform_node transform_node.cc transform.cc)
+add_dependencies(transform_node ${PROJECT_NAME}_gencfg)
 target_link_libraries(transform_node velodyne_rawdata
                       ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES})
 install(TARGETS transform_node
         RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
 
 add_library(transform_nodelet transform_nodelet.cc transform.cc)
+add_dependencies(transform_nodelet ${PROJECT_NAME}_gencfg)
 target_link_libraries(transform_nodelet velodyne_rawdata
                       ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES})
 install(TARGETS transform_nodelet

diff --git a/velodyne_pointcloud/src/conversions/convert.cc b/velodyne_pointcloud/src/conversions/convert.cc
@@ -31,8 +31,8 @@ namespace velodyne_pointcloud
       node.advertise<sensor_msgs::PointCloud2>("velodyne_points", 10);
 
     srv_ = boost::make_shared <dynamic_reconfigure::Server<velodyne_pointcloud::
-      VelodyneConfigConfig> > (private_nh);
-    dynamic_reconfigure::Server<velodyne_pointcloud::VelodyneConfigConfig>::
+      CloudNodeConfig> > (private_nh);
+    dynamic_reconfigure::Server<velodyne_pointcloud::CloudNodeConfig>::
       CallbackType f;
     f = boost::bind (&Convert::callback, this, _1, _2);
     srv_->setCallback (f);
@@ -44,7 +44,7 @@ namespace velodyne_pointcloud
                      ros::TransportHints().tcpNoDelay(true));
   }
 
-  void Convert::callback(velodyne_pointcloud::VelodyneConfigConfig &config,
+  void Convert::callback(velodyne_pointcloud::CloudNodeConfig &config,
                 uint32_t level)
   {
   ROS_INFO("Reconfigure Request");

diff --git a/velodyne_pointcloud/src/conversions/convert.h b/velodyne_pointcloud/src/conversions/convert.h
@@ -23,7 +23,7 @@
 #include <velodyne_pointcloud/rawdata.h>
 
 #include <dynamic_reconfigure/server.h>
-#include <velodyne_pointcloud/VelodyneConfigConfig.h>
+#include <velodyne_pointcloud/CloudNodeConfig.h>
 
 namespace velodyne_pointcloud
 {
@@ -36,13 +36,13 @@ namespace velodyne_pointcloud
 
   private:
 
-    void callback(velodyne_pointcloud::VelodyneConfigConfig &config,
+    void callback(velodyne_pointcloud::CloudNodeConfig &config,
                 uint32_t level);
     void processScan(const velodyne_msgs::VelodyneScan::ConstPtr &scanMsg);
 
     ///Pointer to dynamic reconfigure service srv_
     boost::shared_ptr<dynamic_reconfigure::Server<velodyne_pointcloud::
-      VelodyneConfigConfig> > srv_;
+      CloudNodeConfig> > srv_;
 
     boost::shared_ptr<velodyne_rawdata::RawData> data_;
     ros::Subscriber velodyne_scan_;

diff --git a/velodyne_pointcloud/src/conversions/transform.cc b/velodyne_pointcloud/src/conversions/transform.cc
@@ -38,6 +38,13 @@ namespace velodyne_pointcloud
     output_ =
       node.advertise<sensor_msgs::PointCloud2>("velodyne_points", 10);
 
+    srv_ = boost::make_shared <dynamic_reconfigure::Server<velodyne_pointcloud::
+      TransformNodeConfig> > (private_nh);
+    dynamic_reconfigure::Server<velodyne_pointcloud::TransformNodeConfig>::
+      CallbackType f;
+    f = boost::bind (&Transform::callback, this, _1, _2);
+    srv_->setCallback (f);
+
     // subscribe to VelodyneScan packets using transform filter
     velodyne_scan_.subscribe(node, "velodyne_packets", 10);
     tf_filter_ =
@@ -46,6 +53,15 @@ namespace velodyne_pointcloud
                                                          config_.frame_id, 10);
     tf_filter_->registerCallback(boost::bind(&Transform::processScan, this, _1));
   }
+
+  void Transform::callback(velodyne_pointcloud::TransformNodeConfig &config,
+                uint32_t level)
+  {
+    ROS_INFO("Reconfigure Request");
+    data_->setParameters(config.min_range, config.max_range, 
+                         config.view_direction, config.view_width);
+    config_.frame_id = config.frame_id;
+  }
 
   /** @brief Callback for raw scan messages.
    *

diff --git a/velodyne_pointcloud/src/conversions/transform.h b/velodyne_pointcloud/src/conversions/transform.h
@@ -26,6 +26,9 @@
 #include <velodyne_pointcloud/rawdata.h>
 #include <velodyne_pointcloud/point_types.h>
 
+#include <dynamic_reconfigure/server.h>
+#include <velodyne_pointcloud/TransformNodeConfig.h>
+
 // include template implementations to transform a custom point cloud
 #include <pcl_ros/impl/transforms.hpp>
 
@@ -53,6 +56,12 @@ namespace velodyne_pointcloud
 
     void processScan(const velodyne_msgs::VelodyneScan::ConstPtr &scanMsg);
 
+    ///Pointer to dynamic reconfigure service srv_
+    boost::shared_ptr<dynamic_reconfigure::Server<velodyne_pointcloud::
+      TransformNodeConfig> > srv_;
+    void callback(velodyne_pointcloud::TransformNodeConfig &config,
+                  uint32_t level);
+
     boost::shared_ptr<velodyne_rawdata::RawData> data_;
     message_filters::Subscriber<velodyne_msgs::VelodyneScan> velodyne_scan_;
     tf::MessageFilter<velodyne_msgs::VelodyneScan> *tf_filter_;