Merge branch 'feature/multiscan136_laserscan_msg'

CHANGELOG.md

@@ -8,9 +8,12 @@ features that will be removed in future versions **Removed** for deprecated feat
 
 ## Released ##
 
+### v2.8.14 - Laserscan messages for Multiscan
+  - **Update** Laserscan messages for Multiscan lidar, #96
+
 ### v2.8.13 - Dynamical pointcloud transform and QoS configuration
-  - **Update** Dynamical configuration of an additional pointcloud transform by rosparam, #104
   - **Update** Configuration of ROS quality of service by launchfile, #101
+  - **Update** Dynamical configuration of an additional pointcloud transform by rosparam, #104
 
 ### v2.8.11 - LMS 1xxx support
   - **Update** LMS 1xxx support with scan configuration (scan frequency and angular resolution for firmware 2.x)

driver/src/sick_generic_laser.cpp

@@ -93,7 +93,7 @@
 
 #define SICK_GENERIC_MAJOR_VER "2"
 #define SICK_GENERIC_MINOR_VER "8"
-#define SICK_GENERIC_PATCH_LEVEL "13"
+#define SICK_GENERIC_PATCH_LEVEL "14"
 
 #define DELETE_PTR(p) if(p){delete(p);p=0;}
 

driver/src/sick_scan_common.cpp

@@ -4281,7 +4281,7 @@ namespace sick_scan
                 {
                   // numEchos
                   char szTmp[255] = {0};
-                  if (false) // if (this->parser_->getCurrentParamPtr()->getNumberOfLayers() > 1) // Use configured frame_id for both laser scan and pointcloud messages
+                  if (this->parser_->getCurrentParamPtr()->getNumberOfLayers() > 1)
                   {
                     const char *cpFrameId = config_.frame_id.c_str();
 #if 0
@@ -4304,7 +4304,7 @@ namespace sick_scan
                   }
                   else
                   {
-                    strcpy(szTmp, config_.frame_id.c_str());
+                    strcpy(szTmp, config_.frame_id.c_str());  // Use configured frame_id for both laser scan and pointcloud messages
                   }
 
                   msg.header.frame_id = std::string(szTmp);
@@ -4403,9 +4403,9 @@ namespace sick_scan
                 numTmpLayer = 1; // LMS_1XXX has 4 interlaced layer, each layer published in one pointcloud message
                 baseLayer = 0;
                 layer = 0;
+                msg.header.frame_id = config_.frame_id; // Use configured frame_id for both laser scan and pointcloud messages
               }
 
-
               cloud_.header.stamp = recvTimeStamp + rosDurationFromSec(config_.time_offset);
               // ROS_DEBUG_STREAM("laser_scan timestamp: " << msg.header.stamp << ", pointclound timestamp: " << cloud_.header.stamp);
               cloud_.header.frame_id = config_.frame_id;

driver/src/sick_scan_services.cpp

@@ -172,6 +172,7 @@ bool sick_scan::SickScanServices::sendSopasAndCheckAnswer(const std::string& sop
 {
   if(m_common_tcp)
   {
+    ROS_INFO_STREAM("SickScanServices: Sending request \"" << sopasCmd << "\"");
     std::string sopasRequest = std::string("\x02") + sopasCmd + "\x03";
     int result = -1;
     if (m_cola_binary)

driver/src/sick_scansegment_xd/config.cpp

@@ -163,6 +163,12 @@ sick_scansegment_xd::Config::Config()
     msgpack_validator_filter_settings.msgpack_validator_layer_filter = { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 }; // default for full scan: 16 layer active, i.e. { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 }
     msgpack_validator_valid_segments = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11 }; // default for full scan: 12 segments, i.e. { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11 }
 
+    // Configuration of laserscan messages (ROS only):
+    // Parameter "laserscan_layer_filter" sets a mask to create laserscan messages for configured layer (0: no laserscan message, 1: create laserscan messages for this layer)
+    // Use "0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0" to activate resp. "1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1" to activate laserscan messages for all 16 layers of the Multiscan136
+    // Default is "0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0", i.e. laserscan messages for layer 5, (elevation -0.07 degree, max number of scan points)
+    laserscan_layer_filter = { 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+
 }
 
 /*
@@ -278,6 +284,12 @@ bool sick_scansegment_xd::Config::Init(rosNodePtr _node)
     ROS_DECL_GET_PARAMETER(node, "range_filter_handling", range_filter_handling);
     range_filter = sick_scan::SickRangeFilter(range_min, range_max, (sick_scan::RangeFilterResultHandling)range_filter_handling);
     ROS_INFO_STREAM("Range filter configuration for sick_scansegment_xd: range_min=" << range_min << ", range_max=" << range_max << ", range_filter_handling=" << range_filter_handling);
+
+    // Configuration of laserscan messages (ROS only), activate/deactivate laserscan messages for each layer
+    std::string str_laserscan_layer_filter = "0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0";
+    ROS_DECL_GET_PARAMETER(node, "laserscan_layer_filter", str_laserscan_layer_filter);
+    sick_scansegment_xd::util::parseVector(str_laserscan_layer_filter, laserscan_layer_filter);
+
     return true;
 }
 
@@ -363,7 +375,7 @@ bool sick_scansegment_xd::Config::Init(int argc, char** argv)
     setOptionalArgument(cli_parameter_map, "msgpack_validator_verbose", msgpack_validator_verbose);
     setOptionalArgument(cli_parameter_map, "msgpack_validator_discard_msgpacks_out_of_bounds", msgpack_validator_discard_msgpacks_out_of_bounds);
     setOptionalArgument(cli_parameter_map, "msgpack_validator_check_missing_scandata_interval", msgpack_validator_check_missing_scandata_interval);
-    std::string cli_msgpack_validator_required_echos, cli_msgpack_validator_valid_segments, cli_msgpack_validator_layer_filter;
+    std::string cli_msgpack_validator_required_echos, cli_msgpack_validator_valid_segments, cli_msgpack_validator_layer_filter, cli_laserscan_layer_filter;
     float cli_msgpack_validator_azimuth_start_deg, cli_msgpack_validator_azimuth_end_deg, cli_msgpack_validator_elevation_start_deg, cli_msgpack_validator_elevation_end_deg;
     if (setOptionalArgument(cli_parameter_map, "msgpack_validator_required_echos", cli_msgpack_validator_required_echos))
         sick_scansegment_xd::util::parseVector(cli_msgpack_validator_required_echos, msgpack_validator_filter_settings.msgpack_validator_required_echos);
@@ -379,6 +391,8 @@ bool sick_scansegment_xd::Config::Init(int argc, char** argv)
         sick_scansegment_xd::util::parseVector(cli_msgpack_validator_valid_segments, msgpack_validator_valid_segments);
     if (setOptionalArgument(cli_parameter_map, "msgpack_validator_layer_filter", cli_msgpack_validator_layer_filter))
         sick_scansegment_xd::util::parseVector(cli_msgpack_validator_layer_filter, msgpack_validator_filter_settings.msgpack_validator_layer_filter);
+    if (setOptionalArgument(cli_parameter_map, "laserscan_layer_filter", cli_laserscan_layer_filter))
+        sick_scansegment_xd::util::parseVector(cli_laserscan_layer_filter, laserscan_layer_filter);
 
     PrintConfig();
 
@@ -422,6 +436,7 @@ void sick_scansegment_xd::Config::PrintConfig(void)
     ROS_INFO_STREAM("host_set_LFPangleRangeFilter:     " << host_set_LFPangleRangeFilter);
     ROS_INFO_STREAM("host_LFPlayerFilter:              " << host_LFPlayerFilter);
     ROS_INFO_STREAM("host_set_LFPlayerFilter:          " << host_set_LFPlayerFilter);
+    ROS_INFO_STREAM("laserscan_layer_filter:           " << sick_scansegment_xd::util::printVector(laserscan_layer_filter));
     ROS_INFO_STREAM("msgpack_validator_enabled:                         " << msgpack_validator_enabled);
     ROS_INFO_STREAM("msgpack_validator_verbose:                         " << msgpack_validator_verbose);
     ROS_INFO_STREAM("msgpack_validator_discard_msgpacks_out_of_bounds:  " << msgpack_validator_discard_msgpacks_out_of_bounds);

driver/src/sick_scansegment_xd/msgpack_parser.cpp

@@ -752,8 +752,6 @@ bool sick_scansegment_xd::MsgPackParser::Parse(std::istream& msgpack_istream, fi
 				groupData.push_back(sick_scansegment_xd::MsgPackParserOutput::Scanline());
 				sick_scansegment_xd::MsgPackParserOutput::Scanline& scanline = groupData.back();
 				scanline.points.reserve(iPointCount);
-				scanline.range_min = +FLT_MAX;
-				scanline.range_max = -FLT_MAX;
 				for (int pointIdx = 0; pointIdx < iPointCount; pointIdx++)
 				{
 					float dist = 0.001f * distValues[echoIdx].data()[pointIdx]; // convert distance to meter
@@ -771,21 +769,9 @@ bool sick_scansegment_xd::MsgPackParser::Parse(std::istream& msgpack_istream, fi
 							msgpack_validator_data.update(echoIdx, segment_idx, azimuth_norm, elevation);
 							msgpack_validator_data_collector.update(echoIdx, segment_idx, azimuth_norm, elevation);
 						}
-						scanline.range_min = std::min(dist, scanline.range_min);
-						scanline.range_max = std::min(dist, scanline.range_max);
 						scanline.points.push_back(sick_scansegment_xd::MsgPackParserOutput::LidarPoint(x, y, z, intensity, dist, azimuth, elevation, groupIdx, echoIdx, pointIdx));
 				    }
 				}
-				if (iPointCount > 0)
-				{
-					scanline.angle_min = normalizeAngle(scanline.points.front().azimuth);
-					scanline.angle_max = normalizeAngle(scanline.points.back().azimuth);
-					scanline.angle_increment = (scanline.angle_max - scanline.angle_min) / (float)iPointCount;
-				}
-				else
-				{
-					scanline = sick_scansegment_xd::MsgPackParserOutput::Scanline();
-				}
 			}
 
 			// debug output