From 74a517e4e5c1b9ba6edbbb5a2e6053d70505c279 Mon Sep 17 00:00:00 2001
From: Natalia Lyubova <nlyubova@aldebaran.com>
Date: Fri, 3 Mar 2017 15:39:26 +0100
Subject: [PATCH] adding sonar sensors to Gazebo

---
 .../urdf/pepper1.0_generated_urdf/pepper.urdf | 161 +++++++++---------
 .../pepperGazebo.xacro                        |  52 +++---
 2 files changed, 105 insertions(+), 108 deletions(-)

diff --git a/pepper_description/urdf/pepper1.0_generated_urdf/pepper.urdf b/pepper_description/urdf/pepper1.0_generated_urdf/pepper.urdf
index c3327bb..32ef089 100644
--- a/pepper_description/urdf/pepper1.0_generated_urdf/pepper.urdf
+++ b/pepper_description/urdf/pepper1.0_generated_urdf/pepper.urdf
@@ -2231,89 +2231,86 @@
   <!--
     Sonars
 -->
-  <!--
-<gazebo reference="SonarBack_frame">
-  <sensor type="ray" name="SonarBack">
-    <pose>0 0 0 0 0 0</pose>
-    <update_rate>20</update_rate>
-    <visualize>false</visualize>
-    <ray>
-      <scan>
-        <horizontal>
-          <samples>5</samples>
-          <resolution>1</resolution>
-          <min_angle>-0.2617993877991494</min_angle>
-          <max_angle>0.2617993877991494</max_angle> 
-        </horizontal>
-        <vertical>
-          <samples>5</samples>
-          <resolution>1</resolution>
-          <min_angle>-0.13</min_angle>
-          <max_angle>0.13</max_angle> 
-        </vertical>
-      </scan>
-      <range>
-        <min>0.025</min>
-        <max>2.55</max>
-        <resolution>1</resolution>
-      </range>
-    </ray>
-    <plugin filename="libgazebo_ros_range.so" name="gazebo_ros_range">
-      <robotNamespace>/pepper_robot</robotNamespace>
-      <gaussianNoise>0.05</gaussianNoise>
-      <alwaysOn>true</alwaysOn>
-      <updateRate>20</updateRate>
-      <topicName>sonar_back</topicName>
-      <frameName>SonarBack_frame</frameName>
-      <minRange>0.025</minRange>
-      <maxRange>2.55</maxRange>
-      <fov>0.5235987755982988</fov> 
-      <radiation>ultrasound</radiation>
-    </plugin>
-  </sensor>
-</gazebo> 
-
-<gazebo reference="SonarFront_frame">
-  <sensor type="ray" name="SonarFront">
-    <pose>0 0 0 0 0 0</pose>
-    <update_rate>20</update_rate>
-    <visualize>false</visualize>
-    <ray>
-      <scan>
-        <horizontal>
-          <samples>5</samples>
-          <resolution>1</resolution>
-          <min_angle>-0.2617993877991494</min_angle>
-          <max_angle>0.2617993877991494</max_angle> 
-        </horizontal>
-        <vertical>
-          <samples>5</samples>
+  <gazebo reference="SonarBack_frame">
+    <sensor name="SonarBack" type="ray">
+      <pose>0 0 0 0 0 0</pose>
+      <update_rate>20</update_rate>
+      <visualize>false</visualize>
+      <ray>
+        <scan>
+          <horizontal>
+            <samples>5</samples>
+            <resolution>3</resolution>
+            <min_angle>-0.52</min_angle>
+            <max_angle>0.52</max_angle>
+          </horizontal>
+          <vertical>
+            <samples>5</samples>
+            <resolution>3</resolution>
+            <min_angle>-0.52</min_angle>
+            <max_angle>0.52</max_angle>
+          </vertical>
+        </scan>
+        <range>
+          <min>0.0</min>
+          <max>5.0</max>
           <resolution>1</resolution>
-          <min_angle>-0.13</min_angle>
-          <max_angle>0.13</max_angle>
-        </vertical>
-      </scan>
-      <range>
-        <min>0.025</min>
-        <max>2.55</max> 
-        <resolution>1</resolution>
-      </range>
-    </ray>
-    <plugin filename="libgazebo_ros_range.so" name="gazebo_ros_range">
-      <robotNamespace>/pepper_robot</robotNamespace>
-      <gaussianNoise>0.05</gaussianNoise>
-      <alwaysOn>true</alwaysOn>
-      <updateRate>20</updateRate>
-      <topicName>sonar_front</topicName>
-      <frameName>SonarFront_frame</frameName>
-      <minRange>0.025</minRange>
-      <maxRange>2.55</maxRange>
-      <fov>0.5235987755982988</fov>
-      <radiation>ultrasound</radiation>
-    </plugin>
-  </sensor>
-</gazebo> 
--->
+        </range>
+      </ray>
+      <plugin filename="libgazebo_ros_range.so" name="gazebo_ros_range">
+        <robotNamespace>/pepper_robot</robotNamespace>
+        <gaussianNoise>0.05</gaussianNoise>
+        <alwaysOn>true</alwaysOn>
+        <updateRate>20</updateRate>
+        <topicName>sonar_back</topicName>
+        <frameName>SonarBack_frame</frameName>
+        <minRange>0.0</minRange>
+        <maxRange>5.0</maxRange>
+        <fov>1.05</fov>
+        <radiation>ultrasound</radiation>
+      </plugin>
+    </sensor>
+  </gazebo>
+  <gazebo reference="SonarFront_frame">
+    <sensor name="SonarFront" type="ray">
+      <pose>0 0 0 0 0 0</pose>
+      <update_rate>20</update_rate>
+      <visualize>false</visualize>
+      <ray>
+        <scan>
+          <horizontal>
+            <samples>5</samples>
+            <resolution>3</resolution>
+            <min_angle>-0.52</min_angle>
+            <max_angle>0.52</max_angle>
+          </horizontal>
+          <vertical>
+            <samples>5</samples>
+            <resolution>3</resolution>
+            <min_angle>-0.52</min_angle>
+            <max_angle>0.52</max_angle>
+          </vertical>
+        </scan>
+        <range>
+          <min>0.0</min>
+          <max>5.0</max>
+          <resolution>3</resolution>
+        </range>
+      </ray>
+      <plugin filename="libgazebo_ros_range.so" name="gazebo_ros_range">
+        <robotNamespace>/pepper_robot</robotNamespace>
+        <gaussianNoise>0.05</gaussianNoise>
+        <alwaysOn>true</alwaysOn>
+        <updateRate>20</updateRate>
+        <topicName>sonar_front</topicName>
+        <frameName>SonarFront_frame</frameName>
+        <minRange>0.0</minRange>
+        <maxRange>5.0</maxRange>
+        <fov>1.05</fov>
+        <radiation>ultrasound</radiation>
+      </plugin>
+    </sensor>
+  </gazebo>
   <!-- Drive controller -->
   <gazebo>
     <plugin filename="libgazebo_ros_diff_drive.so" name="differential_drive_controller">
diff --git a/pepper_description/urdf/pepper1.0_generated_urdf/pepperGazebo.xacro b/pepper_description/urdf/pepper1.0_generated_urdf/pepperGazebo.xacro
index fbf619e..9323eda 100644
--- a/pepper_description/urdf/pepper1.0_generated_urdf/pepperGazebo.xacro
+++ b/pepper_description/urdf/pepper1.0_generated_urdf/pepperGazebo.xacro
@@ -637,7 +637,7 @@
 <!--
     Sonars
 -->
-<!--
+
 <gazebo reference="SonarBack_frame">
   <sensor type="ray" name="SonarBack">
     <pose>0 0 0 0 0 0</pose>
@@ -647,21 +647,21 @@
       <scan>
         <horizontal>
           <samples>5</samples>
-          <resolution>1</resolution>
-          <min_angle>-0.2617993877991494</min_angle>
-          <max_angle>0.2617993877991494</max_angle> 
+          <resolution>3</resolution>
+          <min_angle>-0.52</min_angle>
+          <max_angle>0.52</max_angle>
         </horizontal>
         <vertical>
           <samples>5</samples>
-          <resolution>1</resolution>
-          <min_angle>-0.13</min_angle>
-          <max_angle>0.13</max_angle> 
+          <resolution>3</resolution>
+          <min_angle>-0.52</min_angle>
+          <max_angle>0.52</max_angle>
         </vertical>
       </scan>
       <range>
-        <min>0.025</min>
-        <max>2.55</max>
-        <resolution>1</resolution>
+        <min>0.0</min>
+        <max>5.0</max>
+        <resolution>3</resolution>
       </range>
     </ray>
     <plugin filename="libgazebo_ros_range.so" name="gazebo_ros_range">
@@ -671,9 +671,9 @@
       <updateRate>20</updateRate>
       <topicName>sonar_back</topicName>
       <frameName>SonarBack_frame</frameName>
-      <minRange>0.025</minRange>
-      <maxRange>2.55</maxRange>
-      <fov>0.5235987755982988</fov> 
+      <minRange>0.0</minRange>
+      <maxRange>5.0</maxRange>
+      <fov>1.05</fov>
       <radiation>ultrasound</radiation>
     </plugin>
   </sensor>
@@ -688,21 +688,21 @@
       <scan>
         <horizontal>
           <samples>5</samples>
-          <resolution>1</resolution>
-          <min_angle>-0.2617993877991494</min_angle>
-          <max_angle>0.2617993877991494</max_angle> 
+          <resolution>3</resolution>
+          <min_angle>-0.52</min_angle>
+          <max_angle>0.52</max_angle>
         </horizontal>
         <vertical>
           <samples>5</samples>
-          <resolution>1</resolution>
-          <min_angle>-0.13</min_angle>
-          <max_angle>0.13</max_angle>
+          <resolution>3</resolution>
+          <min_angle>-0.52</min_angle>
+          <max_angle>0.52</max_angle>
         </vertical>
       </scan>
       <range>
-        <min>0.025</min>
-        <max>2.55</max> 
-        <resolution>1</resolution>
+        <min>0.0</min>
+        <max>5.0</max>
+        <resolution>3</resolution>
       </range>
     </ray>
     <plugin filename="libgazebo_ros_range.so" name="gazebo_ros_range">
@@ -712,14 +712,14 @@
       <updateRate>20</updateRate>
       <topicName>sonar_front</topicName>
       <frameName>SonarFront_frame</frameName>
-      <minRange>0.025</minRange>
-      <maxRange>2.55</maxRange>
-      <fov>0.5235987755982988</fov>
+      <minRange>0.0</minRange>
+      <maxRange>5.0</maxRange>
+      <fov>1.05</fov>
       <radiation>ultrasound</radiation>
     </plugin>
   </sensor>
 </gazebo> 
--->
+
 <!-- Drive controller -->
 <gazebo>
   <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">