dillenberger · Mar 3, 2019 · Apr 5, 2019 · Apr 5, 2019 · Apr 5, 2019 · Oct 15, 2020
diff --git a/dummy_slam_broadcaster/package.xml b/dummy_slam_broadcaster/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>dummy_slam_broadcaster</name>
-  <version>0.1.3</version>
+  <version>0.1.6</version>
   <description>The dummy_slam_broadcaster package</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag --> 

diff --git a/pepperl_fuchs_r2000/CHANGELOG.rst b/pepperl_fuchs_r2000/CHANGELOG.rst
@@ -2,6 +2,22 @@
 Changelog for package pepperl_fuchs_r2000
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.1.6 (2025-02-12)
+------------------
+* Reconnect on failure during initialization.
+* Contributors: Patrick Chin
+
+0.1.5 (2020-10-16)
+------------------
+* Fix isolated build.
+* Contributors: Patrick Chin
+
+0.1.4 (2019-04-05)
+------------------
+* Add dynamic reconfigure server.
+* Allow configuration of the minimum and maximum angle of the published scan data.
+* Contributors: Patrick Chin
+
 0.1.3 (2016-09-15)
 ------------------
 * Initial release

diff --git a/pepperl_fuchs_r2000/CMakeLists.txt b/pepperl_fuchs_r2000/CMakeLists.txt
@@ -12,6 +12,7 @@ ENDIF(NOT CMAKE_BUILD_TYPE)
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=c++0x -Wfatal-errors")
 
 find_package(catkin REQUIRED COMPONENTS
+  dynamic_reconfigure
   roscpp
   std_msgs
   sensor_msgs
@@ -20,6 +21,10 @@ find_package(Boost REQUIRED COMPONENTS
   thread
 )
 
+generate_dynamic_reconfigure_options(
+  cfg/PepperlFuchsR2000.cfg
+)
+
 catkin_package(
   INCLUDE_DIRS include ${Boost_INCLUDE_DIRS}
   LIBRARIES r2000_driver
@@ -48,6 +53,7 @@ add_executable(r2000_node
 target_link_libraries(r2000_node
   r2000_driver
 )
+add_dependencies(r2000_node ${PROJECT_NAME}_gencfg)
 
 add_executable(driver_example
   src/example/driver_example.cpp

diff --git a/pepperl_fuchs_r2000/cfg/PepperlFuchsR2000.cfg b/pepperl_fuchs_r2000/cfg/PepperlFuchsR2000.cfg
@@ -0,0 +1,15 @@
+#!/usr/bin/env python
+PACKAGE = "pepperl_fuchs_r2000"
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+from math import pi
+
+gen = ParameterGenerator()
+
+gen.add("frame_id", str_t, 0, "The frame-id in the header of the published laser scan messages.", "laser")
+gen.add("scan_frequency", int_t, 0, "The scan frequency (rotation speed of scanner head) in Hz", 35, 10, 50)
+gen.add("samples_per_scan", int_t, 0, "The number of distances measured per scan", 3600, 72, 25200)
+gen.add("min_ang", double_t, 0, "The angle of the first range measurement in radians.", -pi, -pi, pi)
+gen.add("max_ang", double_t, 0, "The angle of the last range measurement in radians.", pi, -pi, pi)
+
+exit(gen.generate(PACKAGE, "pepperl_fuchs_r2000", "PepperlFuchsR2000"))
diff --git a/pepperl_fuchs_r2000/package.xml b/pepperl_fuchs_r2000/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>pepperl_fuchs_r2000</name>
-  <version>0.1.3</version>
+  <version>0.1.6</version>
   <description>The Pepperl+Fuchs R2000 laser range finder driver package</description>
   <maintainer email="denis.dillenberger@gmail.com">Denis Dillenberger</maintainer>
   <author email="denis.dillenberger@gmail.com">Denis Dillenberger</author> 
@@ -12,10 +12,12 @@
 
   <buildtool_depend>catkin</buildtool_depend>
 
+  <build_depend>dynamic_reconfigure</build_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>sensor_msgs</build_depend>
   <build_depend>std_msgs</build_depend>
 
+  <run_depend>dynamic_reconfigure</run_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>sensor_msgs</run_depend>
   <run_depend>std_msgs</run_depend>

diff --git a/pepperl_fuchs_r2000/src/rosnode/r2000_node.cpp b/pepperl_fuchs_r2000/src/rosnode/r2000_node.cpp
@@ -37,21 +37,31 @@ namespace pepperl_fuchs {
 R2000Node::R2000Node():nh_("~")
 {
     driver_ = 0;
+
+    // ROS dynamic reconfigure server
+    //-------------------------------------------------------------------------
+    ds_.setCallback(boost::bind(&R2000Node::handleConfig, this, _1, _2));
+
     // Reading and checking parameters
     //-------------------------------------------------------------------------
     nh_.param("frame_id", frame_id_, std::string("/scan"));
     nh_.param("scanner_ip",scanner_ip_,std::string(""));
     nh_.param("scan_frequency",scan_frequency_,35);
     nh_.param("samples_per_scan",samples_per_scan_,3600);
+    nh_.param("min_ang", min_ang_, -M_PI);
+    nh_.param("max_ang", max_ang_, +M_PI);
 
     if( scanner_ip_ == "" )
     {
         std::cerr << "IP of laser range finder not set!" << std::endl;
         return;
     }
 
-    if( !connect() )
-        return;
+    while( !connect() )
+    {
+        std::cout << "ERROR: Connect failed. Trying again in 2 seconds..." << std::endl;
+        usleep((2*1000000));
+    }
 
     // Declare publisher and create timer
     //-------------------------------------------------------------------------
@@ -131,16 +141,49 @@ void R2000Node::getScanData(const ros::TimerEvent &e)
     scanmsg.range_min = std::atof(driver_->getParametersCached().at("radial_range_min").c_str());
     scanmsg.range_max = std::atof(driver_->getParametersCached().at("radial_range_max").c_str());
 
-    scanmsg.ranges.resize(scandata.distance_data.size());
-    scanmsg.intensities.resize(scandata.amplitude_data.size());
-    for( std::size_t i=0; i<scandata.distance_data.size(); i++ )
+    // adjust angle_min to min_ang config param
+    int index_min = 0;
+    while (scanmsg.angle_min + scanmsg.angle_increment < min_ang_)
+    {
+        scanmsg.angle_min += scanmsg.angle_increment;
+        index_min++;
+    }
+
+    // adjust angle_max to max_ang config param
+    int index_max = scandata.distance_data.size() - 1;
+    while (scanmsg.angle_max - scanmsg.angle_increment > max_ang_)
     {
-        scanmsg.ranges[i] = float(scandata.distance_data[i])/1000.0f;
-        scanmsg.intensities[i] = scandata.amplitude_data[i];
+      scanmsg.angle_max -= scanmsg.angle_increment;
+      index_max--;
+    }
+
+    scanmsg.ranges.resize(index_max - index_min + 1);
+    scanmsg.intensities.resize(index_max - index_min + 1);
+    for (int i = index_min; i <= index_max; i++)
+    {
+        scanmsg.ranges[i - index_min] = float(scandata.distance_data[i])/1000.0f;
+        scanmsg.intensities[i - index_min] = scandata.amplitude_data[i];
     }
     scan_publisher_.publish(scanmsg);
 }
 
+//-----------------------------------------------------------------------------
+void R2000Node::handleConfig(pepperl_fuchs_r2000::PepperlFuchsR2000Config& config, uint32_t level)
+{
+  ROS_INFO("Configuration received.");
+  frame_id_ = config.frame_id;
+  scan_frequency_ = config.scan_frequency;
+  samples_per_scan_ = config.samples_per_scan;
+  min_ang_ = config.min_ang;
+  max_ang_ = config.max_ang;
+
+  if (driver_)
+  {
+    driver_->setScanFrequency(scan_frequency_);
+    driver_->setSamplesPerScan(samples_per_scan_);
+  }
+}
+
 //-----------------------------------------------------------------------------
 void R2000Node::cmdMsgCallback(const std_msgs::StringConstPtr &msg)
 {

diff --git a/pepperl_fuchs_r2000/src/rosnode/r2000_node.h b/pepperl_fuchs_r2000/src/rosnode/r2000_node.h
@@ -32,6 +32,8 @@
 
 #include <ros/ros.h>
 #include <std_msgs/String.h>
+#include <dynamic_reconfigure/server.h>
+#include <pepperl_fuchs_r2000/PepperlFuchsR2000Config.h>
 
 namespace pepperl_fuchs {
 class R2000Driver;
@@ -44,6 +46,9 @@ class R2000Node
     //! Initialize and connect to laser range finder
     R2000Node();
 
+    //! Callback function for dynamic reconfigure server
+    void handleConfig(pepperl_fuchs_r2000::PepperlFuchsR2000Config& config, uint32_t level);
+
     //! Callback function for control commands
     void cmdMsgCallback( const std_msgs::StringConstPtr& msg );
 
@@ -79,8 +84,17 @@ class R2000Node
     //! samples_per_scan parameter
     int samples_per_scan_;
 
+    //! min_ang parameter
+    double min_ang_;
+
+    //! max_ang parameter
+    double max_ang_;
+
     //! Pointer to driver
     R2000Driver* driver_;
+
+    //! ROS dynamic reconfigure server
+    dynamic_reconfigure::Server<pepperl_fuchs_r2000::PepperlFuchsR2000Config> ds_;
 };
 }