From ac3bf5dabcd9c6d32b836c617bb140f1aace48ed Mon Sep 17 00:00:00 2001
From: Rafael Martin <rmartin@robotnik.es>
Date: Wed, 15 May 2024 16:46:47 +0200
Subject: [PATCH 1/2] fix tf z offset bpearl

---
 urdf/rs_bpearl.urdf.xacro | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/urdf/rs_bpearl.urdf.xacro b/urdf/rs_bpearl.urdf.xacro
index 9a05ad2..dcfd32e 100644
--- a/urdf/rs_bpearl.urdf.xacro
+++ b/urdf/rs_bpearl.urdf.xacro
@@ -44,7 +44,8 @@
     <joint name="${prefix}_joint" type="fixed">
       <parent link="${prefix}_base_link"/>
       <child link="${prefix}_link" />
-      <origin xyz="0 0 0.09427" rpy="0 0 0"/>
+      <!-- <origin xyz="0 0 0.09427" rpy="0 0 0"/> -->
+      <origin xyz="0 0 0" rpy="0 0 0"/>
     </joint>
     <link name="${prefix}_link"/>
 

From b861402f16cfd36af41bebb6be647d55073aee89 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Rafael=20Mart=C3=ADn?= <theverbat@gmail.com>
Date: Sun, 16 Jun 2024 17:21:06 +0200
Subject: [PATCH 2/2] revert previous commit and clean

---
 urdf/rs_bpearl.urdf.xacro | 170 +++++++++++++++++++++-----------------
 1 file changed, 92 insertions(+), 78 deletions(-)

diff --git a/urdf/rs_bpearl.urdf.xacro b/urdf/rs_bpearl.urdf.xacro
index dcfd32e..b83d8f6 100644
--- a/urdf/rs_bpearl.urdf.xacro
+++ b/urdf/rs_bpearl.urdf.xacro
@@ -1,39 +1,50 @@
 <?xml version="1.0"?>
-<robot name="sensor_rs_bpearl" xmlns:xacro="http://wiki.ros.org/xacro">
-
-  <xacro:property name="M_PI" value="3.1415926535897931" />
-
-  <xacro:macro name="sensor_rs_bpearl" params="prefix parent prefix_topic suffix_topic:='' *origin range_min range_max hfov samples vfov lasers fps gpu:=true include_inertial:=^|true">
-
-
+<robot
+  name="sensor_rs_bpearl"
+  xmlns:xacro="http://wiki.ros.org/xacro"
+>
+  <xacro:macro
+    name="sensor_rs_bpearl"
+    params="
+      prefix
+      parent
+      *origin
+      prefix_topic
+      suffix_topic:=''
+      range_min
+      range_max
+      hfov
+      samples
+      vfov
+      lasers
+      fps
+      gpu:=true
+      include_inertial:=^|true
+    "
+  >
     <joint name="${prefix}_base_joint" type="fixed">
       <xacro:insert_block name="origin" />
-      <parent link="${parent}"/>
+      <parent link="${parent}" />
       <child link="${prefix}_base_link" />
     </joint>
-
     <link name="${prefix}_base_link">
       <visual>
-      <!-- origin xyz="0 0 0" rpy="0 0 1.5708"/ -->
-      <origin xyz="0 0 0" rpy="0 0 1.5708"/>
+        <origin xyz="0 0 0" rpy="0 0 1.5708" />
         <geometry>
           <mesh filename="package://robotnik_sensors/meshes/rs_bpearl.stl" />
         </geometry>
-        
         <material name="blackgray_color">
-          <color rgba="0.25 0.25 0.25 1"/>     
+          <color rgba="0.25 0.25 0.25 1" />
         </material>
-
       </visual>
       <collision>
-        <origin xyz="0 0 0" rpy="0 0 1.5708"/>
+        <origin xyz="0 0 0" rpy="0 0 1.5708" />
         <geometry>
           <mesh filename="package://robotnik_sensors/meshes/rs_bpearl.stl" />
         </geometry>
       </collision>
       <xacro:if value="${include_inertial}">
         <inertial>
-          <!-- The following are not reliable values, and should not be used for modeling -->
           <mass value="0.92" />
           <origin xyz="0 0 0.055" />
           <xacro:solid_cuboid_inertia m="0.92" w="0.111" h="0.111" d="0.11" />
@@ -42,43 +53,49 @@
     </link>
 
     <joint name="${prefix}_joint" type="fixed">
-      <parent link="${prefix}_base_link"/>
+      <parent link="${prefix}_base_link" />
       <child link="${prefix}_link" />
-      <!-- <origin xyz="0 0 0.09427" rpy="0 0 0"/> -->
-      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <origin xyz="0 0 0.09427" rpy="0 0 0" />
     </joint>
-    <link name="${prefix}_link"/>
-
-  <xacro:sensor_rs_bpearl_gazebo range_min="${range_min}" range_max="${range_max}" hfov="${hfov}" samples="${samples}" vfov="${vfov}" lasers="${lasers}" fps="${fps}" gpu="${gpu}"/>
+    <link name="${prefix}_link" />
 
+    <xacro:sensor_rs_bpearl_gazebo
+      range_min="${range_min}"
+      range_max="${range_max}"
+      hfov="${hfov}"
+      samples="${samples}"
+      vfov="${vfov}"
+      lasers="${lasers}"
+      fps="${fps}"
+      gpu="${gpu}"
+    />
   </xacro:macro>
 
-  <xacro:macro name="sensor_rs_bpearl_gazebo" params="range_min range_max hfov samples vfov lasers fps gpu">
-
+  <xacro:macro
+    name="sensor_rs_bpearl_gazebo"
+    params="range_min range_max hfov samples vfov lasers fps gpu"
+  >
     <gazebo reference="${prefix}_base_link">
-      <visual>  
-        <material>  
-          <ambient>0.5 0.5 0.5 1.0</ambient>  
-          <diffuse>0.250754 0.250754 0.250754 1.0</diffuse>  
-          <specular>0.2508273 0.2508273 0.2508273 1.0</specular>  
-          <emissive>0.0 0.0 0.0 0.0</emissive>  
-        </material>  
-      </visual>  
+      <visual>
+        <material>
+          <ambient>0.5 0.5 0.5 1.0</ambient>
+          <diffuse>0.250754 0.250754 0.250754 1.0</diffuse>
+          <specular>0.2508273 0.2508273 0.2508273 1.0</specular>
+          <emissive>0.0 0.0 0.0 0.0</emissive>
+        </material>
+      </visual>
     </gazebo>
-
     <gazebo reference="${prefix}_link">
-
-      <visual>  
-        <material>  
-          <ambient>1.2 0.2 0.2 1.0</ambient>  
-          <diffuse>0.250754 0.250754 0.250754 1.0</diffuse>  
-          <specular>0.2508273 0.2508273 0.2508273 1.0</specular>  
-          <emissive>0.0 0.0 0.0 0.0</emissive>  
-        </material>  
-      </visual>  
+      <visual>
+        <material>
+          <ambient>1.2 0.2 0.2 1.0</ambient>
+          <diffuse>0.250754 0.250754 0.250754 1.0</diffuse>
+          <specular>0.2508273 0.2508273 0.2508273 1.0</specular>
+          <emissive>0.0 0.0 0.0 0.0</emissive>
+        </material>
+      </visual>
 
       <xacro:if value="${gpu}">
-        <!-- Using GPU needs: https://github.com/RobotnikAutomation/velodyne_simulator -->
         <xacro:property name="ray_type" value="gpu_ray" />
         <xacro:property name="plugin_lib" value="libgazebo_ros_velodyne_gpu_laser.so" />
       </xacro:if>
@@ -86,41 +103,38 @@
         <xacro:property name="ray_type" value="ray" />
         <xacro:property name="plugin_lib" value="libgazebo_ros_velodyne_laser.so" />
       </xacro:unless>
-        <sensor type="${ray_type}" name="${prefix}_sensor">
-          <pose>0 0 0 0 0 0.0</pose>
-          <visualize>false</visualize>
-          <update_rate>${fps}</update_rate>
-          <ray>
-            <scan>
-              <horizontal>
-				<!-- for a high quality simulation -->
-                <samples>${samples}</samples>
-                <min_angle>-${hfov/2*M_PI/180.0}</min_angle>
-                <max_angle>${hfov/2*M_PI/180.0}</max_angle>
-              </horizontal>
-              <vertical>
-                <samples>${lasers}</samples>
-                <min_angle>-${5*M_PI/180.0}</min_angle>  <!-- bpearl has 90 degrees of vertical field of view-->
-                <max_angle> ${(vfov-10)*M_PI/180.0}</max_angle>
-              </vertical>
-            </scan>
-            <range>
-              <min>${range_min}</min>
-              <max>${range_max}</max>
-              <resolution>0.01</resolution>
-            </range>
-            <noise>
-              <type>none</type>
-            </noise>
-          </ray>
-          <plugin name="${prefix}_controller" filename="${plugin_lib}">
-            <topicName>${prefix_topic}/points${suffix_topic}</topicName>
-            <frameName>/${prefix}_link</frameName>
-            <gaussianNoise>0.008</gaussianNoise>
-          </plugin>
-        </sensor>
+      <sensor type="${ray_type}" name="${prefix}_sensor">
+        <pose>0 0 0 0 0 0</pose>
+        <visualize>false</visualize>
+        <update_rate>${fps}</update_rate>
+        <ray>
+          <scan>
+            <horizontal>
+              <samples>${samples}</samples>
+              <min_angle>-${radians(hfov/2)}</min_angle>
+              <max_angle>${radians(hfov/2)}</max_angle>
+            </horizontal>
+            <vertical>
+              <samples>${lasers}</samples>
+              <min_angle>${radians(0.5)}</min_angle>
+              <max_angle>${radians(vfov-5)}</max_angle>
+            </vertical>
+          </scan>
+          <range>
+            <min>${range_min}</min>
+            <max>${range_max}</max>
+            <resolution>0.01</resolution>
+          </range>
+          <noise>
+            <type>none</type>
+          </noise>
+        </ray>
+        <plugin name="${prefix}_controller" filename="${plugin_lib}">
+          <topicName>${prefix_topic}/points${suffix_topic}</topicName>
+          <frameName>/${prefix}_link</frameName>
+          <gaussianNoise>0.008</gaussianNoise>
+        </plugin>
+      </sensor>
     </gazebo>
-
   </xacro:macro>
-  
 </robot>