nav2 and slam toolbox configurations are fixed

deploy/docker/Dockerfile.nav2

@@ -13,6 +13,7 @@ RUN apt-get update && apt-get install -y \
     ros-humble-nav2-* ros-humble-navigation2 \
     ros-humble-slam-toolbox \ 
     ros-humble-robot-localization\
+    ros-humble-image-geometry \
     && rm -rf /var/lib/apt/lists/*
 
 # Cheange the ROS2 RMW to CycloneDDS as instructed by Unitree
@@ -22,6 +23,7 @@ cd .. && colcon build --packages-select cyclonedds && source /opt/ros/humble/set
 
 # copy the go2py ros2 nodes
 COPY ros2_nodes/sportmode_nav2 /nav2_ws/src/sportmode_nav2
+COPY ros2_nodes/m-explore-ros2 /nav2_ws/src/m-explore-ros2
 COPY ros2_nodes/go2py_messages /nav2_ws/src/go2py_messages
 RUN cd /nav2_ws && source /opt/ros/humble/setup.bash && source /unitree_ros2/cyclonedds_ws/install/setup.bash && colcon build --symlink-install
 

deploy/ros2_nodes/go2_description/xacro/robot.xacro

@@ -157,7 +157,7 @@
         </visual>
     </link>
 
-    <link name="utlidar">
+    <!-- <link name="utlidar">
         <inertial>
         <origin rpy="0 0 0" xyz="0 0 0"/>
         <mass value="0"/>
@@ -169,7 +169,7 @@
         <parent link="base_link"/>
         <child link="utlidar"/>
         <axis xyz="0 0 0"/>
-    </joint>
+    </joint> -->
 
     <link name="hesai">
         <inertial>

deploy/ros2_nodes/lidar_node/src/manager/source_driver_ros2.hpp

@@ -298,10 +298,10 @@ inline void SourceDriver::publishLaserScan(
   scan_msg->angle_increment = 0.0087/2.;
   scan_msg->time_increment = 0.0;
   scan_msg->scan_time = 0.3333;
-  scan_msg->range_min = 0.2;
+  scan_msg->range_min = 0.3;
   scan_msg->range_max = 100;
   float max_height_ = 1.0;
-  float min_height_ = -0.33;
+  float min_height_ = -0.1;
   
 
   // determine amount of rays to create

deploy/ros2_nodes/sportmode_nav2/launch/cbf_navigation_launch.py

@@ -28,10 +28,10 @@ from nav2_common.launch import RewrittenYaml
 
 def generate_launch_description():
     # Get the launch directory
-    bringup_dir = get_package_share_directory('nav2_bringup')
+    bringup_dir = get_package_share_directory('sportmode_nav2')
 
     namespace = LaunchConfiguration('namespace')
-    use_sim_time = LaunchConfiguration('use_sim_time')
+    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
     autostart = LaunchConfiguration('autostart')
     params_file = LaunchConfiguration('params_file')
     use_composition = LaunchConfiguration('use_composition')

deploy/ros2_nodes/sportmode_nav2/launch/navigation_launch.py

@@ -28,10 +28,10 @@ from nav2_common.launch import RewrittenYaml
 
 def generate_launch_description():
     # Get the launch directory
-    bringup_dir = get_package_share_directory('nav2_bringup')
+    bringup_dir = get_package_share_directory('sportmode_nav2')
 
     namespace = LaunchConfiguration('namespace')
-    use_sim_time = LaunchConfiguration('use_sim_time')
+    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
     autostart = LaunchConfiguration('autostart')
     params_file = LaunchConfiguration('params_file')
     use_composition = LaunchConfiguration('use_composition')

deploy/ros2_nodes/sportmode_nav2/params/mapping_async.yaml

@@ -28,7 +28,7 @@ slam_toolbox:
     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: 5.0 #for rastering images
     minimum_time_interval: 0.5
     transform_timeout: 0.2
     tf_buffer_duration: 30.

deploy/ros2_nodes/sportmode_nav2/params/nav2_params.yaml

@@ -1,12 +1,12 @@
 amcl:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
     alpha1: 0.2
     alpha2: 0.2
     alpha3: 0.2
     alpha4: 0.2
     alpha5: 0.2
-    base_frame_id: "base_footprint"
+    base_frame_id: "base_link"
     beam_skip_distance: 0.5
     beam_skip_error_threshold: 0.9
     beam_skip_threshold: 0.3
@@ -37,14 +37,14 @@ amcl:
     z_max: 0.05
     z_rand: 0.5
     z_short: 0.05
-    scan_topic: scan
+    scan_topic: /go2/scan
 
 bt_navigator:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
     global_frame: map
     robot_base_frame: base_link
-    odom_topic: /odom
+    odom_topic: /go2/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:
@@ -102,15 +102,15 @@ bt_navigator:
 
 bt_navigator_navigate_through_poses_rclcpp_node:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
 
 bt_navigator_navigate_to_pose_rclcpp_node:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
 
 controller_server:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
     controller_frequency: 20.0
     min_x_velocity_threshold: 0.001
     min_y_velocity_threshold: 0.5
@@ -134,8 +134,8 @@ controller_server:
     general_goal_checker:
       stateful: True
       plugin: "nav2_controller::SimpleGoalChecker"
-      xy_goal_tolerance: 0.25
+      xy_goal_tolerance: 0.5
-      yaw_goal_tolerance: 0.25
+      yaw_goal_tolerance: 1.
     # DWB parameters
     FollowPath:
       plugin: "dwb_core::DWBLocalPlanner"
@@ -186,7 +186,7 @@ local_costmap:
       publish_frequency: 2.0
       global_frame: odom
       robot_base_frame: base_link
-      use_sim_time: True
+      use_sim_time: False
       rolling_window: true
       width: 3
       height: 3
@@ -229,10 +229,10 @@ global_costmap:
       publish_frequency: 1.0
       global_frame: map
       robot_base_frame: base_link
-      use_sim_time: True
+      use_sim_time: False
       robot_radius: 0.22
       resolution: 0.05
-      track_unknown_space: false
+      track_unknown_space: true
       plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
       obstacle_layer:
         plugin: "nav2_costmap_2d::ObstacleLayer"
@@ -259,14 +259,14 @@ global_costmap:
 
 map_server:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
     # Overridden in launch by the "map" launch configuration or provided default value.
     # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below.
     yaml_filename: ""
 
 map_saver:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
     save_map_timeout: 5.0
     free_thresh_default: 0.25
     occupied_thresh_default: 0.65
@@ -275,7 +275,7 @@ map_saver:
 planner_server:
   ros__parameters:
     expected_planner_frequency: 20.0
-    use_sim_time: True
+    use_sim_time: False
     planner_plugins: ["GridBased"]
     GridBased:
       plugin: "nav2_navfn_planner/NavfnPlanner"
@@ -285,7 +285,7 @@ planner_server:
 
 smoother_server:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
     smoother_plugins: ["simple_smoother"]
     simple_smoother:
       plugin: "nav2_smoother::SimpleSmoother"
@@ -312,7 +312,7 @@ behavior_server:
     global_frame: odom
     robot_base_frame: base_link
     transform_tolerance: 0.1
-    use_sim_time: true
+    use_sim_time: False
     simulate_ahead_time: 2.0
     max_rotational_vel: 1.0
     min_rotational_vel: 0.4
@@ -320,11 +320,11 @@ behavior_server:
 
 robot_state_publisher:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
 
 waypoint_follower:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
     loop_rate: 20
     stop_on_failure: false
     waypoint_task_executor_plugin: "wait_at_waypoint"
@@ -335,7 +335,7 @@ waypoint_follower:
 
 velocity_smoother:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
     smoothing_frequency: 20.0
     scale_velocities: False
     feedback: "OPEN_LOOP"
@@ -343,7 +343,7 @@ velocity_smoother:
     min_velocity: [-0.26, 0.0, -1.0]
     max_accel: [2.5, 0.0, 3.2]
     max_decel: [-2.5, 0.0, -3.2]
-    odom_topic: "odom"
+    odom_topic: "/go2/odom"
     odom_duration: 0.1
     deadband_velocity: [0.0, 0.0, 0.0]
-    velocity_timeout: 1.0
+    velocity_timeout: 1.0

scripts/run_nav2.sh

@@ -0,0 +1,46 @@
+#!/bin/bash
+
+CONTAINER_NAME="go2py_nav2"
+
+# Function to print usage
+function usage() {
+    echo "Usage: run_go2py_nav2.sh"
+}
+
+# Function to check if docker command succeeded
+function check_docker() {
+    if [ $? -ne 0 ]; then
+        echo "Docker command failed. Please check your Docker installation."
+        exit 1
+    fi
+}
+
+# Prevent running as root
+if [ $(id -u) -eq 0 ]; then
+    echo "This script cannot be executed with root privileges."
+    echo "Please re-run without sudo and configure docker for non-root user if needed."
+    exit 1
+fi
+
+# Check if user can run docker without root
+if [[ ! $(groups $USER) =~ docker ]]; then
+    echo "User |$USER| is not a member of the 'docker' group and cannot run docker commands without sudo."
+    echo "Run 'sudo usermod -aG docker \$USER && newgrp docker' to add user to 'docker' group, then re-run this script."
+    echo "See: https://docs.docker.com/engine/install/linux-postinstall/"
+    exit 1
+fi
+
+# Check if able to run docker commands
+docker ps &>/dev/null
+check_docker
+
+# Check if the container is running
+if [ "$(docker ps -q -f name=$CONTAINER_NAME)" ]; then
+    echo "Attaching to running container: $CONTAINER_NAME"
+    docker exec -it --workdir /home/nav2_ws $CONTAINER_NAME /bin/bash 
+    check_docker
+else
+    echo "Starting new container: $CONTAINER_NAME"
+    sudo docker run -it --rm --name $CONTAINER_NAME --privileged --network host -v $(pwd)/deploy/ros2_nodes/sportmode_nav2:/home/nav2_ws/src/sportmode_nav2 -v /dev/*:/dev/* -v /etc/localtime:/etc/localtime:ro --runtime nvidia --workdir /home/nav2_ws go2py_nav2:latest  
+    check_docker
+fi

setup.cfg

@@ -18,7 +18,7 @@ include_package_data = True
 install_requires =
     pygame
     pyyaml
-    pynput 
+    # pynput 
-    pin
+#    pin
-    mujoco
+#    mujoco
-    cyclonedds
+    cyclonedds