git commit -m "first commit for v2"

This commit is contained in:
2025-12-29 16:21:22 +07:00
commit aa3d832d5c
1807 changed files with 307078 additions and 0 deletions

View File

@@ -0,0 +1,18 @@
<launch>
<!-- sick_line_guidance: run sick_canopen_simu_node -->
<arg name="device"/> <!-- can device, either "OLS" or "MLS" -->
<node name="sick_canopen_simu_node" pkg="sick_line_guidance" type="sick_canopen_simu_node" />
<param name="sick_canopen_simu_cfg_file" type="str" value="$(find sick_line_guidance)/sick_canopen_simu_cfg.xml" /> <!-- configuration file and testcases for OLS and MLS simulation -->
<param name="can_node_id" type="int" value="10" /> <!-- can node id of OLS/MLS simulator -->
<param name="sick_device_family" type="str" value="$(arg device)" /> <!-- simulation of either "OLS10", "OLS20" or "MLS" device -->
<param name="can_subscribe_topic" type="str" value="can0" /> <!-- topic for ros can messages (input), message type can_msgs::Frame -->
<param name="mls_subscribe_topic" type="str" value="mls" /> <!-- topic for ros measurement messages (input), message type MLS_Measurement -->
<param name="ols_subscribe_topic" type="str" value="ols" /> <!-- topic for ros measurement messages (input), message type OLS_Measurement -->
<param name="publish_topic" type="str" value="ros2can0" /> <!-- topic for ros messages (output), message type can_msgs::Frame -->
<param name="pdo_rate" type="double" value="10" /> <!-- rate of PDOs: 50 PDOs simulated per second, i.e. 20 ms between two PDOs -->
<param name="pdo_repeat_cnt" type="int" value="25" /> <!-- each sensor state spefied in sick_canopen_simu_cfg.xml is repeated 25 times before switching to the next state (sensor state changes after 0.5 seconds) -->
<param name="can_message_queue_size" type="int" value="16" /> <!-- buffer size for can messages -->
<param name="sensor_state_queue_size" type="int" value="2" /> <!-- buffer size for simulated sensor states (OLS: 2 TPDOs) -->
</launch>

View File

@@ -0,0 +1,35 @@
<launch>
<!-- sick_line_guidance: global configuration -->
<arg name="yaml" default="$(find sick_line_guidance)/sick_line_guidance_mls.yaml"/>
<rosparam command="load" file="$(arg yaml)" /> <!-- Global CAN configuration by ols or mls yaml-file 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" >
<rosparam command="load" file="$(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" >
<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 and MLS_Measurement messages to PointCloud2 -->
<node name="sick_line_guidance_cloud_publisher" pkg="sick_line_guidance" type="sick_line_guidance_cloud_publisher">
<param name="mls_topic_publish" type="str" value="mls" /> <!-- MLS_Measurement data are published in topic "/mls" -->
<param name="ols_topic_publish" type="str" value="ols" /> <!-- OLS_Measurement data are published in topic "/ols" -->
<param name="cloud_topic_publish" type="str" value="cloud" /> <!-- sensor_msgs::PointCloud messages are published in topic "/cloud" -->
<param name="mls_cloud_frame_id" type="str" value="mls_frame" /> <!-- MLS PointCloud data are published with frame id "mls_frame" -->
<param name="ols_cloud_frame_id" type="str" value="ols_frame" /> <!-- OLS PointCloud data are published with frame id "ols_frame" -->
<param name="subscribe_queue_size" type="int" value="1" /> <!-- buffer size for ros messages -->
</node>
</launch>

View File

@@ -0,0 +1,8 @@
<launch>
<!-- sick_line_guidance: run can2ros_node support tool, converts can messages to ros messages -->
<node name="sick_line_guidance_can2ros_node" pkg="sick_line_guidance" type="sick_line_guidance_can2ros_node" />
<param name="can_device" type="str" value="can0" /> <!-- name of can net device (socketcan interface) -->
<param name="ros_topic" type="str" value="can0" /> <!-- topic for ros messages (output), message type can_msgs::Frame -->
</launch>

View File

@@ -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>

View File

@@ -0,0 +1,9 @@
<launch>
<!-- sick_line_guidance: run ros2can_node support tool, converts ros messages to can messages -->
<node name="sick_line_guidance_ros2can_node" pkg="sick_line_guidance" type="sick_line_guidance_ros2can_node" />
<param name="can_device" type="str" value="can0" /> <!-- name of can net device (socketcan interface) -->
<param name="ros_topic" type="str" value="ros2can0" /> <!-- topic for ros messages (input), message type can_msgs::Frame -->
<param name="subscribe_queue_size" type="int" value="32" /> <!-- buffer size for ros messages -->
</launch>