From 082fb3f1a76142010711643ea3268ae03e6552f0 Mon Sep 17 00:00:00 2001
From: Nicola Piga <nicolapiga@gmail.com>
Date: Wed, 27 Sep 2023 17:03:58 +0000
Subject: [PATCH] Support joints publishing on ROS 2 for
 iCubGazeboV2_5_visuomanip

---
 .../iCubGazeboV2_5_visuomanip/icub_ros2.xml   | 43 +++++++++++++++++++
 .../motorControl/alljoints-mc_nws_ros2.xml    | 11 +++++
 .../motorControl/alljoints-mc_remapper.xml    | 21 +++++++++
 3 files changed, 75 insertions(+)
 create mode 100644 iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/icub_ros2.xml
 create mode 100644 iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/wrappers/motorControl/alljoints-mc_nws_ros2.xml
 create mode 100644 iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/wrappers/motorControl/alljoints-mc_remapper.xml

diff --git a/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/icub_ros2.xml b/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/icub_ros2.xml
new file mode 100644
index 0000000..6c06d0b
--- /dev/null
+++ b/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/icub_ros2.xml
@@ -0,0 +1,43 @@
+<?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="iCubGazeboV3" build="1" portprefix="/icubSim" xmlns:xi="http://www.w3.org/2001/XInclude">
+    <devices>
+
+    <!-- MOTOR CONTROLLERS -->
+
+        <!-- wrapper are all inherited from automatically generated models -->
+        <xi:include href="../../../iCub/conf/wrappers/motorControl/torso-mc_wrapper.xml" />
+        <xi:include href="../../../iCub/conf/wrappers/motorControl/head-mc_wrapper.xml" />
+        <xi:include href="../../../iCub/conf/wrappers/motorControl/left_arm-mc_wrapper.xml" />
+        <xi:include href="../../../iCub/conf/wrappers/motorControl/right_arm-mc_wrapper.xml" />
+
+        <!-- only the torso remapper is inherited from automatically generated models -->
+        <xi:include href="../../../iCub/conf/wrappers/motorControl/torso-mc_remapper.xml" />
+
+        <!-- all the other remappers are specific to this model due to eyes and fingers -->
+        <xi:include href="wrappers/motorControl/head-mc_remapper.xml" />
+        <xi:include href="wrappers/motorControl/left_arm-mc_remapper.xml" />
+        <xi:include href="wrappers/motorControl/right_arm-mc_remapper.xml" />
+    <!-- -->
+
+    <!-- HEAD SENSORS -->
+
+        <xi:include href="wrappers/camera/left_camera-rgbd_wrapper.xml" />
+        <xi:include href="wrappers/camera/right_camera-rgb_wrapper.xml" />
+
+    <!-- -->
+
+    <!-- INERTIAL SENSORS -->
+
+        <!-- this wrapper is inherited from automatically generated models -->
+        <xi:include href="../../../iCub/conf/wrappers/inertials/head-inertials_wrapper.xml" />
+
+    <!-- -->
+
+    <!-- ROS 2 entries -->
+    <xi:include href="wrappers/motorControl/alljoints-mc_remapper.xml" />
+    <xi:include href="wrappers/motorControl/alljoints-mc_nws_ros2.xml" />
+
+    </devices>
+</robot>
diff --git a/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/wrappers/motorControl/alljoints-mc_nws_ros2.xml b/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/wrappers/motorControl/alljoints-mc_nws_ros2.xml
new file mode 100644
index 0000000..7432e32
--- /dev/null
+++ b/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/wrappers/motorControl/alljoints-mc_nws_ros2.xml
@@ -0,0 +1,11 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE devices PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<device xmlns:xi="http://www.w3.org/2001/XInclude" name="alljoints-mc_nws_ros2" type="controlBoard_nws_ros2">
+    <param name="node_name"> icub_cb_node </param>
+    <param name="topic_name"> /joint_states </param>
+    <action phase="startup" level="15" type="attach">
+        <param name="device"> alljoints-mc_remapper </param>
+    </action>
+    <action phase="shutdown" level="20" type="detach" />
+</device>
diff --git a/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/wrappers/motorControl/alljoints-mc_remapper.xml b/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/wrappers/motorControl/alljoints-mc_remapper.xml
new file mode 100644
index 0000000..99d7392
--- /dev/null
+++ b/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/wrappers/motorControl/alljoints-mc_remapper.xml
@@ -0,0 +1,21 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE devices PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<device xmlns:xi="http://www.w3.org/2001/XInclude" name="alljoints-mc_remapper" type="controlboardremapper">
+    <paramlist name="networks">
+        <elem name="head_joints">       (  0  2 0 2 )</elem>
+        <elem name="left_arm_joints">   (  3  9 0 6 )</elem>
+        <elem name="right_arm_joints">  ( 10 16 0 6 )</elem>
+        <elem name="torso_joints">      ( 17 19 0 2 )</elem>
+    </paramlist>
+    <param name="joints"> 20 </param>
+    <action phase="startup" level="6" type="attach">
+        <paramlist name="networks">
+            <elem name="head_joints">       head_hardware_device </elem>
+            <elem name="left_arm_joints">   left_arm_hardware_device</elem>
+            <elem name="right_arm_joints">  right_arm_hardware_device </elem>
+            <elem name="torso_joints">      torso_hardware_device </elem>
+        </paramlist>
+    </action>
+    <action phase="shutdown" level="19" type="detach" />
+</device>