From 370f060c71653d1970decb89ca9ccd2b28a22e5a 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

---
 CHANGELOG.md                                  |  2 +-
 .../iCubGazeboV2_5_visuomanip/icub_ros2.xml   | 43 +++++++++++++++++++
 .../motorControl/alljoints-mc_nws_ros2.xml    | 11 +++++
 .../motorControl/alljoints-mc_remapper.xml    | 15 +++++++
 4 files changed, 70 insertions(+), 1 deletion(-)
 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/CHANGELOG.md b/CHANGELOG.md
index 89440b7..edec9ea 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -14,7 +14,7 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo
 ### Changed
 
 * Tune position PID gains for torso for iCub 2.* (https://github.com/robotology/icub-models-generator/pull/241).
-
+* Enable publishing of robot joints in ROS 2 via `controlBoard_nws_ros2` for `iCubGazeboV2_5_visuomanip` (https://github.com/robotology/icub-models/pull/211).
 
 # [2.1.0] - 2023-06-05
 
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..58e54a4
--- /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="10" type="attach">
+        <param name="device"> alljoints-mc_remapper </param>
+    </action>
+    <action phase="shutdown" level="15" 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..6f9a1df
--- /dev/null
+++ b/iCub_manual/conf_manual/iCubGazeboV2_5_visuomanip/wrappers/motorControl/alljoints-mc_remapper.xml
@@ -0,0 +1,15 @@
+<?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">
+    <param name="axesNames">(neck_pitch neck_roll neck_yaw l_shoulder_pitch l_shoulder_roll l_shoulder_yaw l_elbow l_wrist_prosup l_wrist_pitch l_wrist_yaw r_shoulder_pitch r_shoulder_roll r_shoulder_yaw r_elbow r_wrist_prosup r_wrist_pitch r_wrist_yaw torso_yaw torso_roll torso_pitch)</param>
+    <action phase="startup" level="5" 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="20" type="detach"/>
+</device>