Merge pull request #62 from abizovnuralem/small_fixes

Small fixes for nav2 and slam_toolbox

.github/workflows/ros_build.yaml

@@ -37,8 +37,12 @@ jobs:
         uses: actions/checkout@v2
         with:
           submodules: recursive
-      - name: rust
-        uses: actions-rust-lang/setup-rust-toolchain@v1
+      - name: Setup Rust
+        uses: actions-rs/toolchain@v1
+        with:
+          toolchain: 1.79.0
+          profile: minimal
+          override: true
       - name: build and test
         uses: ros-tooling/action-ros-ci@master
         with:

README.md

@@ -13,6 +13,8 @@ We are happy to present you our integration of the Unitree Go2 with ROS2 over Wi
 
 This repo will empower your Unitree GO2 AIR/PRO/EDU robots with ROS2 capabilities, using both WebRTC (Wi-Fi) and CycloneDDS (Ethernet) protocols.
 
+If you are using WebRTC (Wi-Fi) protocol, close the connection with a mobile app before connecting to the robot.
+
 ## Project RoadMap:
 
 1. URDF :white_check_mark: 
@@ -28,9 +30,9 @@ This repo will empower your Unitree GO2 AIR/PRO/EDU robots with ROS2 capabilitie
 12. Multi robot support :white_check_mark:
 13. WebRTC and CycloneDDS support :white_check_mark:
 14. Creating a PointCloud map and store it :white_check_mark:
-15. SLAM (slam_toolbox) (in the current version is not working, need to fix params)
-16. Navigation (nav2) (in the current version is not working, need to fix params)
-17. Object detection :white_check_mark:
+15. SLAM (slam_toolbox) :white_check_mark:
+16. Navigation (nav2) :white_check_mark:
+17. Object detection
 18. AutoPilot
 
 ## Your feedback and support mean the world to us. 
@@ -80,20 +82,27 @@ 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
-sudo apt install ros-humble-image-tools
-sudo apt install ros-humble-vision-msgs
+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.
 
-NOTE 2: for real time object detection and tracking, please install [PyTorch](https://pytorch.org/).
 
 Install `rust` language support in your system: [instructions](https://www.rust-lang.org/tools/install) 
 
-`cargo` should work in terminal
+After, you need to install 1.79 version of the cargo
+
+```
+rustup install 1.79.0
+rustup default 1.79.0
+```
+
+
+`cargo` should work in terminal 1.79 version
 ```shell
 cargo --version
 ```
@@ -117,9 +126,6 @@ ros2 launch go2_robot_sdk robot.launch.py
 
 ## Real time image detection and tracking
 
-<p align="center">
-<img width="300" src="https://github.com/abizovnuralem/go2_ros2_sdk/doc_images/go2_air_giraffe.png" alt='Giraffe Detection and Tracking'>
-</p>
 
 This capability is directly based on [J. Francis's work](https://github.com/jfrancis71/ros2_coco_detector). Once you have launched the sdk, the color image data will be available at `go2_camera/color/image`. In another terminal enter:
 

go2_robot_sdk/config/mapper_params_online_async.yaml

@@ -13,15 +13,14 @@ slam_toolbox:
     odom_frame: odom
     map_frame: map
     base_frame: base_link
-    scan_topic: /scan
+    scan_topic: scan
     # use_map_saver: true
     mode: localization
 
     # 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
     # will use pose
-    map_file_name: /home/brimo/Desktop/final/go2_ros2_sdk/room_serial
-    # map_start_pose: [0.0, 0.0, 0.0]
+    map_start_pose: [0.0, 0.0, 0.0]
     map_start_at_dock: true
 
     debug_logging: false
@@ -71,4 +70,4 @@ slam_toolbox:
     coarse_angle_resolution: 0.0349        
     minimum_angle_penalty: 0.9
     minimum_distance_penalty: 0.5
-    use_response_expansion: true
+    use_response_expansion: true

go2_robot_sdk/config/nav2_params.yaml

@@ -44,7 +44,7 @@ bt_navigator:
     use_sim_time: False
     global_frame: odom
     robot_base_frame: base_link
-    odom_topic: /odom
+    odom_topic: odom
     bt_loop_duration: 10
     default_server_timeout: 20
     # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
@@ -178,34 +178,31 @@ controller_server:
       RotateToGoal.slowing_factor: 5.0
       RotateToGoal.lookahead_time: -1.0
 
+
 local_costmap:
   local_costmap:
     ros__parameters:
-      update_frequency: 5.0
-      publish_frequency: 2.0
+      update_frequency: 1.0
+      publish_frequency: 1.0
       global_frame: odom
       robot_base_frame: base_link
       use_sim_time: False
       rolling_window: true
-      width: 3
-      height: 3
+      width: 6
+      height: 6
       resolution: 0.05
-      footprint: "[ [0.360, 0.500], [0.360, -0.200], [-0.800, -0.200], [-0.800, 0.500] ]"
-      plugins: ["voxel_layer", "inflation_layer"]
-      inflation_layer:
-        plugin: "nav2_costmap_2d::InflationLayer"
-        cost_scaling_factor: 3.0
-        inflation_radius: 0.2
+      footprint: "[ [0.360, 0.200], [0.360, -0.200], [-0.450, -0.200], [-0.450, 0.200] ]"
+      plugins: ["static_layer", "voxel_layer", "inflation_layer"]
+
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        map_subscribe_transient_local: True
+
       voxel_layer:
         plugin: "nav2_costmap_2d::VoxelLayer"
         enabled: True
         publish_voxel_map: True
-        origin_z: 0.0
-        z_resolution: 0.05
-        z_voxels: 16
-        max_obstacle_height: 2.0
-        mark_threshold: 0
-        observation_sources: scan3d
+        observation_sources: scan
         scan:
           topic: /scan
           max_obstacle_height: 2.0
@@ -216,21 +213,11 @@ local_costmap:
           raytrace_min_range: 0.0
           obstacle_max_range: 2.5
           obstacle_min_range: 0.0
-        scan3d:
-          topic: /point_cloud2
-          max_obstacle_height: 10.0
-          min_obstacle_height: 0.1
-          clearing: True
-          marking: True
-          data_type: "PointCloud2"
-          raytrace_max_range: 3.0
-          raytrace_min_range: 0.0
-          obstacle_max_range: 2.5
-          obstacle_min_range: 0.0
-      static_layer:
-        plugin: "nav2_costmap_2d::StaticLayer"
-        map_subscribe_transient_local: True
-      always_send_full_costmap: True
+
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 1.0
+        inflation_radius: 0.1
 
 global_costmap:
   global_costmap:
@@ -240,16 +227,27 @@ global_costmap:
       global_frame: odom
       robot_base_frame: base_link
       use_sim_time: False
-      footprint: "[ [0.360, 0.200], [0.360, -0.200], [-0.360, -0.200], [-0.360, 0.200] ]"
       resolution: 0.05
-      track_unknown_space: true
-      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
-      obstacle_layer:
-        plugin: "nav2_costmap_2d::ObstacleLayer"
+      track_unknown_space: False
+      static_map: True
+      width: 500
+      height: 500
+      origin_x: -250.0 
+      origin_y: -250.0
+      footprint: "[ [0.360, 0.200], [0.360, -0.200], [-0.450, -0.200], [-0.450, 0.200] ]"
+      plugins: ["static_layer", "voxel_layer", "inflation_layer"]
+
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        map_subscribe_transient_local: True
+
+      voxel_layer:
+        plugin: "nav2_costmap_2d::VoxelLayer"
         enabled: True
-        observation_sources: scan3d
+        publish_voxel_map: True
+        observation_sources: scan
         scan:
-          topic: /scan
+          topic: scan
           max_obstacle_height: 2.0
           clearing: True
           marking: True
@@ -258,25 +256,12 @@ global_costmap:
           raytrace_min_range: 0.0
           obstacle_max_range: 2.5
           obstacle_min_range: 0.0
-        scan3d:
-          topic: /point_cloud2
-          max_obstacle_height: 5.0
-          min_obstacle_height: 0.1
-          clearing: True
-          marking: True
-          data_type: "PointCloud2"
-          raytrace_max_range: 3.0
-          raytrace_min_range: 0.0
-          obstacle_max_range: 2.5
-          obstacle_min_range: 0.0
-      static_layer:
-        plugin: "nav2_costmap_2d::StaticLayer"
-        map_subscribe_transient_local: True
+
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
-        cost_scaling_factor: 3.0
-        inflation_radius: 0.2
-      always_send_full_costmap: True
+        cost_scaling_factor: 1.0
+        inflation_radius: 0.1
+
 
 map_server:
   ros__parameters: