From dbc3bd8fab8a79c78516a7d96e8512368beae87a Mon Sep 17 00:00:00 2001
From: Jack <projectbyjx@gmail.com>
Date: Mon, 3 Oct 2022 19:27:28 -0400
Subject: [PATCH] [Local Changes]

---
 urdf/all_sensors.urdf.xacro |   3 +
 urdf/vlp16.urdf(bkup).xacro | 155 +++++++++++++++++++++++++++++++
 urdf/vlp16.urdf.xacro       | 177 ++++++++++++++++++++++++++++++++++++
 3 files changed, 335 insertions(+)
 create mode 100644 urdf/vlp16.urdf(bkup).xacro
 create mode 100644 urdf/vlp16.urdf.xacro

diff --git a/urdf/all_sensors.urdf.xacro b/urdf/all_sensors.urdf.xacro
index bbad442..d6e7a1c 100644
--- a/urdf/all_sensors.urdf.xacro
+++ b/urdf/all_sensors.urdf.xacro
@@ -77,5 +77,8 @@
 
   <!-- Pointgrey Bumblebee -->
   <xacro:include filename="$(find robotnik_sensors)/urdf/pointgrey_bumblebee2.urdf.xacro" /> 
+
+  <!-- velodyne VLP16 -->
+  <xacro:include filename="$(find robotnik_sensors)/urdf/vlp16.urdf.xacro" />
   
 </robot>
diff --git a/urdf/vlp16.urdf(bkup).xacro b/urdf/vlp16.urdf(bkup).xacro
new file mode 100644
index 0000000..5f776c5
--- /dev/null
+++ b/urdf/vlp16.urdf(bkup).xacro
@@ -0,0 +1,155 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:property name="M_PI" value="3.1415926535897931" />
+
+    <xacro:macro name="vlp16_model" params="name parent *origin">
+      <joint name="${name}_joint" type="fixed">
+        <xacro:insert_block name="origin" />
+        <parent link="${parent}"/>
+        <child link="${name}_frame"/>
+      </joint>
+
+      <link name="${name}_frame">
+        <xacro:inertial_sphere mass="0.8" diameter="0.1033" />
+        <visual>        
+          <origin rpy="0 0 0" xyz="0 0 0.0018500"/>
+          <geometry>
+            <cylinder radius="0.0516" length="0.0717"/>
+          </geometry>
+        </visual>
+        <collision>
+          <origin rpy="0 0 0" xyz="0 0 0.0018500"/>
+          <geometry>
+            <cylinder radius="0.0516" length="0.0717"/>
+          </geometry>
+        </collision>
+      </link>
+    </xacro:macro>
+    
+    <xacro:macro name="vlp16" params="name parent *origin ros_topic">
+      <xacro:vlp16_advanced_parameters name="${name}" parent="${parent}" ros_topic="${ros_topic}" update_rate="10" horizontal_samples="512" vertical_samples="8" min_range="0.45" max_range="100">
+        <xacro:insert_block name="origin" />
+      </xacro:vlp16_advanced_parameters>    
+    </xacro:macro>
+
+    <xacro:macro name="vlp16_advanced_parameters" params="name parent *origin ros_topic update_rate horizontal_samples vertical_samples min_range max_range">
+        
+      <xacro:vlp16_model name="${name}" parent="${parent}">
+        <xacro:insert_block name="origin" />
+      </xacro:vlp16_model>
+
+      <gazebo reference="${name}_frame">
+<!--        <sensor type="ray" name="${name}">
+          <always_on>true</always_on>
+          <update_rate>${update_rate}</update_rate>
+          <pose>0 0 0 0 0 0</pose>
+          <visualize>false</visualize>
+          <ray>
+            <scan>
+              <horizontal>
+                <samples>${ray_count}</samples>
+                <resolution>1</resolution>
+                <min_angle>${min_angle * M_PI/180}</min_angle>
+                <max_angle>${max_angle * M_PI/180}</max_angle>
+              </horizontal>
+            </scan>
+            <range>
+              <min>0.2</min>
+              <max>30.0</max>
+              <resolution>0.01</resolution>
+            </range>
+            <noise>
+              <type>gaussian</type>
+              <mean>0.0</mean>
+              <stddev>0.004</stddev>
+            </noise>
+          </ray>
+          <plugin name="gazebo_ros_${name}_controller" filename="libgazebo_ros_laser.so">
+            <topicName>${ros_topic}</topicName>
+            <frameName>${name}_frame</frameName>
+          </plugin>
+        </sensor>-->
+        
+        
+        
+      <sensor type="ray" name="${name}-VLP16">
+        <pose>0 0 0 0 0 0</pose>
+        <visualize>false</visualize>
+        <update_rate>${update_rate}</update_rate>
+        <ray>
+          <scan>
+            <horizontal>
+              <samples>${horizontal_samples}</samples>
+              <resolution>1</resolution>
+              <min_angle>-${M_PI}</min_angle>
+              <max_angle> ${M_PI}</max_angle>
+            </horizontal>
+            <vertical>
+              <samples>${vertical_samples}</samples>
+              <resolution>1</resolution>
+              <min_angle>-${15.0*M_PI/180.0}</min_angle>
+              <max_angle> ${15.0*M_PI/180.0}</max_angle>
+            </vertical>
+          </scan>
+          <range>
+            <min>${min_range}</min>
+            <max>${max_range}</max>
+            <resolution>0.001</resolution>
+          </range>
+          <noise>
+            <type>gaussian</type>
+            <mean>0.0</mean>
+            <stddev>0.0</stddev>
+          </noise>
+        </ray>
+        <plugin name="gazebo_ros_laser_controller" filename="libgazebo_ros_velodyne_laser.so">
+          <topicName>${ros_topic}</topicName>
+          <frameName>${name}_frame</frameName>
+          <min_range>${min_range}</min_range>
+          <max_range>${max_range}</max_range>
+          <gaussianNoise>0.008</gaussianNoise>
+        </plugin>
+      </sensor>
+      </gazebo>
+    </xacro:macro>
+  
+    <xacro:macro name="vlp16_gpu" params="name parent *origin ros_topic update_rate ray_count min_angle max_angle">
+      <xacro:vlp16_model name="${name}" parent="${parent}">
+        <xacro:insert_block name="origin" />
+      </xacro:vlp16_model>
+
+      <gazebo reference="${name}_frame">
+        <sensor type="gpu_ray" name="${name}">          
+          <update_rate>${update_rate}</update_rate>
+          <pose>0 0 0 0 0 0</pose>
+          <visualize>false</visualize>
+          <ray>
+            <scan>
+              <horizontal>
+                <samples>${ray_count}</samples>
+                <resolution>1</resolution>
+                <min_angle>${min_angle * M_PI/180}</min_angle>
+                <max_angle>${max_angle * M_PI/180}</max_angle>
+              </horizontal>
+            </scan>
+            <range>
+              <min>0.2</min>
+              <max>30.0</max>
+              <resolution>0.01</resolution>
+            </range>
+            <noise>
+              <type>gaussian</type>
+              <mean>0.0</mean>
+              <stddev>0.004</stddev>
+            </noise>
+          </ray>
+          <plugin name="gazebo_ros_${name}_controller" filename="libgazebo_ros_gpu_laser.so">
+            <topicName>${ros_topic}</topicName>
+            <frameName>${name}_frame</frameName>
+          </plugin>
+        </sensor>
+      </gazebo>
+    </xacro:macro>
+</robot>
+  
diff --git a/urdf/vlp16.urdf.xacro b/urdf/vlp16.urdf.xacro
new file mode 100644
index 0000000..756779e
--- /dev/null
+++ b/urdf/vlp16.urdf.xacro
@@ -0,0 +1,177 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:property name="M_PI" value="3.1415926535897931" />
+
+    <!--xacro:macro name="vlp16_model" params="name parent *origin">
+      <joint name="${name}_joint" type="fixed"-->
+    <xacro:macro name="sensor_vlp16" params="prefix parent prefix_topic:='front_laser' *origin">
+
+    <link name="${prefix}_link">
+	</link>
+      <joint name="${prefix}_joint" type="fixed">
+        <parent link="${parent}"/>
+        <child link="${prefix}_link"/>
+        <xacro:insert_block name="origin" />
+	</joint>
+
+    <joint name="${prefix}_base_joint" type="fixed">
+      <!--origin xyz="0.00 0 -0.05" rpy="0 0 0"/-->
+      <origin xyz="0 0 0.004" rpy="0 0 0"/>
+      <parent link="${prefix}_link"/>
+      <child link="${prefix}_base_link"/>
+     
+    </joint>
+
+    <link name="${prefix}_base_link">
+        <!--xacro:inertial_sphere mass="0.8" diameter="0.1033" /-->
+
+        <collision>
+          <origin rpy="0 0 0" xyz="0 0 0.0018500"/>
+          <geometry>
+            <cylinder radius="0.0516" length="0.0717"/>
+          </geometry>
+        </collision>
+
+        <visual>       
+          <origin rpy="0 0 0" xyz="0 0 0.0018500"/>
+          <geometry>
+            <cylinder radius="0.0516" length="0.0717"/>
+          </geometry>
+        </visual>
+
+      <inertial>
+        <mass value="0.8" />
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <inertia ixx="0.08" ixy="0" ixz="0" iyy="0.08" iyz="0" izz="0.08" />
+      </inertial>
+
+      </link>
+    </xacro:macro>
+    
+    <xacro:macro name="vlp16" params="name parent *origin ros_topic">
+      <xacro:vlp16_advanced_parameters name="${name}" parent="${parent}" ros_topic="${ros_topic}" update_rate="10" horizontal_samples="512" vertical_samples="8" min_range="0.45" max_range="100">
+        <xacro:insert_block name="origin" />
+      </xacro:vlp16_advanced_parameters>    
+    </xacro:macro>
+
+    <xacro:macro name="vlp16_advanced_parameters" params="name parent *origin ros_topic update_rate horizontal_samples vertical_samples min_range max_range">
+        
+      <xacro:vlp16_model name="${name}" parent="${parent}">
+        <xacro:insert_block name="origin" />
+      </xacro:vlp16_model>
+
+      <gazebo reference="${name}_frame">
+<!--        <sensor type="ray" name="${name}">
+          <always_on>true</always_on>
+          <update_rate>${update_rate}</update_rate>
+          <pose>0 0 0 0 0 0</pose>
+          <visualize>false</visualize>
+          <ray>
+            <scan>
+              <horizontal>
+                <samples>${ray_count}</samples>
+                <resolution>1</resolution>
+                <min_angle>${min_angle * M_PI/180}</min_angle>
+                <max_angle>${max_angle * M_PI/180}</max_angle>
+              </horizontal>
+            </scan>
+            <range>
+              <min>0.2</min>
+              <max>30.0</max>
+              <resolution>0.01</resolution>
+            </range>
+            <noise>
+              <type>gaussian</type>
+              <mean>0.0</mean>
+              <stddev>0.004</stddev>
+            </noise>
+          </ray>
+          <plugin name="gazebo_ros_${name}_controller" filename="libgazebo_ros_laser.so">
+            <topicName>${ros_topic}</topicName>
+            <frameName>${name}_frame</frameName>
+          </plugin>
+        </sensor>-->
+        
+        
+        
+      <sensor type="ray" name="${name}-VLP16">
+        <pose>0 0 0 0 0 0</pose>
+        <visualize>false</visualize>
+        <update_rate>${update_rate}</update_rate>
+        <ray>
+          <scan>
+            <horizontal>
+              <samples>${horizontal_samples}</samples>
+              <resolution>1</resolution>
+              <min_angle>-${M_PI}</min_angle>
+              <max_angle> ${M_PI}</max_angle>
+            </horizontal>
+            <vertical>
+              <samples>${vertical_samples}</samples>
+              <resolution>1</resolution>
+              <min_angle>-${15.0*M_PI/180.0}</min_angle>
+              <max_angle> ${15.0*M_PI/180.0}</max_angle>
+            </vertical>
+          </scan>
+          <range>
+            <min>${min_range}</min>
+            <max>${max_range}</max>
+            <resolution>0.001</resolution>
+          </range>
+          <noise>
+            <type>gaussian</type>
+            <mean>0.0</mean>
+            <stddev>0.0</stddev>
+          </noise>
+        </ray>
+        <plugin name="gazebo_ros_laser_controller" filename="libgazebo_ros_velodyne_laser.so">
+          <topicName>${ros_topic}</topicName>
+          <frameName>${name}_frame</frameName>
+          <min_range>${min_range}</min_range>
+          <max_range>${max_range}</max_range>
+          <gaussianNoise>0.008</gaussianNoise>
+        </plugin>
+      </sensor>
+      </gazebo>
+    </xacro:macro>
+  
+    <xacro:macro name="vlp16_gpu" params="name parent *origin ros_topic update_rate ray_count min_angle max_angle">
+      <xacro:vlp16_model name="${name}" parent="${parent}">
+        <xacro:insert_block name="origin" />
+      </xacro:vlp16_model>
+
+      <gazebo reference="${name}_frame">
+        <sensor type="gpu_ray" name="${name}">          
+          <update_rate>${update_rate}</update_rate>
+          <pose>0 0 0 0 0 0</pose>
+          <visualize>false</visualize>
+          <ray>
+            <scan>
+              <horizontal>
+                <samples>${ray_count}</samples>
+                <resolution>1</resolution>
+                <min_angle>${min_angle * M_PI/180}</min_angle>
+                <max_angle>${max_angle * M_PI/180}</max_angle>
+              </horizontal>
+            </scan>
+            <range>
+              <min>0.2</min>
+              <max>30.0</max>
+              <resolution>0.01</resolution>
+            </range>
+            <noise>
+              <type>gaussian</type>
+              <mean>0.0</mean>
+              <stddev>0.004</stddev>
+            </noise>
+          </ray>
+          <plugin name="gazebo_ros_${name}_controller" filename="libgazebo_ros_gpu_laser.so">
+            <topicName>${ros_topic}</topicName>
+            <frameName>${name}_frame</frameName>
+          </plugin>
+        </sensor>
+      </gazebo>
+    </xacro:macro>
+</robot>
+