Added range sensor

Signed-off-by: Jakub Delicat <[email protected]>

Author: delihus

rosbot_description/urdf/body.urdf.xacro

@@ -1,7 +1,7 @@
 <?xml version="1.0" encoding="utf-8"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
   <!-- body defining macro -->
-  <xacro:macro name="body" params="wheel_radius mecanum use_gpu">
+  <xacro:macro name="body" params="wheel_radius mecanum use_gpu namespace">
     <xacro:include filename="$(find rosbot_description)/urdf/components/vl53lox.urdf.xacro" ns="range_sensor" />
 
     <link name="base_link" />
@@ -79,10 +79,10 @@
 
     <link name="camera_link" />
 
-    <xacro:range_sensor.vl53lox parent="body_link" xyz="0.0926  0.05 0.015"   rpy="0.0 0.0 ${radians(12.5)}"  prefix="fl" />
-    <xacro:range_sensor.vl53lox parent="body_link" xyz="0.0926 -0.05 0.015"   rpy="0.0 0.0 ${radians(-12.5)}" prefix="fr" />
-    <xacro:range_sensor.vl53lox parent="body_link" xyz="-0.0926 0.05 0.0115"  rpy="0.0 0.0 ${radians(167.5)}" prefix="rl" />
-    <xacro:range_sensor.vl53lox parent="body_link" xyz="-0.0926 -0.05 0.0115" rpy="0.0 0.0 ${radians(192.5)}" prefix="rr" />
+    <xacro:range_sensor.vl53lox parent="body_link" xyz="0.0926  0.05 0.015"   rpy="0.0 0.0 ${radians(12.5)}"  prefix="fl" namespace="${namespace}" />
+    <xacro:range_sensor.vl53lox parent="body_link" xyz="0.0926 -0.05 0.015"   rpy="0.0 0.0 ${radians(-12.5)}" prefix="fr" namespace="${namespace}" />
+    <xacro:range_sensor.vl53lox parent="body_link" xyz="-0.0926 0.05 0.0115"  rpy="0.0 0.0 ${radians(167.5)}" prefix="rl" namespace="${namespace}" />
+    <xacro:range_sensor.vl53lox parent="body_link" xyz="-0.0926 -0.05 0.0115" rpy="0.0 0.0 ${radians(192.5)}" prefix="rr" namespace="${namespace}" />
 
   </xacro:macro>
 </robot>

rosbot_description/urdf/components/vl53lox.urdf.xacro

@@ -1,7 +1,16 @@
 <?xml version="1.0" encoding="utf-8"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
   <!-- vl53lox defining macro -->
-  <xacro:macro name="vl53lox" params="parent xyz rpy prefix">
+  <xacro:macro name="vl53lox" params="parent xyz rpy prefix namespace">
+
+    <xacro:if value="${namespace == 'None'}">
+      <xacro:property name="namespace_ext" value="" />
+      <xacro:property name="gz_control_namespace" value="/" />
+    </xacro:if>
+    <xacro:unless value="${namespace == 'None'}">
+        <xacro:property name="namespace_ext" value="${namespace}/" />
+        <xacro:property name="gz_control_namespace" value="${namespace}" />
+    </xacro:unless>
 
     <joint name="${parent}_${prefix}_range_joint" type="fixed">
       <origin xyz="${xyz}" rpy="${rpy}" />
@@ -11,6 +20,48 @@
 
     <link name="${prefix}_range" />
 
+    <!-- an IR sensor or a sonar are not implemented yet
+    https://github.com/gazebosim/gz-sensors/issues/19 -->
+    <xacro:if value="${simulation_engine == 'ignition-gazebo'}">
+      <gazebo reference="${prefix}_range">
+        <sensor name="${namespace_ext}${prefix}_range" type='gpu_lidar'>
+
+          <topic>${gz_control_namespace}range/${prefix}</topic>
+          <frame_id>${prefix}_range</frame_id>
+          <ignition_frame_id>${prefix}_range</ignition_frame_id>
+
+          <update_rate>5.0</update_rate>
+          <ray>
+            <scan>
+              <horizontal>
+                <samples>1</samples>
+                <resolution>1</resolution>
+                <min_angle>-0.01</min_angle>
+                <max_angle>0.01</max_angle>
+              </horizontal>
+            </scan>
+            <range>
+              <min>0.01</min>
+              <max>0.90</max>
+              <resolution>0.02</resolution>
+            </range>
+            <noise>
+              <type>gaussian</type>
+              <mean>0.1</mean>
+              <stddev>0.005</stddev>
+            </noise>
+          </ray>
+          <always_on>true</always_on>
+          <visualize>false</visualize>
 
+          <ros>
+            <namespace></namespace>
+          </ros>
+        </sensor>
+        <plugin filename="libignition-gazebo-sensors-system.so"  name="ignition::gazebo::systems::Sensors">
+          <render_engine>ogre2</render_engine>
+        </plugin>
+      </gazebo>
+    </xacro:if>
   </xacro:macro>
 </robot>

rosbot_description/urdf/rosbot_macro.urdf.xacro

@@ -23,7 +23,7 @@
     <xacro:include filename="$(find rosbot_description)/urdf/wheel.urdf.xacro" ns="wheel" />
 
     <!-- BODY DECLARATION -->
-    <xacro:body.body wheel_radius="${wheel_radius}" mecanum="${mecanum}" use_gpu="${use_gpu}" />
+    <xacro:body.body wheel_radius="${wheel_radius}" mecanum="${mecanum}" use_gpu="${use_gpu}" namespace="${namespace}" />
 
     <!-- WHEEL DECLARATION -->
     <xacro:wheel.wheel wheel_radius="${wheel_radius}" side="fr" mecanum="${mecanum}" />