From 05f300ca9f71bb0680958f78360f44ee0e6cfe0f Mon Sep 17 00:00:00 2001
From: Simone Micheletti <86918431+SimoneMic@users.noreply.github.com>
Date: Wed, 14 Jun 2023 07:43:17 +0200
Subject: [PATCH] added configuration for launching ros2 sensors wrappers
 (#530)

---
 ergoCubSN000/sensors/realsense_ros2.xml | 48 +++++++++++++++++++++++
 ergoCubSN000/sensors/rplidar_ros2.xml   | 51 +++++++++++++++++++++++++
 ergoCubSN000/sensors_ros2.xml           | 11 ++++++
 3 files changed, 110 insertions(+)
 create mode 100644 ergoCubSN000/sensors/realsense_ros2.xml
 create mode 100644 ergoCubSN000/sensors/rplidar_ros2.xml
 create mode 100644 ergoCubSN000/sensors_ros2.xml

diff --git a/ergoCubSN000/sensors/realsense_ros2.xml b/ergoCubSN000/sensors/realsense_ros2.xml
new file mode 100644
index 000000000..ef81480cf
--- /dev/null
+++ b/ergoCubSN000/sensors/realsense_ros2.xml
@@ -0,0 +1,48 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<devices>
+    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="realsenseD450" type="realsense2">
+        <group name="SETTINGS">
+            <param name="depthResolution"> (640 480) </param>
+            <param name="rgbResolution">  (640 480) </param>
+            <param name="framerate">    30  </param>
+            <param name="enableEmitter"> true </param>
+            <param name="alignmentFrame"> RGB </param>
+        </group>
+        <group name="HW_DESCRIPTION">
+            <param name="clipPlanes"> (0.2 10.0) </param>
+        </group>
+        <group name="QUANT_PARAM">
+            <param name="depth_quant"> 2 </param>
+        </group>
+        <param name="rotateImage180"> false </param>
+    </device>
+
+    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="RGBDWrapperyarp" type="rgbdSensor_nws_yarp">
+        <param name="period"> 0.03 </param>
+        <param name="name">    /depthCamera </param>
+        <action phase="startup" level="5" type="attach">
+            <paramlist name="networks">
+                <elem name="subdevicergbd"> realsenseD450 </elem>
+            </paramlist>
+        </action>
+        <action phase="shutdown" level="5" type="detach" />
+    </device>
+
+    <device xmlns:xi="http://www.w3.org/2001/XInclude"  name="depthWrapR" type="rgbdSensor_nws_ros2">
+        <param name="period">0.033</param>
+        <param name="node_name">rgbdsensor_ros2_node</param>
+        <param name="color_topic_name">/camera_rgbd/color/image_rect_color</param>
+        <param name="depth_topic_name">/camera_rgbd/depth/image_rect</param>
+        <param name="color_frame_id">realsense_rgb_frame</param>
+        <param name="depth_frame_id">realsense_rgb_frame</param>
+        <action phase="startup" level="5" type="attach">
+            <paramlist name="networks">
+            <elem name="the_device"> realsenseD450 </elem>
+            </paramlist>
+        </action>
+        <action phase="shutdown" level="5" type="detach" />
+    </device>
+
+</devices>
\ No newline at end of file
diff --git a/ergoCubSN000/sensors/rplidar_ros2.xml b/ergoCubSN000/sensors/rplidar_ros2.xml
new file mode 100644
index 000000000..01f44e828
--- /dev/null
+++ b/ergoCubSN000/sensors/rplidar_ros2.xml
@@ -0,0 +1,51 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+ 
+<devices>
+    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="rpLidarS2" type="rpLidar4">
+        <group name="GENERAL">
+            <param name="name">                rpLidar4      </param>
+            <param name="serial_baudrate">     1000000       </param>
+            <param name="serial_port">         /dev/ttyUSB0  </param>
+            <param name="sample_buffer_life">  1             </param>
+        </group>
+        <group name="SENSOR">
+            <param name="max_distance">    30.0         </param>
+            <param name="min_distance">    0.1          </param>
+            <param name="max_angle">       360          </param>
+            <param name="min_angle">       0            </param>
+            <param name="resolution">      0.12         </param>
+            <param name="allow_infinity">  1            </param>
+        </group>
+        <group name="RPLIDAR">
+            <param name="scan_mode">  DenseBoost        </param>
+        </group>
+    </device>
+
+    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="laser_wrapper_yarp" type="rangefinder2D_nws_yarp">
+        <param name="period">  0.01      </param>
+        <param name="name">    /ergocub/laser:o  </param>
+        <action phase="startup" level="10" type="attach">
+            <paramlist name="networks">
+            <elem name="laser"> rpLidarS2 </elem>
+            </paramlist>
+        </action>
+        <action phase="shutdown" level="15" type="detach"/>
+    </device>
+
+
+    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="lidarWrapR" type="rangefinder2D_nws_ros2">
+        <param name="period"> 0.1 </param>
+        <param name="node_name">    rangefinder_ros2_node    </param>
+        <param name="topic_name">   /scan   </param>
+        <param name="frame_id">    head_laser_frame   </param>
+        <action phase="startup" level="5" type="attach">
+            <paramlist name="networks">
+            <elem name="the_device"> rpLidarS2 </elem>
+            </paramlist>
+        </action>
+        <action phase="shutdown" level="5" type="detach" />
+    </device>
+
+</devices>
\ No newline at end of file
diff --git a/ergoCubSN000/sensors_ros2.xml b/ergoCubSN000/sensors_ros2.xml
new file mode 100644
index 000000000..a93726bfc
--- /dev/null
+++ b/ergoCubSN000/sensors_ros2.xml
@@ -0,0 +1,11 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<robot name="ergoCubSN000" build="1" portprefix="/ergocub" xmlns:xi="http://www.w3.org/2001/XInclude">
+
+    <devices>
+        <xi:include href="sensors/rplidar_ros2.xml" /> 
+        <xi:include href="sensors/realsense_ros2.xml" />
+    </devices>
+
+</robot>
\ No newline at end of file