From c3572bf8f5685d049e4a756d5672fcaf53d6b1d3 Mon Sep 17 00:00:00 2001
From: Emiliano Borghi <eborghiorue@frba.utn.edu.ar>
Date: Sun, 12 Apr 2020 15:47:53 -0300
Subject: [PATCH] WIP

---
 .../launch/create_description.launch          |  4 +++-
 ca_description/urdf/common_properties.xacro   |  1 +
 .../urdf/sensors/lasers/rplidar.xacro         | 22 +++++++++++++------
 3 files changed, 19 insertions(+), 8 deletions(-)

diff --git a/ca_description/launch/create_description.launch b/ca_description/launch/create_description.launch
index 44a3f971..1a2346a1 100644
--- a/ca_description/launch/create_description.launch
+++ b/ca_description/launch/create_description.launch
@@ -7,6 +7,7 @@
   <arg name="model" value="$(find ca_description)/urdf/create_2.xacro" doc="Path to Xacro description of the robot"/>
 
   <arg name="visualize" default="$(optenv VISUALIZE false)" doc="Flag to visualize rays in Gazebo"/>
+  <arg name="use_gpu"   default="$(optenv GPU false)"       doc="Use GPU for laser processing"/>
 
   <!-- Stacks:
     * turtlebot
@@ -28,7 +29,8 @@
   <arg name="xacro_args" value="visualize:=$(arg visualize)
                                 robot_name:=$(arg ns)
                                 stack:=$(arg stack)
-                                laser:=$(arg laser)"/>
+                                laser:=$(arg laser)
+                                use_gpu:=$(arg use_gpu)"/>
   <!-- TODO (@eborghi10): Use only 1 robot_description for multiple robots -->
   <param name="robot_description" command="$(find xacro)/xacro $(arg model) $(arg xacro_args)"/>
   <!-- Robot state publisher-->
diff --git a/ca_description/urdf/common_properties.xacro b/ca_description/urdf/common_properties.xacro
index 8b39f28f..81d65d98 100644
--- a/ca_description/urdf/common_properties.xacro
+++ b/ca_description/urdf/common_properties.xacro
@@ -66,5 +66,6 @@
   </xacro:macro>
 
   <xacro:property name="cm_to_meters" value="${1./100.}"/>
+  <xacro:property name="mm_to_meters" value="${1./1000.}"/>
 
 </robot>
diff --git a/ca_description/urdf/sensors/lasers/rplidar.xacro b/ca_description/urdf/sensors/lasers/rplidar.xacro
index 51a0f71a..e23fb0e8 100644
--- a/ca_description/urdf/sensors/lasers/rplidar.xacro
+++ b/ca_description/urdf/sensors/lasers/rplidar.xacro
@@ -9,15 +9,23 @@
 <xacro:property name="laser_link"    value="laser_link"/>
 
 <xacro:property name="rate_hz"       value="10"/>
-<xacro:property name="min_range_m"   value="0.15"/>
+<xacro:property name="min_range_cm"  value="15"/>
 <xacro:property name="max_range_m"   value="12.0"/>
-<xacro:property name="samples"       value="720"/>
+<xacro:property name="samples"       value="1000"/>
 <xacro:property name="resolution_mm" value="0.5"/>
 <xacro:property name="min_angle_rad" value="${-pi}"/>
 <xacro:property name="max_angle_rad" value="${pi}"/>
 
 <xacro:property name="topic_name"    value="${name}/scan"/>
 
+<!-- Set GPU values -->
+<xacro:if value="$(arg use_gpu)">
+  <xacro:property name="gpu" value="gpu_"/>
+</xacro:if>
+<xacro:unless value="$(arg use_gpu)">
+  <xacro:property name="gpu" value=""/>
+</xacro:unless>
+
 <joint name="${name}_joint" type="fixed">
   <origin xyz="${laser_origin}"/>
   <parent link="${parent_link}"/>
@@ -45,22 +53,22 @@
 </gazebo>
 
 <gazebo reference="${laser_link}">
-  <sensor type="ray" name="${name}_sensor">
+  <sensor type="${gpu}ray" name="${name}_sensor">
     <visualize>$(arg visualize)</visualize>
     <update_rate>${rate_hz}</update_rate>
     <ray>
       <scan>
         <horizontal>
           <samples>${samples}</samples>
-          <resolution>1</resolution>
+          <resolution>10</resolution>
           <min_angle>${min_angle_rad}</min_angle>
           <max_angle>${max_angle_rad}</max_angle>
         </horizontal>
       </scan>
       <range>
-        <min>${min_range_m}</min>
+        <min>${min_range_cm*cm_to_meters}</min>
         <max>${max_range_m}</max>
-        <resolution>${resolution_mm / 1000.0}</resolution>
+        <resolution>${resolution_mm*mm_to_meters}</resolution>
       </range>
       <noise>
         <type>gaussian</type>
@@ -68,7 +76,7 @@
         <stddev>0.01</stddev>
       </noise>
     </ray>
-    <plugin name="gazebo_ros_${name}" filename="libgazebo_ros_laser.so">
+    <plugin name="gazebo_ros_${name}" filename="libgazebo_ros_${gpu}laser.so">
       <topicName>${topic_name}</topicName>
       <frameName>${laser_link}</frameName>
     </plugin>