From e0406da97b064d117369d0cf8419980c7bd49f37 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= <shernand@iri.upc.edu>
Date: Wed, 30 Aug 2017 18:16:29 +0200
Subject: [PATCH] Added a namespace to the sensor model to be used in both the
 darwin and the bioloid robots.

---
 bioloid_description/urdf/sensors/cny70_ir.gazebo |  6 +++---
 bioloid_description/urdf/sensors/cny70_ir.xacro  |  4 ++--
 bioloid_description/urdf/sensors/feet_ir.xacro   | 12 ++++++------
 3 files changed, 11 insertions(+), 11 deletions(-)

diff --git a/bioloid_description/urdf/sensors/cny70_ir.gazebo b/bioloid_description/urdf/sensors/cny70_ir.gazebo
index ed1b1f6..907f5a1 100644
--- a/bioloid_description/urdf/sensors/cny70_ir.gazebo
+++ b/bioloid_description/urdf/sensors/cny70_ir.gazebo
@@ -2,7 +2,7 @@
 
 <root xmlns:xacro="http://ros.org/wiki/xacro">
 
-  <xacro:macro name="cny70_ir_gazebo" params="name update_rate fov min_range max_range">
+  <xacro:macro name="cny70_ir_gazebo" params="name namespace update_rate fov min_range max_range">
     <gazebo reference="${name}_ir_link">
       <material>Gazebo/Black</material>
       <gravity>true</gravity>
@@ -42,8 +42,8 @@
           </range>
         </ray>
         <plugin name="${name}_range_sensor" filename="libgazebo_ros_range.so">
-          <frameName>/bioloid/${name}_ir_link</frameName>
-          <topicName>/bioloid/sensors/range</topicName>
+          <frameName>/${namespace}/${name}_ir_link</frameName>
+          <topicName>/${namespace}/sensors/range</topicName>
           <radiation>infrared</radiation>
           <fov>${fov}</fov>
           <gaussianNoise>0.0005</gaussianNoise>
diff --git a/bioloid_description/urdf/sensors/cny70_ir.xacro b/bioloid_description/urdf/sensors/cny70_ir.xacro
index 94ba899..bac20e1 100644
--- a/bioloid_description/urdf/sensors/cny70_ir.xacro
+++ b/bioloid_description/urdf/sensors/cny70_ir.xacro
@@ -4,7 +4,7 @@
 
   <xacro:include filename="$(find bioloid_description)/urdf/sensors/cny70_ir.gazebo" />
 
-  <xacro:macro name="cny70_ir" params="name parent *origin update_rate fov min_range max_range">
+  <xacro:macro name="cny70_ir" params="name namespace parent *origin update_rate fov min_range max_range">
   <!-- IR distance sensors -->
     <link name="${name}_ir_link">
       <collision>
@@ -32,7 +32,7 @@
       <child link="${name}_ir_link"/>
     </joint>
 
-    <xacro:cny70_ir_gazebo name="${name}" update_rate="${update_rate}" fov="${fov}" min_range="${min_range}" max_range="${max_range}"/>
+    <xacro:cny70_ir_gazebo name="${name}" namespace="${namespace}" update_rate="${update_rate}" fov="${fov}" min_range="${min_range}" max_range="${max_range}"/>
   </xacro:macro>
 </root>
 
diff --git a/bioloid_description/urdf/sensors/feet_ir.xacro b/bioloid_description/urdf/sensors/feet_ir.xacro
index 4e6b8de..ecb4495 100644
--- a/bioloid_description/urdf/sensors/feet_ir.xacro
+++ b/bioloid_description/urdf/sensors/feet_ir.xacro
@@ -34,15 +34,15 @@
     </joint>
 
     <!-- sensor links --> 
-    <xacro:cny70_ir name="left_front_fwd" parent="left_board_link" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
+    <xacro:cny70_ir name="left_front_fwd" namespace="bioloid" parent="left_board_link" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
       <origin xyz="0.0085 -0.0055 0.038" rpy="-1.5707 -1.5707 0" />
     </xacro:cny70_ir>
 
-    <xacro:cny70_ir name="left_front_dwn" parent="left_board_link" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
+    <xacro:cny70_ir name="left_front_dwn" namespace="bioloid" parent="left_board_link" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
       <origin xyz="0.0225 -0.012 0.0345" rpy="0 0 -1.5707" />
     </xacro:cny70_ir>
 
-    <xacro:cny70_ir name="left_lateral_dwn" parent="left_board_link" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
+    <xacro:cny70_ir name="left_lateral_dwn" namespace="bioloid" parent="left_board_link" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
       <origin xyz="0.0575 -0.012 0.0065" rpy="0 0 -1.5707" />
     </xacro:cny70_ir>
 
@@ -74,15 +74,15 @@
     </joint>
 
     <!-- sensor links --> 
-    <xacro:cny70_ir name="right_front_fwd" parent="right_board_link" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
+    <xacro:cny70_ir name="right_front_fwd" namespace="bioloid" parent="right_board_link" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
       <origin xyz="-0.0085 -0.0055 0.038" rpy="-1.5707 -1.5707 0" />
     </xacro:cny70_ir>
 
-    <xacro:cny70_ir name="right_front_dwn" parent="right_board_link" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
+    <xacro:cny70_ir name="right_front_dwn" namespace="bioloid" parent="right_board_link" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
       <origin xyz="-0.0225 -0.012 0.0345" rpy="0 0 -1.5707" />
     </xacro:cny70_ir>
 
-    <xacro:cny70_ir name="right_lateral_dwn" parent="right_board_link" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
+    <xacro:cny70_ir name="right_lateral_dwn" namespace="bioloid" parent="right_board_link" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
       <origin xyz="-0.0575 -0.012 0.0065" rpy="0 0 -1.5707" />
     </xacro:cny70_ir>
 
-- 
GitLab