navigation parameters updated

deploy/docker/Dockerfile.nav2

@@ -10,6 +10,7 @@ RUN apt-get update && apt-get install -y \
     build-essential \
     cmake \
     git \
+    python3-pip \
     ros-humble-nav2-* ros-humble-navigation2 \
     ros-humble-slam-toolbox \ 
     ros-humble-robot-localization\
@@ -27,5 +28,9 @@ COPY ros2_nodes/m-explore-ros2 /home/nav2_ws/src/m-explore-ros2
 COPY ros2_nodes/go2py_messages /home/nav2_ws/src/go2py_messages
 RUN cd /home/nav2_ws && source /opt/ros/humble/setup.bash && source /unitree_ros2/cyclonedds_ws/install/setup.bash && colcon build --symlink-install
 
+RUN cd / && \
+    python3 -m pip install --upgrade pip && \
+    python3 -m pip install numpy scipy paramiko openai
+    
 # set the entrypoint to bash
 ENTRYPOINT ["/bin/bash"]

deploy/ros2_nodes/front-camera-publisher.py

@@ -11,7 +11,7 @@ class ImagePublisher(Node):
         self.publisher_ = self.create_publisher(Image, '/go2/front/image_raw', 10)
         self.bridge = CvBridge()
         self.gstreamer_str = "udpsrc address=230.1.1.1 port=1720 multicast-iface=eth0 ! application/x-rtp, media=video, encoding-name=H264 ! rtph264depay ! h264parse ! avdec_h264 ! videoconvert ! video/x-raw,width=1280,height=720,format=BGR ! appsink drop=1"
-        self.cap = cv2.VideoCapture(0)
+        self.cap = cv2.VideoCapture(6)
 
     def publish_images(self):
         while rclpy.ok():

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

@@ -293,16 +293,27 @@ inline void SourceDriver::publishLaserScan(
   auto scan_msg = std::make_unique<sensor_msgs::msg::LaserScan>();
   scan_msg->header = cloud_msg->header;
 
-  scan_msg->angle_min = -3.1415;
-  scan_msg->angle_max =  3.1415;
-  scan_msg->angle_increment = 0.0087/2.;
-  scan_msg->time_increment = 0.0;
-  scan_msg->scan_time = 0.3333;
-  scan_msg->range_min = 0.3;
-  scan_msg->range_max = 100;
-  float max_height_ = 1.0;
-  float min_height_ = -0.1;
+   scan_msg->angle_min = -3.1415;
+   scan_msg->angle_max =  3.1415;
+   scan_msg->angle_increment = 0.0087/2.;
+   scan_msg->time_increment = 0.0;
+   scan_msg->scan_time = 0.3333;
+   scan_msg->range_min = 0.1;
+   scan_msg->range_max = 15;
+   float max_height_ = 1.0;
+   float min_height_ = -0.1;
   
+  // scan_msg->angle_min = -(3*3.1415)/4;
+  // scan_msg->angle_max =  -3.1415/4;
+  // scan_msg->angle_min = -3.1415;
+  //scan_msg->angle_max =  3.1415;
+  //scan_msg->angle_increment = 0.0087/2.;
+  //scan_msg->time_increment = 0.0;
+  //scan_msg->scan_time = 0.3333;
+  //scan_msg->range_min = 0.3;
+  //scan_msg->range_max = 5;
+  //float max_height_ = 1.0;
+  //float min_height_ = -0.1;
 
   // determine amount of rays to create
   uint32_t ranges_size = std::ceil(

deploy/ros2_nodes/sportmode_nav2/params/nav2_params.yaml

@@ -196,7 +196,7 @@ local_costmap:
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 3.0
-        inflation_radius: 0.55
+        inflation_radius: 0.2
       voxel_layer:
         plugin: "nav2_costmap_2d::VoxelLayer"
         enabled: True
@@ -254,7 +254,7 @@ global_costmap:
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 3.0
-        inflation_radius: 0.55
+        inflation_radius: 0.65
       always_send_full_costmap: True
 
 map_server:
@@ -346,4 +346,4 @@ velocity_smoother:
     odom_topic: "/go2/odom"
     odom_duration: 0.1
     deadband_velocity: [0.0, 0.0, 0.0]
-    velocity_timeout: 1.0
+    velocity_timeout: 1.0

deploy/ros2_nodes/sportmode_nav2/params/ros_ekf.yaml

@@ -4,7 +4,7 @@ ekf_filter_node:
 # The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
 # computation until it receives at least one message from one of the inputs. It will then run continuously at the
 # frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
-        frequency: 100.0
+        frequency: 50.0
 
 # The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict
 # cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the

deploy/services/frontcam-v4l-loopback.sh

@@ -1,7 +1,7 @@
 /usr/sbin/modprobe v4l2loopback
 # Run the GStreamer pipeline
 /usr/bin/gst-launch-1.0 udpsrc address=230.1.1.1 port=1720 multicast-iface=eth0 ! queue ! application/x-rtp, media=video, \
-encoding-name=H264 ! rtph264depay ! h264parse ! avdec_h264 ! videoconvert ! v4l2sink device=/dev/video0 &
+encoding-name=H264 ! rtph264depay ! h264parse ! avdec_h264 ! videoconvert ! v4l2sink device=/dev/video6 &
 sleep 1
 # Run the ROS image publisher node
-/usr/bin/docker run --rm --name go2py_frontcam --privileged --network host -v /dev/*:/dev/* -v /etc/localtime:/etc/localtime:ro go2py_frontcam_publisher:latest
+/usr/bin/docker run --rm --name go2py_frontcam --privileged --network host -v /dev/*:/dev/* -v /etc/localtime:/etc/localtime:ro go2py_frontcam_publisher:latest

scripts/run_nav2.sh

@@ -41,6 +41,6 @@ if [ "$(docker ps -q -f name=$CONTAINER_NAME)" ]; then
     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  
+    sudo docker run -it --rm --name $CONTAINER_NAME --privileged --network host --ipc=host -v $(pwd)/deploy/ros2_nodes/sportmode_nav2:/home/nav2_ws/src/sportmode_nav2 -v /dev/*:/dev/* -v /etc/localtime:/etc/localtime:ro -v /home/unitree/Go2Py/deploy/SGNav/llm_planner:/home/llm_planner --runtime nvidia --workdir /home/nav2_ws go2py_nav2:latest  
     check_docker
 fi