Merge pull request #6 from tongtybj/PR/agile_multirotor/gazebo_hokuyo

[Agile Multirotor] add virtual hokuyo 2D LiDAR model
HarukiKozukapenguin · Dec 1, 2022 · f2fc90c · f2fc90c
2 parents d272cf7 + 5e4c821
commit f2fc90c
Show file tree

Hide file tree

Showing 4 changed files with 93 additions and 13 deletions.
diff --git a/aerial_robot_model/package.xml b/aerial_robot_model/package.xml
@@ -54,5 +54,6 @@
   <run_depend>urdf</run_depend>
   <run_depend>visualization_msgs</run_depend>
   <run_depend>xacro</run_depend>
+  <run_depend>hector_sensors_description</run_depend>
 
 </package>
diff --git a/robots/agile_multirotor/config/rviz_config b/robots/agile_multirotor/config/rviz_config
@@ -64,6 +64,8 @@ Visualization Manager:
           Value: true
         multirotor/fc:
           Value: true
+        multirotor/laser_frame:
+          Value: true
         multirotor/main_body:
           Value: true
         multirotor/rgbd_camera_frame:
@@ -95,6 +97,8 @@ Visualization Manager:
             multirotor/main_body:
               multirotor/fc:
                 {}
+              multirotor/laser_frame:
+                {}
               multirotor/rgbd_camera_frame:
                 multirotor/rgbd_camera_optical_frame:
                   {}
@@ -122,6 +126,11 @@ Visualization Manager:
           Alpha: 1
           Show Axes: false
           Show Trail: false
+        laser_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
         main_body:
           Alpha: 1
           Show Axes: false
@@ -184,7 +193,7 @@ Visualization Manager:
       Decay Time: 0
       Depth Map Topic: /multirotor/rgbd/depth/depth_registered
       Depth Map Transport Hint: raw
-      Enabled: true
+      Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Min Color: 0; 0; 0
@@ -200,6 +209,34 @@ Visualization Manager:
       Topic Filter: true
       Use Fixed Frame: true
       Use rainbow: true
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic: /multirotor/scan
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
       Value: true
   Enabled: true
   Global Options:
@@ -229,41 +266,41 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 14.51597785949707
+      Distance: 5.93232536315918
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0.7939224243164062
-        Y: 0.1640194207429886
-        Z: 0.2843625247478485
+        X: 0.9098455309867859
+        Y: 0.18769778311252594
+        Z: 0.37073153257369995
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.3047972321510315
+      Pitch: 0.3147972822189331
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 3.4604008197784424
+      Yaw: 3.5554003715515137
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
   Height: 2105
   Hide Left Dock: false
-  Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000016a0000079bfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000079b000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000079bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000079b000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000ebd0000003efc0100000002fb0000000800540069006d0065010000000000000ebd000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000c380000079b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a0000079bfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000079b000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000079bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000079b000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000075f0000003efc0100000002fb0000000800540069006d006501000000000000075f000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005ef0000079b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
     collapsed: false
   Tool Properties:
     collapsed: false
   Views:
-    collapsed: false
-  Width: 3773
+    collapsed: true
+  Width: 1887
   X: 67
   Y: 27
diff --git a/robots/agile_multirotor/urdf/quadrotor.gazebo.xacro b/robots/agile_multirotor/urdf/quadrotor.gazebo.xacro
@@ -107,4 +107,41 @@
     </sensor>
   </gazebo>
 
+  <!-- hokuyo -->
+  <xacro:property name="ray_count" default="1081" />
+  <xacro:property name="max_angle" default="135" />
+  <gazebo reference="laser_frame">
+    <sensor type="ray" name="laser">
+      <always_on>true</always_on>
+      <update_rate>40</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>${-max_angle * M_PI/180}</min_angle>
+            <max_angle>${max_angle * M_PI/180}</max_angle>
+          </horizontal>
+        </scan>
+        <range>
+          <min>0.1</min>
+          <max>20.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_laser_controller" filename="libgazebo_ros_laser.so">
+        <robotNamespace>$(arg robot_name)</robotNamespace>
+        <topicName>scan</topicName>
+        <frameName>laser_frame</frameName>
+      </plugin>
+    </sensor>
+  </gazebo>
+
 </robot>
diff --git a/robots/agile_multirotor/urdf/quadrotor.urdf.xacro b/robots/agile_multirotor/urdf/quadrotor.urdf.xacro
@@ -132,9 +132,9 @@
     </visual>
     <!-- collision is not sphere to be able to  -->
     <collision>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <origin rpy="0 0 0" xyz="0 0 -0.05"/>
       <geometry>
-        <box size="0.3525 0.26 0.092" />
+        <box size="0.3525 0.26 0.04" />
       </geometry>
     </collision>
   </link>
@@ -158,5 +158,10 @@
   <xacro:propeller_module id = "3" x = "${pos_bound/2}" y = "${pos_bound/2}" z = "${rotor_z}"  pich = "${opt_pich_3}" yaw = "${opt_yaw_3}" direction = "1" />
   <xacro:propeller_module id = "4" x = "${-pos_bound/2}" y = "${pos_bound/2}" z = "${rotor_z}"  pich = "${opt_pich_4}" yaw = "${opt_yaw_4}"  direction = "-1" />
 
+  <!-- hokuyo 2D LiDAR -->
+  <xacro:include filename="$(find hector_sensors_description)/urdf/hokuyo_utm30lx.urdf.xacro" />
+  <xacro:hokuyo_utm30lx_model name="laser" parent="main_body">
+    <origin xyz="0.0 0.0 0.06" rpy="0 0 0"/>
+  </xacro:hokuyo_utm30lx_model>
 
 </robot>