Merge branch 'feature/multiple_ols20'

Author: rostest

CMakeLists.txt

@@ -294,6 +294,7 @@ install(DIRECTORY include/${PROJECT_NAME}/
 install(FILES
   launch/sick_line_guidance.launch
   launch/sick_line_guidance_can2ros_node.launch
+  launch/sick_line_guidance_ols20_twin.launch
   launch/sick_line_guidance_ros2can_node.launch
   launch/sick_canopen_simu.launch
   mls/SICK-MLS.eds

README.md

@@ -86,8 +86,8 @@ source ./install/setup.bash
 ```
 after modifications.
 
-See https://github.com/SICKAG/sick_line_guidance/tree/master/doc/sick_line_guidance_configuration.md for details about the 
-sick_line_guidance driver and sensor configuration.    
+See [sick_line_guidance configuration](doc/sick_line_guidance_configuration.md) for details about the 
+sick_line_guidance driver and sensor configuration. See [Configuration for multiple devices](doc/sick_line_guidance_configuration.md#configuration-for-multiple-devices) for the configuration of multiple devices, f.e. of two OLS20 devices.
 
 # Starting
 

doc/sick_line_guidance_configuration.md

@@ -179,10 +179,9 @@ Device specific settings can be configured in section "nodes/node1/dcf_overlay".
 Set the CAN-ID of your device with `id: 0x0A # CAN-Node-ID of MLS (default: 0x0A)` and
 configure device specific settings in section `dcf_overlay` by appending a line `"objectindex": "parametervalue"` for each parameter.
 
-## yaml configuration for multiple can devices
+## Configuration for multiple devices
 
-If you want to use multiple can devices (MLS or OLS) on one system, you have to append multiple nodes in your yaml-file. Each node must contain the configuration of
-one can device as shown above. Just make sure to use different node names for each device:
+If you want to use multiple can devices (MLS or OLS) on one system, you have to append multiple nodes in your yaml-file. Each node must contain the configuration of one can device as shown above. Make sure to use different node names for each device:
 ```yaml
 nodes:
   node1:
@@ -191,11 +190,22 @@ nodes:
     id: ... # CAN-ID for the 2. device
 ```
 
-If you configure different measurement topics for each can device (f.e. `sick_topic: "ols20-1"` for the first OLS20 and `sick_topic: "ols20-1"` for the second OLS20 device),
-you have to start multiple sick_line_guidance_cloud_publisher nodes, too. Otherwise, measurement messges from this device can't be transformed to PointCloud2 messsages.
+If you configure different measurement topics for each can device (f.e. `sick_topic: "ols20A"` for the first OLS20 and `sick_topic: "ols20B"` for the second OLS20 device), you have to start multiple sick_line_guidance_cloud_publisher nodes, too. Otherwise, measurement messges from this device can't be transformed to PointCloud2 messsages.
+
 Append multiple sick_line_guidance_cloud_publisher nodes in file sick_line_guidance.launch, each sick_line_guidance_cloud_publisher node configured with the corresponding 
 ros topic in parameter `"mls_topic_publish"` resp. `"ols_topic_publish"`.
 
+Launchfile [sick_line_guidance_ols20_twin.launch](../launch/sick_line_guidance_ols20_twin.launch) and yaml-configuration [sick_line_guidance_ols20_twin.yaml](../ols/sick_line_guidance_ols20_twin.yaml) show an example how to run two OLS20 devices with can node ids 0x0A and 0x0B. Run the example with
+```
+rosrun rviz rviz &
+roslaunch -v --screen sick_line_guidance sick_line_guidance_ols20_twin.launch
+```
+The point clouds of both OLS20 devices will be published on topic "cloudA" and "cloudB" with frame ids "olsA_frame" and "olsB_frame". Use a static_transform_publisher to setup a transform for each OLS frame to a reference coordinate system, e.g.
+```
+rosrun tf static_transform_publisher 0 0 0 0 0 0 olsA_frame cloud 10
+rosrun tf static_transform_publisher 0 0 0 0 0 0 olsB_frame cloud 10
+```
+
 ## Read and write parameter during runtime
 
 You can read and write to the object dictionary of a can device on runtime, too, by using the ros services implemented by the canopen driver.

launch/sick_line_guidance_ols20_twin.launch

@@ -0,0 +1,45 @@
+<launch>
+
+  <!-- sick_line_guidance: configuration for two OLS20 devices (can node ids A and B) -->
+  <arg name="yaml" default="sick_line_guidance_ols20_twin.yaml"/>
+  <rosparam command="load" file="$(find sick_line_guidance)/$(arg yaml)" />                    <!-- Global CAN configuration two OLS20 devices (can node ids A and B) incl. link to eds-file -->
+  
+  <!-- sick_line_guidance: run canopen_chain_node -->
+  <!-- to run sick_line_guidance_can_chain_node in gdb: add parameter launch-prefix="gdb -ex run - -args": -->
+  <!-- node name="sick_line_guidance_can_chain_node" pkg="sick_line_guidance" type="sick_line_guidance_can_chain_node" launch-prefix="gdb -ex run - -args" output="screen" -->
+  <node name="sick_line_guidance_can_chain_node" pkg="sick_line_guidance" type="sick_line_guidance_can_chain_node" output="screen" >
+    <rosparam command="load" file="$(find sick_line_guidance)/$(arg yaml)" />                 <!-- Private CAN configuration for canopen_chain_node by ols or mls yaml-file -->
+    <param name="diagnostic_topic" type="str" value="diagnostics" />                          <!-- ROS topic for diagnostic messages -->
+  </node>
+
+  <!-- sick_line_guidance: run sick_line_guidance_node, which implements the ROS driver for OLS and MLS -->
+  <node name="sick_line_guidance_node" pkg="sick_line_guidance" type="sick_line_guidance_node" output="screen" >
+    <param name="diagnostic_topic" type="str" value="diagnostics" />                           <!-- ROS topic for diagnostic messages -->
+    <param name="can_connect_init_at_startup" type="bool" value="true" />                      <!-- Additional CAN configuration: if true, canopen services are initialized at startup -->
+    <param name="initial_sensor_state" type="int" value="7" />                                 <!-- initial sensor states (f.e. 0x07 for 3 detected lines, or 8 to indicate sensor error) -->
+    <param name="subscribe_queue_size" type="int" value="16" />                                <!-- buffer size for ros messages -->
+    <param name="max_sdo_rate" type="double" value="100.0" />                                  <!-- max. sdo query and publish rate -->
+    <param name="max_num_retries_after_sdo_error" type="int" value="2" />                      <!-- After SDO error, the SDO query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted. -->
+  </node>
+
+  <!-- sick_line_guidance: cloud_publisher_node to convert OLS_Measurement messages of first OLS20 device (can node id A) to PointCloud2 (topic "cloudA", frame id "olsA_frame") -->
+  <node name="sick_line_guidance_cloudA_publisher" pkg="sick_line_guidance" type="sick_line_guidance_cloud_publisher" output="screen" >
+    <param name="mls_topic_publish" type="str" value="mlsA" />                                 <!-- MLS_Measurement data are published in topic "/mlsA" -->
+    <param name="ols_topic_publish" type="str" value="olsA" />                                 <!-- OLS_Measurement data are published in topic "/olsA" -->
+    <param name="cloud_topic_publish" type="str" value="cloudA" />                             <!-- sensor_msgs::PointCloud messages are published in topic "/cloudA" -->
+    <param name="mls_cloud_frame_id" type="str" value="mlsA_frame" />                          <!-- MLS PointCloud data are published with frame id "mlsA_frame" -->
+    <param name="ols_cloud_frame_id" type="str" value="olsA_frame" />                          <!-- OLS PointCloud data are published with frame id "olsA_frame" -->
+    <param name="subscribe_queue_size" type="int" value="1" />                                 <!-- buffer size for ros messages -->
+  </node>
+
+  <!-- sick_line_guidance: cloud_publisher_node to convert OLS_Measurement messages of second OLS20 device (can node id B) to PointCloud2 (topic "cloudB", frame id "olsB_frame") -->
+  <node name="sick_line_guidance_cloudB_publisher" pkg="sick_line_guidance" type="sick_line_guidance_cloud_publisher" output="screen" >
+    <param name="mls_topic_publish" type="str" value="mlsB" />                                 <!-- MLS_Measurement data are published in topic "/mlsB" -->
+    <param name="ols_topic_publish" type="str" value="olsB" />                                 <!-- OLS_Measurement data are published in topic "/olsB" -->
+    <param name="cloud_topic_publish" type="str" value="cloudB" />                             <!-- sensor_msgs::PointCloud messages are published in topic "/cloudB" -->
+    <param name="mls_cloud_frame_id" type="str" value="mlsB_frame" />                          <!-- MLS PointCloud data are published with frame id "mlsB_frame" -->
+    <param name="ols_cloud_frame_id" type="str" value="olsB_frame" />                          <!-- OLS PointCloud data are published with frame id "olsB_frame" -->
+    <param name="subscribe_queue_size" type="int" value="1" />                                 <!-- buffer size for ros messages -->
+  </node>
+
+</launch>

ols/sick_line_guidance_ols20_twin.yaml

@@ -19,7 +19,7 @@ nodes:
 
     # sick_line_guidance configuration of this node:
     sick_device_family: "OLS20"            # can devices currently supported: "OLS10", "OLS20" or "MLS"
-    sick_topic: "ols"                      # OLS_Measurement messages are published in topic "/ols"
+    sick_topic: "olsA"                     # OLS_Measurement messages are published in topic "/ols"
     sick_frame_id: "ols20A_measurement_frame" # OLS_Measurement messages are published with frame_id "ols_measurement_frame"
 
     # device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue"
@@ -39,6 +39,6 @@ nodes:
 
     # sick_line_guidance configuration of this node:
     sick_device_family: "OLS20"            # can devices currently supported: "OLS10", "OLS20" or "MLS"
-    sick_topic: "ols"                      # OLS_Measurement messages are published in topic "/ols"
+    sick_topic: "olsB"                     # OLS_Measurement messages are published in topic "/ols"
     sick_frame_id: "ols20B_measurement_frame" # OLS_Measurement messages are published with frame_id "ols_measurement_frame"
 

src/sick_line_guidance_cloud_publisher.cpp

@@ -129,18 +129,26 @@ int main(int argc, char** argv)
 {
   ros::init(argc, argv, "sick_line_guidance_cloud_publisher");
   ros::NodeHandle nh;
+  ros::NodeHandle nh_priv("~");
 
   int subscribe_queue_size = 1;
   std::string ols_topic_publish = "ols", mls_topic_publish = "mls", cloud_topic_publish = "cloud";
-  nh.param("/sick_line_guidance_cloud_publisher/mls_topic_publish", mls_topic_publish, mls_topic_publish);
-  nh.param("/sick_line_guidance_cloud_publisher/ols_topic_publish", ols_topic_publish, ols_topic_publish);
-  nh.param("/sick_line_guidance_cloud_publisher/cloud_topic_publish", cloud_topic_publish, cloud_topic_publish);
-  nh.param("/sick_line_guidance_cloud_publisher/mls_cloud_frame_id", mls_cloud_frame_id, mls_cloud_frame_id);
-  nh.param("/sick_line_guidance_cloud_publisher/ols_cloud_frame_id", ols_cloud_frame_id, ols_cloud_frame_id);
-  nh.param("/sick_line_guidance_cloud_publisher/subscribe_queue_size", subscribe_queue_size, subscribe_queue_size); // buffer size for ros messages
-  
+  nh_priv.param("mls_topic_publish", mls_topic_publish, mls_topic_publish);
+  nh_priv.param("ols_topic_publish", ols_topic_publish, ols_topic_publish);
+  nh_priv.param("cloud_topic_publish", cloud_topic_publish, cloud_topic_publish);
+  nh_priv.param("mls_cloud_frame_id", mls_cloud_frame_id, mls_cloud_frame_id);
+  nh_priv.param("ols_cloud_frame_id", ols_cloud_frame_id, ols_cloud_frame_id);
+  nh_priv.param("subscribe_queue_size", subscribe_queue_size, subscribe_queue_size); // buffer size for ros messages
+
+
   ROS_INFO_STREAM("sick_line_guidance_cloud_publisher: version " << sick_line_guidance::Version::getVersionInfo());
   ROS_INFO_STREAM("sick_line_guidance_cloud_publisher started.");
+  ROS_INFO_STREAM("sick_line_guidance_cloud_publisher: mls_topic=" << mls_topic_publish);
+  ROS_INFO_STREAM("sick_line_guidance_cloud_publisher: ols_topic=" << ols_topic_publish);
+  ROS_INFO_STREAM("sick_line_guidance_cloud_publisher: cloud_topic=" << cloud_topic_publish);
+  ROS_INFO_STREAM("sick_line_guidance_cloud_publisher: mls_frame_id=" << mls_cloud_frame_id);
+  ROS_INFO_STREAM("sick_line_guidance_cloud_publisher: ols_frame_id=" << ols_cloud_frame_id);
+  ROS_INFO_STREAM("sick_line_guidance_cloud_publisher: queue_size=" << subscribe_queue_size);
   cloud_publisher = nh.advertise<sensor_msgs::PointCloud2>(cloud_topic_publish, 1);
   ros::Subscriber ols_subscriber = nh.subscribe(ols_topic_publish, subscribe_queue_size, olsMessageCb);
   ros::Subscriber mls_subscriber = nh.subscribe(mls_topic_publish, subscribe_queue_size, mlsMessageCb);