updated tf frames and added IMU

wilselby · Apr 15, 2019 · 2a2ba35 · 2a2ba35
1 parent 102fada
commit 2a2ba35
Show file tree

Hide file tree

Showing 4 changed files with 84 additions and 58 deletions.
diff --git a/ouster_description/rviz/example.rviz b/ouster_description/rviz/example.rviz
@@ -6,7 +6,7 @@ Panels:
       Expanded:
         - /TF1/Frames1
       Splitter Ratio: 0.5
-    Tree Height: 565
+    Tree Height: 546
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -15,7 +15,7 @@ Panels:
       - /2D Nav Goal1
       - /Publish Point1
     Name: Tool Properties
-    Splitter Ratio: 0.588679016
+    Splitter Ratio: 0.5886790156364441
   - Class: rviz/Views
     Expanded:
       - /Current View1
@@ -26,6 +26,8 @@ Panels:
     Name: Time
     SyncMode: 0
     SyncSource: OS1-64
+Preferences:
+  PromptSaveOnExit: true
 Visualization Manager:
   Class: ""
   Displays:
@@ -35,7 +37,7 @@ Visualization Manager:
       Color: 160; 160; 164
       Enabled: true
       Line Style:
-        Line Width: 0.0299999993
+        Line Width: 0.029999999329447746
         Value: Lines
       Name: Grid
       Normal Cell Count: 0
@@ -56,7 +58,11 @@ Visualization Manager:
           Value: true
         base_link:
           Value: true
-        ouster:
+        os1_imu:
+          Value: true
+        os1_lidar:
+          Value: true
+        os1_sensor:
           Value: true
       Marker Scale: 1
       Name: TF
@@ -66,8 +72,11 @@ Visualization Manager:
       Tree:
         base_footprint:
           base_link:
-            ouster:
-              {}
+            os1_sensor:
+              os1_imu:
+                {}
+              os1_lidar:
+                {}
       Update Interval: 0
       Value: true
     - Alpha: 1
@@ -89,7 +98,15 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
-        ouster:
+        os1_imu:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        os1_lidar:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        os1_sensor:
           Alpha: 1
           Show Axes: false
           Show Trail: false
@@ -115,17 +132,17 @@ Visualization Manager:
       Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 63
+      Max Intensity: -999999
       Min Color: 0; 0; 0
-      Min Intensity: 0
+      Min Intensity: 999999
       Name: OS1-64
       Position Transformer: XYZ
       Queue Size: 10
       Selectable: true
       Size (Pixels): 3
-      Size (m): 0.00999999978
+      Size (m): 0.009999999776482582
       Style: Flat Squares
-      Topic: /points
+      Topic: /os1_cloud_node/points
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
@@ -155,33 +172,33 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/XYOrbit
-      Distance: 4.27314997
+      Distance: 4.091195106506348
       Enable Stereo Rendering:
-        Stereo Eye Separation: 0.0599999987
+        Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -0.346589327
-        Y: 0.821905255
+        X: -0.3465893268585205
+        Y: 0.821905255317688
         Z: 0
       Focal Shape Fixed Size: false
-      Focal Shape Size: 0.0500000007
+      Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
-      Near Clip Distance: 0.00999999978
-      Pitch: 0.440399081
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.44039908051490784
       Target Frame: <Fixed Frame>
       Value: XYOrbit (rviz)
-      Yaw: 5.63362122
+      Yaw: 5.6336212158203125
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 846
+  Height: 843
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd00000004000000000000016a000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000030000fffffffb0000000800540069006d0065010000000000000450000000000000000000000340000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000002adfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002ad000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000340000002ad00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -192,4 +209,4 @@ Window Geometry:
     collapsed: true
   Width: 1200
   X: 400
-  Y: 26
+  Y: 27
diff --git a/ouster_description/urdf/OS1-64.urdf.xacro b/ouster_description/urdf/OS1-64.urdf.xacro
@@ -1,9 +1,9 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="OS1-64">
   <xacro:property name="M_PI" value="3.1415926535897931" />
-  <xacro:macro name="OS1-64" params="*origin parent:=base_link name:=os1 topic:=/os1_cloud_node/points hz:=10 lasers:=64 samples:=512 min_range:=0.9 max_range:=75.0 noise:=0.008 min_angle:=-${M_PI} max_angle:=${M_PI}">
+  <xacro:macro name="OS1-64" params="*origin parent:=base_link name:=os1_sensor topic:=/os1_cloud_node/points hz:=10 lasers:=64 samples:=512 min_range:=0.9 max_range:=75.0 noise:=0.008 min_angle:=-${M_PI} max_angle:=${M_PI}">
 
-    <joint name="${name}_base_mount_joint" type="fixed">
+    <joint name="mount_joint" type="fixed">
       <xacro:insert_block name="origin" /> 
       <parent link="${parent}"/>
       <child link="${name}"/>
@@ -19,58 +19,68 @@
       <collision name="base_collision">
          <origin xyz="0 0 0.0365" rpy="0 0 0" />
          <geometry>
- 	   <cylinder radius="0.04" length="0.073"/>
+ 	        <cylinder radius="0.04" length="0.073"/>
          </geometry>
       </collision>
       <visual name="base_visual">
-         <origin xyz="0 0 0" rpy="0 0 1.5707" />
+         <origin xyz="0 0 0.0" rpy="0 0 1.5707" />
          <geometry>
-	    <mesh filename="package://ouster_description/meshes/os1_64.dae" />
+	         <mesh filename="package://ouster_description/meshes/os1_64.dae" /> 
+           <!-- <cylinder length="0.073" radius="0.04" /> -->
          </geometry>
       </visual>
-   </link>
+    </link>
+
+    <link name="os1_imu" />
+
+    <link name="os1_lidar" />
+
+
+    <joint name="imu_link_joint" type="fixed">
+      <parent link="${name}" />
+      <child link="os1_imu" />
+      <origin xyz="0.006253 -0.011775 0.007645" rpy="0 0 0" />
+    </joint>
+    <gazebo reference="os1_imu">
+    </gazebo>
+
+    <joint name="lidar_link_joint" type="fixed">
+      <parent link="${name}" />
+      <child link="os1_lidar" />
+      <origin xyz="0.0 0.0 0.03618" rpy="0 0 3.14159" />
+    </joint>
 
     <!-- Gazebo requires the ouster_gazebo_plugins package -->
     <gazebo reference="${name}">
         <sensor type="ray" name="${name}-OS1-64">
           <pose>0 0 0 0 0 0</pose>
           <visualize>false</visualize>
           <update_rate>${hz}</update_rate>
-          <ray>
-            <scan>
-              <horizontal>
-                <samples>${samples}</samples>
-                <resolution>1</resolution>
-                <min_angle>${min_angle}</min_angle>
-                <max_angle>${max_angle}</max_angle>
-              </horizontal>
-              <vertical>
-                <samples>${lasers}</samples>
-                <resolution>1</resolution>
-                <min_angle>-${15*M_PI/180.0}</min_angle>
-                <max_angle>${15*M_PI/180.0}</max_angle>
-              </vertical>
-            </scan>
-            <range>
-              <min>${min_range}</min>
-              <max>${max_range}</max>
-              <resolution>0.03</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_ouster_laser.so">
             <topicName>${topic}</topicName>
-            <frameName>${name}</frameName>
+            <frameName>os1_lidar</frameName>
             <min_range>${min_range}</min_range>
             <max_range>${max_range}</max_range>
             <gaussianNoise>${noise}</gaussianNoise>
           </plugin>
         </sensor>
     </gazebo>
 
+    <!-- IMU -->
+  <gazebo>
+    <plugin name="imu_controller" filename="libhector_gazebo_ros_imu.so">
+      <robotNamespace>/</robotNamespace>
+      <updateRate>200.0</updateRate> 
+      <bodyName>os1_imu</bodyName>
+      <topicName>/os1_cloud_node/imu</topicName>
+      <accelDrift>0.005 0.005 0.005</accelDrift>
+      <accelGaussianNoise>0.005 0.005 0.005</accelGaussianNoise>
+      <rateDrift>0.005 0.005 0.005 </rateDrift>
+      <rateGaussianNoise>0.005 0.005 0.005 </rateGaussianNoise>
+      <headingDrift>0.005</headingDrift>
+      <headingGaussianNoise>0.005</headingGaussianNoise>
+    </plugin>
+  </gazebo>
+
   </xacro:macro>
 </robot>
diff --git a/ouster_description/urdf/example.urdf.xacro b/ouster_description/urdf/example.urdf.xacro
@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="example">
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="os1_example">
 
   <!-- Base Footprint -->
   <link name="base_footprint" />
@@ -31,7 +31,7 @@
   </link>
 -
   <xacro:include filename="$(find ouster_description)/urdf/OS1-64.urdf.xacro"/>
-  <OS1-64 parent="base_link" name="ouster" topic="/points" hz="10" samples="220">
+  <OS1-64 parent="base_link" name="os1_sensor" hz="10" samples="220">
     <origin xyz="0 0 1.2" rpy="0 0 0" />
   </OS1-64>
 

diff --git a/ouster_description/worlds/os-1-64.world b/ouster_description/worlds/os-1-64.world
@@ -54,7 +54,6 @@
 
           </geometry>
         </visual>
-
       </link>
     </model>
   </world>