Merge branch 'feature/transform_update'

CHANGELOG.md

@@ -8,6 +8,10 @@ features that will be removed in future versions **Removed** for deprecated feat
 
 ## Released ##
 
+### 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
+
 ### v2.8.11 - LMS 1xxx support
   - **Update** LMS 1xxx support with scan configuration (scan frequency and angular resolution for firmware 2.x)
 

driver/src/ldmrs/sick_ldmrs_driver.cpp

@@ -55,7 +55,7 @@
 #endif
 
 // Convert lmdrs scan to PointCloud2
-void ldmrsScanToPointCloud2(const datatypes::Scan* scan, const sick_scan::SickCloudTransform& add_transform_xyz_rpy, sick_scan::SickRangeFilter& range_filter, bool isRearMirrorSide, const std::string& frame_id, ros_sensor_msgs::PointCloud2& msg, ros_sensor_msgs::PointCloud2& msg_polar);
+void ldmrsScanToPointCloud2(const datatypes::Scan* scan, sick_scan::SickCloudTransform& add_transform_xyz_rpy, sick_scan::SickRangeFilter& range_filter, bool isRearMirrorSide, const std::string& frame_id, ros_sensor_msgs::PointCloud2& msg, ros_sensor_msgs::PointCloud2& msg_polar);
 
 namespace sick_ldmrs_driver
 {
@@ -610,7 +610,7 @@ std::string SickLDMRS::flexres_err_to_string(const UINT32 code) const
 } // namespace sick_ldmrs_driver
 
 // Convert lmdrs scan to PointCloud2
-void ldmrsScanToPointCloud2(const datatypes::Scan* scan, const sick_scan::SickCloudTransform& add_transform_xyz_rpy, sick_scan::SickRangeFilter& range_filter, bool isRearMirrorSide, const std::string& frame_id, ros_sensor_msgs::PointCloud2& msg, ros_sensor_msgs::PointCloud2& msg_polar)
+void ldmrsScanToPointCloud2(const datatypes::Scan* scan, sick_scan::SickCloudTransform& add_transform_xyz_rpy, sick_scan::SickRangeFilter& range_filter, bool isRearMirrorSide, const std::string& frame_id, ros_sensor_msgs::PointCloud2& msg, ros_sensor_msgs::PointCloud2& msg_polar)
 {
   typedef struct SICK_LDMRS_Point
   {

driver/src/sick_cloud_transform.cpp

@@ -72,33 +72,51 @@ sick_scan::SickCloudTransform::SickCloudTransform()
 
 sick_scan::SickCloudTransform::SickCloudTransform(rosNodePtr nh, bool cartesian_input_only)
 {
+    m_nh = nh;
     std::string add_transform_xyz_rpy = "0,0,0,0,0,0";
     rosDeclareParam(nh, "add_transform_xyz_rpy", add_transform_xyz_rpy);
     rosGetParam(nh, "add_transform_xyz_rpy", add_transform_xyz_rpy);
-    if (!init(add_transform_xyz_rpy, cartesian_input_only))
+    bool add_transform_check_dynamic_updates = false;
+    rosDeclareParam(nh, "add_transform_check_dynamic_updates", add_transform_check_dynamic_updates);
+    rosGetParam(nh, "add_transform_check_dynamic_updates", add_transform_check_dynamic_updates);
+    if (!init(add_transform_xyz_rpy, cartesian_input_only, add_transform_check_dynamic_updates))
     {
         ROS_ERROR_STREAM("## ERROR SickCloudTransform(): Initialization by \"" << add_transform_xyz_rpy << "\" failed, use 6D pose \"x,y,z,roll,pitch,yaw\" in [m] resp. [rad]");
     }
 }
 
-sick_scan::SickCloudTransform::SickCloudTransform(const std::string& add_transform_xyz_rpy, bool cartesian_input_only)
+sick_scan::SickCloudTransform::SickCloudTransform(rosNodePtr nh, const std::string& add_transform_xyz_rpy, bool cartesian_input_only, bool add_transform_check_dynamic_updates)
 {
-    if (!init(add_transform_xyz_rpy, cartesian_input_only))
+    m_nh = nh;
+    if (!init(add_transform_xyz_rpy, cartesian_input_only, add_transform_check_dynamic_updates))
     {
         ROS_ERROR_STREAM("## ERROR SickCloudTransform(): Initialization by \"" << add_transform_xyz_rpy << "\" failed, use 6D pose \"x,y,z,roll,pitch,yaw\" in [m] resp. [rad]");
     }
 }
 
 // Initializes rotation matrix and translation vector from a 6D pose configuration (x,y,z,roll,pitch,yaw) in [m] resp. [rad]
-bool sick_scan::SickCloudTransform::init(const std::string& add_transform_xyz_rpy, bool cartesian_input_only)
+bool sick_scan::SickCloudTransform::init(const std::string& add_transform_xyz_rpy, bool cartesian_input_only, bool add_transform_check_dynamic_updates)
 {
     // Split string add_transform_xyz_rpy to 6D pose x,y,z,roll,pitch,yaw in [m] resp. [rad]
     std::istringstream config_stream(add_transform_xyz_rpy);
     std::string config_arg;
     std::vector<float> config_values;
     while (getline(config_stream, config_arg, ','))
     {
-        config_values.push_back(std::stof(config_arg));
+        // ROS-2 interpretes parameter values configured by "ros2 param set <node> <arg> <value>" as yaml content, 
+        // but does not remove yaml escape characters required for negative numbers. Any '\\' is removed here.
+        std::string::size_type n = 0;
+        while ((n = config_arg.find('\\', n)) != std::string::npos)
+            config_arg.replace( n, 1, "");
+        try
+        {
+            float arg_value = std::stof(config_arg);
+            config_values.push_back(arg_value);
+        }
+        catch(const std::exception& e)
+        {
+            ROS_ERROR_STREAM("## ERROR SickCloudTransform(): parse error in string \"" << add_transform_xyz_rpy << "\", arg=\"" << config_arg << "\", exception " << e.what());
+        }
     }
     if(config_values.size() != 6)
     {
@@ -136,9 +154,10 @@ bool sick_scan::SickCloudTransform::init(const std::string& add_transform_xyz_rp
             m_azimuth_offset = 0;
         }
     }
-
-    // Configuration is given by 6D pose with P_world = T[world,cloud] * P_cloud. By applying the transform, we compute 
-
+    // Initialization successful
+    m_add_transform_xyz_rpy = add_transform_xyz_rpy;
+    m_cartesian_input_only = cartesian_input_only;
+    m_add_transform_check_dynamic_updates = add_transform_check_dynamic_updates;
     ROS_INFO_STREAM("SickCloudTransform: add_transform_xyz_rpy = (" << add_transform_xyz_rpy << ")");
     ROS_INFO_STREAM("SickCloudTransform: azimuth_offset = " << (m_azimuth_offset * 180.0 / M_PI) << " [deg]");
     ROS_INFO_STREAM("SickCloudTransform: additional 3x3 rotation matrix = { (" 
@@ -147,6 +166,7 @@ bool sick_scan::SickCloudTransform::init(const std::string& add_transform_xyz_rp
         << m_rotation_matrix[2][0] << "," << m_rotation_matrix[2][1] << "," << m_rotation_matrix[2][2] << ") }");
     ROS_INFO_STREAM("SickCloudTransform: apply 3x3 rotation = " << (m_apply_3x3_rotation ? "true" : "false"));
     ROS_INFO_STREAM("SickCloudTransform: additional translation = (" << m_translation_vector[0] << "," << m_translation_vector[1] << "," << m_translation_vector[2] << ")");
+    ROS_INFO_STREAM("SickCloudTransform: check_dynamic_updates = " << (m_add_transform_check_dynamic_updates ? "true" : "false"));
     return true;
 }
 

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 "11"
+#define SICK_GENERIC_PATCH_LEVEL "13"
 
 #define DELETE_PTR(p) if(p){delete(p);p=0;}
 

driver/src/sick_scan_common.cpp

@@ -620,7 +620,7 @@ namespace sick_scan
     // add_transform_xyz_rpy := T[world,cloud] with parent "world" and child "cloud", i.e. P_world = T[world,cloud] * P_cloud
     // The additional transform applies to cartesian lidar pointclouds and visualization marker (fields)
     // It is NOT applied to polar pointclouds, radarscans, ldmrs objects or other messages
-    m_add_transform_xyz_rpy = sick_scan::SickCloudTransform(nh);
+    m_add_transform_xyz_rpy = sick_scan::SickCloudTransform(nh, false);
   }
 
   /*!

driver/src/sick_scansegment_xd/config.cpp

@@ -267,7 +267,9 @@ bool sick_scansegment_xd::Config::Init(rosNodePtr _node)
     // Apply an additional transform to the cartesian pointcloud, default: "0,0,0,0,0,0" (i.e. no transform)
     std::string str_add_transform_xyz_rpy = "0,0,0,0,0,0";
     ROS_DECL_GET_PARAMETER(node, "add_transform_xyz_rpy", str_add_transform_xyz_rpy);
-    add_transform_xyz_rpy = sick_scan::SickCloudTransform(str_add_transform_xyz_rpy);
+    bool add_transform_check_dynamic_updates = false;
+    ROS_DECL_GET_PARAMETER(node, "add_transform_check_dynamic_updates", add_transform_check_dynamic_updates);
+    add_transform_xyz_rpy = sick_scan::SickCloudTransform(node, str_add_transform_xyz_rpy, false, add_transform_check_dynamic_updates);
     // Optional range filter
     float range_min = 0, range_max = 100;
     int range_filter_handling = 0;

driver/src/sick_scansegment_xd/msgpack_parser.cpp

@@ -506,7 +506,7 @@ std::string sick_scansegment_xd::MsgPackParser::MsgpackToHexDump(const std::vect
  * @param[in] verbose true: enable debug output, false: quiet mode
  */
 bool sick_scansegment_xd::MsgPackParser::Parse(const std::vector<uint8_t>& msgpack_data, fifo_timestamp msgpack_timestamp, 
-    const sick_scan::SickCloudTransform& add_transform_xyz_rpy, sick_scan::SickRangeFilter& range_filter, MsgPackParserOutput& result,
+    sick_scan::SickCloudTransform& add_transform_xyz_rpy, sick_scan::SickRangeFilter& range_filter, MsgPackParserOutput& result,
     sick_scansegment_xd::MsgPackValidatorData& msgpack_validator_data_collector, const sick_scansegment_xd::MsgPackValidator& msgpack_validator,
 	bool msgpack_validator_enabled, bool discard_msgpacks_not_validated,
 	bool use_software_pll, bool verbose)
@@ -563,7 +563,7 @@ bool sick_scansegment_xd::MsgPackParser::Parse(const std::vector<uint8_t>& msgpa
  * @param[in] verbose true: enable debug output, false: quiet mode
  */
 bool sick_scansegment_xd::MsgPackParser::Parse(std::istream& msgpack_istream, fifo_timestamp msgpack_timestamp, 
-	const sick_scan::SickCloudTransform& add_transform_xyz_rpy, sick_scan::SickRangeFilter& range_filter, MsgPackParserOutput& result,
+	sick_scan::SickCloudTransform& add_transform_xyz_rpy, sick_scan::SickRangeFilter& range_filter, MsgPackParserOutput& result,
     sick_scansegment_xd::MsgPackValidatorData& msgpack_validator_data_collector, 
 	const sick_scansegment_xd::MsgPackValidator& msgpack_validator,
 	bool msgpack_validator_enabled, bool discard_msgpacks_not_validated,
@@ -751,7 +751,9 @@ bool sick_scansegment_xd::MsgPackParser::Parse(std::istream& msgpack_istream, fi
 				assert(iPointCount == channelTheta.data().size() && iPointCount == distValues[echoIdx].data().size() && iPointCount == rssiValues[echoIdx].data().size());
 				groupData.push_back(sick_scansegment_xd::MsgPackParserOutput::Scanline());
 				sick_scansegment_xd::MsgPackParserOutput::Scanline& scanline = groupData.back();
-				scanline.reserve(iPointCount);
+				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
@@ -762,16 +764,28 @@ bool sick_scansegment_xd::MsgPackParser::Parse(std::istream& msgpack_istream, fi
 						float y = dist * sin_azimuth[pointIdx] * cos_elevation;
 						float z = dist * sin_elevation;
 						add_transform_xyz_rpy.applyTransform(x, y, z);
-    				float azimuth = channelTheta.data()[pointIdx];
+    				    float azimuth = channelTheta.data()[pointIdx];
 						float azimuth_norm = normalizeAngle(azimuth);
 						if (msgpack_validator_enabled)
 						{
 							msgpack_validator_data.update(echoIdx, segment_idx, azimuth_norm, elevation);
 							msgpack_validator_data_collector.update(echoIdx, segment_idx, azimuth_norm, elevation);
 						}
-						scanline.push_back(sick_scansegment_xd::MsgPackParserOutput::LidarPoint(x, y, z, intensity, dist, azimuth, elevation, groupIdx, echoIdx, pointIdx));
+						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
@@ -888,7 +902,7 @@ bool sick_scansegment_xd::MsgPackParser::WriteCSV(const std::vector<MsgPackParse
 		{
 			for (int echoIdx = 0; echoIdx < result.scandata[groupIdx].scanlines.size(); echoIdx++)
 			{
-				const std::vector<sick_scansegment_xd::MsgPackParserOutput::LidarPoint>& scanline = result.scandata[groupIdx].scanlines[echoIdx];
+				const std::vector<sick_scansegment_xd::MsgPackParserOutput::LidarPoint>& scanline = result.scandata[groupIdx].scanlines[echoIdx].points;
 				for (int pointIdx = 0; pointIdx < scanline.size(); pointIdx++)
 				{
 					const sick_scansegment_xd::MsgPackParserOutput::LidarPoint& point = scanline[pointIdx];
@@ -927,7 +941,7 @@ bool sick_scansegment_xd::MsgPackParser::ExportXYZI(const std::vector<MsgPackPar
 	{
 		for (int echoIdx = 0; echoIdx < results[0].scandata[groupIdx].scanlines.size(); echoIdx++)
 		{
-			data_length += results[0].scandata[groupIdx].scanlines[echoIdx].size();
+			data_length += results[0].scandata[groupIdx].scanlines[echoIdx].points.size();
 		}
 	}
 	x.reserve(data_length);
@@ -944,7 +958,7 @@ bool sick_scansegment_xd::MsgPackParser::ExportXYZI(const std::vector<MsgPackPar
 		{
 			for (int echoIdx = 0; echoIdx < result.scandata[groupIdx].scanlines.size(); echoIdx++)
 			{
-				const std::vector<sick_scansegment_xd::MsgPackParserOutput::LidarPoint>& scanline = result.scandata[groupIdx].scanlines[echoIdx];
+				const std::vector<sick_scansegment_xd::MsgPackParserOutput::LidarPoint>& scanline = result.scandata[groupIdx].scanlines[echoIdx].points;
 				for (int pointIdx = 0; pointIdx < scanline.size(); pointIdx++)
 				{
 					const sick_scansegment_xd::MsgPackParserOutput::LidarPoint& point = scanline[pointIdx];

driver/src/sick_scansegment_xd/ros_msgpack_publisher.cpp

@@ -68,7 +68,7 @@
  *            config.publish_frame_id: frame id of ros PointCloud2 messages, default: "world"
  * @param[in] qos quality of service profile for the ros publisher, default: 1
  */
-sick_scansegment_xd::RosMsgpackPublisher::RosMsgpackPublisher(const std::string& node_name, const sick_scansegment_xd::Config& config, const rosQoS& qos)
+sick_scansegment_xd::RosMsgpackPublisher::RosMsgpackPublisher(const std::string& node_name, const sick_scansegment_xd::Config& config, rosQoS qos)
 #if defined __ROS_VERSION && __ROS_VERSION > 1
 	: Node(node_name)
 #endif
@@ -82,12 +82,23 @@ sick_scansegment_xd::RosMsgpackPublisher::RosMsgpackPublisher(const std::string&
 	m_min_azimuth = (float)-M_PI;
 	m_max_azimuth = (float)+M_PI;
 #if defined __ROS_VERSION && __ROS_VERSION > 1 // ROS-2 publisher
+    QoSConverter qos_converter;
+    int qos_val = -1;
+    rosDeclareParam(m_node, "ros_qos", qos_val);
+    rosGetParam(m_node, "ros_qos", qos_val);
+    if (qos_val >= 0)
+        qos = qos_converter.convert(qos_val);
 	m_points_collector = SegmentPointsCollector(m_segment_count);
 	if(m_publish_topic != "")
 	  m_publisher_cur_segment = create_publisher<PointCloud2Msg>(m_publish_topic, qos);
 	if(m_publish_topic_all_segments != "")
 	  m_publisher_all_segments = create_publisher<PointCloud2Msg>(m_publish_topic_all_segments, qos);
 #elif defined __ROS_VERSION && __ROS_VERSION > 0 // ROS-1 publisher
+    int qos_val = -1;
+    rosDeclareParam(m_node, "ros_qos", qos_val);
+    rosGetParam(m_node, "ros_qos", qos_val);
+    if (qos_val >= 0)
+        qos = qos_val;
 	if(m_publish_topic != "")
 		m_publisher_cur_segment = m_node->advertise<PointCloud2Msg>(m_publish_topic, qos);
 	if(m_publish_topic_all_segments != "")
@@ -210,8 +221,8 @@ void sick_scansegment_xd::RosMsgpackPublisher::HandleMsgPackData(const sick_scan
 		echo_count = std::max(msgpack_data.scandata[groupIdx].scanlines.size(), echo_count);
 		for (int echoIdx = 0; echoIdx < msgpack_data.scandata[groupIdx].scanlines.size(); echoIdx++)
 		{
-			total_point_count += msgpack_data.scandata[groupIdx].scanlines[echoIdx].size();
-			point_count_per_echo = std::max(msgpack_data.scandata[groupIdx].scanlines[echoIdx].size(), point_count_per_echo);
+			total_point_count += msgpack_data.scandata[groupIdx].scanlines[echoIdx].points.size();
+			point_count_per_echo = std::max(msgpack_data.scandata[groupIdx].scanlines[echoIdx].points.size(), point_count_per_echo);
 		}
 	}
 	float lidar_points_min_azimuth = +2.0f * (float)M_PI, lidar_points_max_azimuth = -2.0f * (float)M_PI;
@@ -224,7 +235,7 @@ void sick_scansegment_xd::RosMsgpackPublisher::HandleMsgPackData(const sick_scan
 	{
 		for (int echoIdx = 0; echoIdx < msgpack_data.scandata[groupIdx].scanlines.size(); echoIdx++)
 		{
-			const std::vector<sick_scansegment_xd::MsgPackParserOutput::LidarPoint>& scanline = msgpack_data.scandata[groupIdx].scanlines[echoIdx];
+			const std::vector<sick_scansegment_xd::MsgPackParserOutput::LidarPoint>& scanline = msgpack_data.scandata[groupIdx].scanlines[echoIdx].points;
 			for (int pointIdx = 0; pointIdx < scanline.size(); pointIdx++)
 			{
 				const sick_scansegment_xd::MsgPackParserOutput::LidarPoint& point = scanline[pointIdx];