enable slam and nav2

README.md

@@ -74,32 +74,27 @@ Tested systems and ROS2 distro
 |Ubuntu 22.04|humble|![ROS2 CI](https://github.com/abizovnuralem/go2_ros2_sdk/actions/workflows/ros_build.yaml/badge.svg)
 |Ubuntu 22.04|rolling|![ROS2 CI](https://github.com/abizovnuralem/go2_ros2_sdk/actions/workflows/ros_build.yaml/badge.svg)
 
-
 ## Installation
 
 ```shell
-mkdir -p ros2_ws/src
-cd ros2_ws/src
-git clone --recurse-submodules https://github.com/abizovnuralem/go2_ros2_sdk.git
-cp -a go2_ros2_sdk/. .
-rm -r -f go2_ros2_sdk
+mkdir -p ros2_ws
+cd ros2_ws
+git clone --recurse-submodules https://github.com/abizovnuralem/go2_ros2_sdk.git src
 sudo apt install ros-$ROS_DISTRO-image-tools
 sudo apt install ros-$ROS_DISTRO-vision-msgs
 sudo apt install python3-pip clang
 pip install -r requirements.txt
 cd ..
 ```
-
-NOTE 1: check for any error messages, and do not disregard them. If `pip install` does not complete cleanly, various features will not work. For example, `open3d` does not yet support `python3.12` and therefore you will need to set up a 3.11 `venv` first etc.
-
+Pay attention to any error messages. If `pip install` does not complete cleanly, various features will not work. For example, `open3d` does not yet support `python3.12` and therefore you will need to set up a 3.11 `venv` first etc.
 
 Install `rust` language support following these [instructions](https://www.rust-lang.org/tools/install). Then, install version 1.79 of `cargo`, the `rust` package manager.
 ```shell
 rustup install 1.79.0
 rustup default 1.79.0
 ```
 
-`cargo` should now be availible in the terminal:
+`cargo` should now be available in the terminal:
 ```shell
 cargo --version
 ```
@@ -137,7 +132,26 @@ The `robot.launch.py` code starts many services/nodes simultaneously, including
 * slam_toolbox/online_async_launch.py
 * av2_bringup/navigation_launch.py
 
-When you run `robot.launch.py`, `rviz` will fire up, lidar data will begin to accumulate, the front color camera data will be displayed too (typically after 4 seconds), and your dog will be waiting for commands from your joystick (e.g. a X-box controller). You can then steer the dog through your house, e.g., and collect lidar mapping data.  
+When you run `robot.launch.py`, `rviz` will fire up, lidar data will begin to accumulate, the front color camera data will be displayed too (typically after 4 seconds), and your dog will be waiting for commands from your joystick (e.g. a X-box controller). You can then steer the dog through your house, e.g., and collect LIDAR mapping data. 
+
+### SLAM and Nav2
+
+![Initial Rviz Display](doc_images/slam_nav_map.png)
+
+The goal of SLAM overall, and the `slam_toolbox` in particular, is to create a map. The `slam_toolbox` is a grid mapper - it thinks about the world in terms of a fixed grid that the dog operates in. When the dog initially moves through a new space, data accumulate and the developing map is and published it to the `/map` topic. The goal of `Nav2` is to navigate and perform other tasks in this map.
+
+The `rviz` settings that are used upon initial launch (triggered by `ros2 launch go2_robot_sdk robot.launch.py`) showcase various datastreams.  
+
+* `RobotModel` is the dimensionally correct model of the G02 
+* `PointCloud2` are the raw LIDAR data transformed into 3D objects/constraints 
+* `LaserScan` are lower level scan data before translation into an x,y,z frame
+* `Image` are the data from the front-facing color camera 
+* `Map` is the map being created by the `slam_toolbox`
+* `Odometry` is the history of directions/movements of the dog
+
+If there is too much going on in the initial screen, deselect the `map` topic to allow you to see more.
+
+![Simplified Rviz Display](doc_images/slam_nav.png)
 
 ## Real time image detection and tracking
 
@@ -179,7 +193,7 @@ export MAP_SAVE=True
 export MAP_NAME="3d_map"
 ```
 
-Every 10 seconds, a map will be saved to the root folder of the repo.
+Every 10 seconds, a pointcloud map will be saved to the root folder of the repo.
 
 ## Multi robot support 
 If you want to connect several robots for collaboration:

go2_robot_sdk/config/mapper_params_online_async.yaml

@@ -13,9 +13,9 @@ slam_toolbox:
     odom_frame: odom
     map_frame: map
     base_frame: base_link
-    scan_topic: scan
-    # use_map_saver: true
-    mode: localization
+    scan_topic: /scan
+    use_map_saver: true
+    mode: mapping
 
     # if you'd like to immediately start continuing a map at a given pose
     # or at the dock, but they are mutually exclusive, if pose is given
@@ -25,14 +25,14 @@ slam_toolbox:
 
     debug_logging: false
     throttle_scans: 1
-    transform_publish_period: 0.02 #if 0 never publishes odometry
+    transform_publish_period: 0.02 # if 0 never publishes odometry
     map_update_interval: 5.0
     resolution: 0.05
-    max_laser_range: 20.0 #for rastering images
+    max_laser_range: 20.0 # for rastering images
     minimum_time_interval: 0.5
     transform_timeout: 0.2
     tf_buffer_duration: 30.0
-    stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
+    stack_size_to_use: 40000000 # program needs a larger stack size to serialize large maps
     enable_interactive_mode: true
 
     # General Parameters

go2_robot_sdk/config/single_robot_conf.rviz

@@ -1,17 +1,15 @@
 Panels:
   - Class: rviz_common/Displays
-    Help Height: 78
+    Help Height: 153
     Name: Displays
     Property Tree Widget:
       Expanded:
         - /Global Options1
-        - /Status1
         - /PointCloud21/Topic1
         - /LaserScan1/Topic1
-        - /Image1
         - /Image1/Topic1
       Splitter Ratio: 0.5
-    Tree Height: 519
+    Tree Height: 748
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -29,7 +27,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: ""
+    SyncSource: LaserScan
 Visualization Manager:
   Class: ""
   Displays:
@@ -199,15 +197,20 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
-        base:
+        base_footprint:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        base_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        base_footprint:
+        front_camera:
           Alpha: 1
           Show Axes: false
           Show Trail: false
+          Value: true
         imu:
           Alpha: 1
           Show Axes: false
@@ -244,12 +247,12 @@ Visualization Manager:
       Color: 255; 255; 255
       Color Transformer: Intensity
       Decay Time: 0
-      Enabled: false
+      Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Max Intensity: 186
       Min Color: 0; 0; 0
-      Min Intensity: 36
+      Min Intensity: 30
       Name: PointCloud2
       Position Transformer: XYZ
       Selectable: true
@@ -265,7 +268,7 @@ Visualization Manager:
         Value: /point_cloud2
       Use Fixed Frame: true
       Use rainbow: true
-      Value: false
+      Value: true
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
@@ -278,7 +281,7 @@ Visualization Manager:
       Color: 224; 27; 36
       Color Transformer: FlatColor
       Decay Time: 0
-      Enabled: false
+      Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Max Intensity: 4096
@@ -299,7 +302,7 @@ Visualization Manager:
         Value: /scan
       Use Fixed Frame: true
       Use rainbow: true
-      Value: false
+      Value: true
     - Class: rviz_default_plugins/Image
       Enabled: true
       Max Value: 1
@@ -314,6 +317,66 @@ Visualization Manager:
         Reliability Policy: Reliable
         Value: /go2_camera/color/image
       Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz_default_plugins/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /map
+      Update Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /map_updates
+      Use Timestamp: false
+      Value: true
+    - Angle Tolerance: 0.10000000149011612
+      Class: rviz_default_plugins/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.30000001192092896
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: true
+      Keep: 100
+      Name: Odometry
+      Position Tolerance: 0.10000000149011612
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.10000000149011612
+        Color: 255; 25; 0
+        Head Length: 0.30000001192092896
+        Head Radius: 0.10000000149011612
+        Shaft Length: 1
+        Shaft Radius: 0.05000000074505806
+        Value: Arrow
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /odom
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -360,7 +423,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 4.809005260467529
+      Distance: 7.4484639167785645
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -383,12 +446,12 @@ Visualization Manager:
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1179
+  Height: 1686
   Hide Left Dock: false
   Hide Right Dock: false
   Image:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000252000003fdfc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000292000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002d5000001650000002800ffffff000000010000010f000003fdfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003fd000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007b20000003efc0100000002fb0000000800540069006d00650100000000000007b2000002fb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000445000003fd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000003a9000005e7fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006e00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000047000003cc000000f100fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000041a000002140000002f00ffffff000000010000010f000005e7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000047000005e7000000c200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000bc700000040fc0100000002fb0000000800540069006d0065010000000000000bc70000038100fffffffb0000000800540069006d0065010000000000000450000000000000000000000701000005e700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -397,6 +460,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1970
-  X: 275
-  Y: 118
+  Width: 3015
+  X: 274
+  Y: 118

go2_robot_sdk/go2_robot_sdk/lidar_to_point.py

@@ -29,7 +29,6 @@
 from sensor_msgs_py import point_cloud2
 import open3d as o3d
 
-
 class LidarToPointCloud(Node):
     def __init__(self):
         super().__init__('lidar_to_pointcloud')
@@ -54,12 +53,11 @@ def __init__(self):
 
         self.conn_mode = "single" if len(self.robot_ip_lst) == 1 else "multi"
 
-
         if self.conn_mode == 'single':
 
             self.subscription = self.create_subscription(
                 PointCloud2,
-                '/point_cloud2',
+                '/robot0/point_cloud2',
                 self.lidar_callback,
                 10
             )

go2_robot_sdk/launch/robot.launch.py

@@ -84,7 +84,6 @@ def generate_launch_description():
         'foxglove_bridge_launch.xml',
     )
 
-    # TODO Need to fix Nav2
     slam_toolbox_config = os.path.join(
         get_package_share_directory('go2_robot_sdk'),
         'config',
@@ -122,7 +121,7 @@ def generate_launch_description():
             Node(
                 package='ros2_go2_video',
                 executable='ros2_go2_video',
-                parameters=[{'robot_ip': robot_ip_lst[i],
+                parameters=[{'robot_ip': robot_ip_lst[0],
                              'robot_token': robot_token}],
             ),
         )