add real imu + lidar mapping

clubcapra · Jun 9, 2024 · 404d289 · 404d289
1 parent 2ab3ade
commit 404d289
Show file tree

Hide file tree

Showing 11 changed files with 67 additions and 275 deletions.
diff --git a/src/rove_bringup/config/VLP16_velodyne_params.yaml b/src/rove_bringup/config/VLP16_velodyne_params.yaml
@@ -8,7 +8,7 @@ velodyne_driver_node:
         read_once: false
         read_fast: false
         repeat_delay: 0.0
-        frame_id: velodyne
+        frame_id: velodyne_laser
         model: VLP16
         rpm: 600.0
         port: 2368
@@ -25,6 +25,6 @@ velodyne_transform_node:
         min_range: 0.9
         max_range: 130.0
         view_direction: 0.0
-        fixed_frame: ""
-        target_frame: ""
+        fixed_frame: "base_link"
+        target_frame: "velodyne_laser"
         organize_cloud: true
diff --git a/src/rove_bringup/config/vn_300.yaml b/src/rove_bringup/config/vn_300.yaml
@@ -94,7 +94,7 @@ vectornav:
       attitudeField: 0x0000          # ATTITUDEGROUP_NONE
       insField: 0x0000               # INSGROUP_NONE
       gps2Field: 0x0000              # GPSGROUP_NONE
-    frame_id: "vectornav"
+    frame_id: "sensor_link"
 
 vn_sensor_msgs:
   ros__parameters:

diff --git a/src/rove_bringup/launch/real.launch.py b/src/rove_bringup/launch/real.launch.py
@@ -107,11 +107,24 @@ def create_controller_node(node_name:str):
     #     )
     # )
 
+    ###### Sensor ######
+    vectornav = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_rove_bringup, "launch", "vectornav.launch.py"),
+        ),
+    )
+
+    velodyne = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_rove_bringup, "launch", "velodyne.launch.py"),
+        ),
+    )
+
     return LaunchDescription([
             control_node,
             common,
             joint_state_broadcaster_spawner,
             *delayed_controller_nodes,
-            # vectornav,
-            # velodyne,
+            vectornav,
+            velodyne,
             ])
diff --git a/src/rove_description/config/basic.rviz b/src/rove_description/config/basic.rviz
@@ -3,8 +3,7 @@ Panels:
     Help Height: 78
     Name: Displays
     Property Tree Widget:
-      Expanded:
-        - /RobotModel1/Status1
+      Expanded: ~
       Splitter Ratio: 0.5
     Tree Height: 517
   - Class: rviz_common/Selection
@@ -71,60 +70,37 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
-        rove_end_effector:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        rove_link_1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        rove_link_2:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        rove_link_3:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        rove_link_4:
+        sensor_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        rove_link_5:
+        track_fl:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
-        rove_link_6:
+        track_fr:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
-        rove_link_base:
+        track_l:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        sensor_link:
+        track_r:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        track_l:
+        track_rl:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
-        track_r:
+        track_rr:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
         velodyne_laser:
           Alpha: 1
           Show Axes: false
@@ -176,9 +152,9 @@ Visualization Manager:
       Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 0
+      Max Intensity: 125
       Min Color: 0; 0; 0
-      Min Intensity: 0
+      Min Intensity: 1
       Name: LaserScan
       Position Transformer: XYZ
       Selectable: true
@@ -202,40 +178,20 @@ Visualization Manager:
         All Enabled: false
         base_link:
           Value: true
-        flipper_fl:
-          Value: true
-        flipper_fr:
-          Value: true
-        flipper_rl:
-          Value: true
-        flipper_rr:
-          Value: true
-        map:
-          Value: true
-        odom:
-          Value: true
-        rove_end_effector:
-          Value: true
-        rove_link_1:
-          Value: true
-        rove_link_2:
-          Value: true
-        rove_link_3:
-          Value: true
-        rove_link_4:
-          Value: true
-        rove_link_5:
-          Value: true
-        rove_link_6:
+        sensor_link:
           Value: true
-        rove_link_base:
+        track_fl:
           Value: true
-        sensor_link:
+        track_fr:
           Value: true
         track_l:
           Value: true
         track_r:
           Value: true
+        track_rl:
+          Value: true
+        track_rr:
+          Value: true
         velodyne_laser:
           Value: true
         velodyne_link:
@@ -246,35 +202,22 @@ Visualization Manager:
       Show Axes: true
       Show Names: false
       Tree:
-        map:
-          odom:
-            base_link:
-              flipper_fl:
-                {}
-              flipper_fr:
-                {}
-              flipper_rl:
-                {}
-              flipper_rr:
-                {}
-              rove_link_base:
-                rove_link_1:
-                  rove_link_2:
-                    rove_link_3:
-                      rove_link_4:
-                        rove_link_5:
-                          rove_link_6:
-                            rove_end_effector:
-                              {}
-              sensor_link:
-                {}
-              track_l:
-                {}
-              track_r:
-                {}
-              velodyne_link:
-                velodyne_laser:
-                  {}
+        base_link:
+          sensor_link:
+            {}
+          track_l:
+            track_fl:
+              {}
+            track_rl:
+              {}
+          track_r:
+            track_fr:
+              {}
+            track_rr:
+              {}
+          velodyne_link:
+            velodyne_laser:
+              {}
       Update Interval: 0
       Value: true
     - Alpha: 0.699999988079071
@@ -382,7 +325,7 @@ Visualization Manager:
         y_scale: 1
         z_scale: 1
       Class: rviz_imu_plugin/Imu
-      Enabled: false
+      Enabled: true
       Name: Imu
       Topic:
         Depth: 5
@@ -391,12 +334,12 @@ Visualization Manager:
         History Policy: Keep Last
         Reliability Policy: Reliable
         Value: /imu
-      Value: false
+      Value: true
       fixed_frame_orientation: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
-    Fixed Frame: map
+    Fixed Frame: base_link
     Frame Rate: 30
   Name: root
   Tools:
@@ -439,7 +382,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 15.358924865722656
+      Distance: 7.311348915100098
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -454,10 +397,10 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.900398313999176
+      Pitch: 1.0553982257843018
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 5.728583812713623
+      Yaw: 4.913582801818848
     Saved: ~
 Window Geometry:
   Displays:

diff --git a/src/rove_description/package.xml b/src/rove_description/package.xml
@@ -10,6 +10,7 @@
   <exec_depend>rviz2</exec_depend>
   <exec_depend>xacro</exec_depend>
   <exec_depend>velodyne_description</exec_depend>
+  <exec_depend>rviz_imu_plugin</exec_depend>
 
   <buildtool_depend>ament_cmake</buildtool_depend>