Feature/ag moving nav2

.devcontainer/cyclonedds.xml

@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<CycloneDDS>
+  <Domain>
+    <Discovery>
+      <MaxAutoParticipantIndex>512</MaxAutoParticipantIndex>
+    </Discovery>
+  </Domain>
+</CycloneDDS>

.devcontainer/devcontainer.json

@@ -26,6 +26,7 @@
         "ROS_AUTOMATIC_DISCOVERY_RANGE": "SUBNET",
         "ROS_DOMAIN_ID": "42",
         "RMW_IMPLEMENTATION": "rmw_cyclonedds_cpp",
+        "CYCLONEDDS_URI": "file:///home/ros2_ws/.devcontainer/cyclonedds.xml",
         "GNOME_TERMINAL_SCREEN": ""
     },
     "runArgs": [

.devcontainer/start-gazebo-vnc.sh

@@ -3,12 +3,22 @@
 # and rtabmap (and other GUIs) from bringup inside the devcontainer and view
 # them on your Mac. Run once per container session, then connect with a VNC
 # client to localhost:5900.
+#
+# IMPORTANT: You must SOURCE this script so DISPLAY is set in your shell:
+#   source /home/ros2_ws/.devcontainer/start-gazebo-vnc.sh
+# If you run it with ./start-gazebo-vnc.sh, the export is lost when the script exits.
 
 DISPLAY_NUM=99
 VNC_PORT=5900
 # Virtual screen size: wide enough for Gazebo + rtabmap side by side
 XVFB_RES="1920x1080x24"
 
+if [[ "${BASH_SOURCE[0]}" == "${0}" ]]; then
+  echo "WARNING: Run with: source $(readlink -f "${BASH_SOURCE[0]}" 2>/dev/null || echo "$0")"
+  echo "         so DISPLAY=:$DISPLAY_NUM is set in your shell (otherwise Gazebo will be headless)."
+  echo ""
+fi
+
 # Start Xvfb (if already running for this display, it will error and we continue)
 rm -f /tmp/.X${DISPLAY_NUM}-lock
 Xvfb ":$DISPLAY_NUM" -screen 0 ${XVFB_RES} -ac &
@@ -25,7 +35,9 @@ echo "fluxbox (window manager) on :$DISPLAY_NUM"
 x11vnc -display ":$DISPLAY_NUM" -rfbport $VNC_PORT -forever -shared -bg -noxdamage -passwd vnc 2>/dev/null || true
 echo "VNC server on port $VNC_PORT — when prompted, password is: vnc"
 
-# Set DISPLAY and LIBGL for new terminals so Gazebo + rtabmap both render here (idempotent)
+# Set DISPLAY and LIBGL for this shell AND for new terminals (so Gazebo/rtabmap use VNC)
+export DISPLAY=:$DISPLAY_NUM
+export LIBGL_ALWAYS_SOFTWARE=1
 MARKER="# Gazebo + rtabmap VNC display (start-gazebo-vnc.sh)"
 if ! grep -q "$MARKER" ~/.bashrc 2>/dev/null; then
   echo "" >> ~/.bashrc
@@ -35,8 +47,10 @@ fi
 
 echo ""
 echo "1. On your Mac: connect to localhost:$VNC_PORT — password: vnc"
-echo "2. In a new terminal in the container, run:"
+echo "2. In this SAME terminal run bringup:"
 echo "   source /home/ros2_ws/install/setup.bash"
-echo "   ros2 launch igvc_bringup bringup.launch.py use_sim:=true"
-echo "   (Gazebo and rtabmap windows will both appear in the VNC window)"
+echo "   ros2 launch igvc_bringup bringup.launch.py use_sim:=true use_slam:=true use_mock_hardware:=true use_sim_time:=true"
+echo ""
+echo "If you did not run this with 'source start-gazebo-vnc.sh', DISPLAY is wrong."
+echo "Stop and run:  source /home/ros2_ws/.devcontainer/start-gazebo-vnc.sh"
 echo ""

docs/MACOS_Gazebo_running.md

@@ -4,15 +4,19 @@ Short reference for running the Gazebo simulation **with the 3D view** in the de
 
 ---
 
-## 1. Start VNC (once per container session)
+## 1. Start VNC and run bringup (same terminal)
 
-In a terminal **in the devcontainer**:
+You must **source** the script (not run it) so `DISPLAY=:99` is set in your shell. Otherwise Gazebo and rtabmap run headless and the VNC window stays black.
+
+In a single terminal **in the devcontainer**:
 
 ```bash
-/home/ros2_ws/.devcontainer/start-gazebo-vnc.sh
+source /home/ros2_ws/.devcontainer/start-gazebo-vnc.sh
+source /home/ros2_ws/install/setup.bash
+ros2 launch igvc_bringup bringup.launch.py use_sim:=true use_slam:=true use_mock_hardware:=true use_sim_time:=true
 ```
 
-Password when prompted: **vnc**
+Password when connecting to VNC: **vnc**
 
 ---
 
@@ -21,20 +25,9 @@ Password when prompted: **vnc**
 - **Finder:** Cmd+K → `vnc://localhost:5900` → Connect → password: **vnc**
 - Or **Screen Sharing** app → connect to **localhost** → password: **vnc**
 
-You should see a gray/empty VNC window.
-
----
-
-## 3. Run bringup in the container (Gazebo + rtabmap)
-
-In a **second** terminal in the devcontainer:
-
-```bash
-source /home/ros2_ws/install/setup.bash
-ros2 launch igvc_bringup bringup.launch.py use_sim:=true
-```
+You should see a gray/empty VNC window at first; after bringup starts, Gazebo and rtabmap windows appear there. Use the taskbar at the bottom or click a window to bring it to the front (fluxbox window manager).
 
-Do **not** use `headless:=true`. Both the Gazebo and rtabmap windows appear in the VNC window. Use the taskbar at the bottom or click a window to bring it to the front (fluxbox window manager).
+Do **not** use `headless:=true`. Do **not** run bringup in a different terminal than the one where you sourced the script — that terminal would not have `DISPLAY=:99`.
 
 ---
 

docs/RUN_NAV2.md

@@ -0,0 +1,45 @@
+## Running Nav2 with Gazebo (Mac + devcontainer)
+
+- **Terminal 1 – VNC + simulation (inside devcontainer)**
+
+```bash
+
+cd /home/ros2_ws
+colcon build --packages-select igvc_nav igvc_hardware
+source install/setup.bash
+```
+
+
+```bash
+cd /home/ros2_ws
+source .devcontainer/start-gazebo-vnc.sh
+source install/setup.bash
+ros2 launch igvc_bringup bringup.launch.py use_sim:=true use_slam:=true use_mock_hardware:=true use_sim_time:=true
+```
+
+Then, from macOS, connect a VNC client to `vnc://localhost:5900` (password `vnc`).
+
+- **Terminal 2 – Nav2 stack**
+
+```bash
+cd /home/ros2_ws
+source install/setup.bash
+ros2 launch igvc_nav igvc_nav.launch.py use_sim_time:=true
+```
+
+- **Terminal 3 – Send a navigation goal**
+
+```bash
+cd /home/ros2_ws
+source install/setup.bash
+ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose "{
+  pose: {
+    header: { frame_id: 'map' },
+    pose: {
+      position: { x: 2.0, y: 0.0, z: 0.0 },
+      orientation: { x: 0.0, y: 0.0, z: 0.0, w: 1.0 }
+    }
+  }
+}"
+```
+

src/igvc_bringup/launch/bringup.launch.py

@@ -9,6 +9,7 @@
 def generate_launch_description():
     use_sim = LaunchConfiguration('use_sim')
     use_mock_hardware = LaunchConfiguration('use_mock_hardware')
+    use_sim_time = LaunchConfiguration('use_sim_time')
 
     return LaunchDescription([
         # Launch Arguments
@@ -33,6 +34,12 @@ def generate_launch_description():
             default_value='true', 
             description='Launch rtabmap for SLAM'
         ),
+
+        DeclareLaunchArgument(
+            'use_sim_time',
+            default_value='true',
+            description='Use simulation time'
+       ),
 
 
         # Publishers & URDF
@@ -43,7 +50,8 @@ def generate_launch_description():
                  '/publisher.launch.py']
             ),
             launch_arguments={
-                'use_mock_hardware': use_mock_hardware
+                'use_mock_hardware': use_mock_hardware, 
+                'use_sim_time' : use_sim_time, 
                 }.items()
         ),
 
@@ -57,6 +65,9 @@ def generate_launch_description():
                 ]
             ),
             condition=IfCondition(use_sim),
+            launch_arguments={
+                'use_sim_time' : use_sim_time, 
+            }.items(),
         ),
 
         # ROS2_Control
@@ -66,6 +77,9 @@ def generate_launch_description():
                  '/launch',
                  '/control.launch.py']
             ),
+            launch_arguments={
+                'use_sim_time' : use_sim_time, 
+            }.items(),
         ),
 
         # Real hardware
@@ -75,7 +89,10 @@ def generate_launch_description():
                  '/launch',
                  '/hardware.launch.py']
             ),
-            condition = UnlessCondition(use_mock_hardware)
+            condition = UnlessCondition(use_mock_hardware), 
+            launch_arguments={
+                'use_sim_time' : use_sim_time, 
+            }.items(),
         ),
 
         # SLAM
@@ -85,7 +102,10 @@ def generate_launch_description():
                  '/launch',
                  '/sim_rtabmap.launch.py']
             ),
-            condition = IfCondition(LaunchConfiguration('use_slam'))
+            condition = IfCondition(LaunchConfiguration('use_slam')),
+            launch_arguments={
+                'use_sim_time' : use_sim_time, 
+            }.items(),
         ),
 
         # TODO Nav

src/igvc_description/launch/publisher.launch.py

@@ -38,7 +38,7 @@ def generate_launch_description():
 
   declare_use_sim_time_cmd = DeclareLaunchArgument(
     name='use_sim_time',
-    default_value='False',
+    default_value='True',
     description='Use simulation (Gazebo) clock if true')
 
   # Specify the actions
@@ -48,6 +48,7 @@ def generate_launch_description():
     condition=UnlessCondition(gui),
     package='joint_state_publisher',
     executable='joint_state_publisher',
+    parameters=[{'use_sim_time': use_sim_time}],
     name='joint_state_publisher')
 
   # A GUI to manipulate the joint state values

src/igvc_description/rviz/rviz_settings.rviz

@@ -64,11 +64,11 @@ Visualization Manager:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        base_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
+        # base_link:
+        #   Alpha: 1
+        #   Show Axes: false
+        #   Show Trail: false
+        #   Value: true
         box_rotate:
           Alpha: 1
           Show Axes: false

src/igvc_description/urdf/sensors.urdf.xacro

@@ -1,9 +1,115 @@
 <?xml version="1.0" ?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:arg name="camera_name"   default="oak" />
+    <xacro:arg name="camera_model"  default="OAK-D-POE" />
+    <xacro:arg name="base_frame"    default="oak_base" />
+    <xacro:arg name="parent_frame"  default="oak_parent" />
+    <xacro:arg name="cam_pos_x"     default="0.0" />
+    <xacro:arg name="cam_pos_y"     default="0.0" />
+    <xacro:arg name="cam_pos_z"     default="0.0" />
+    <xacro:arg name="cam_roll"      default="0.0" />
+    <xacro:arg name="cam_pitch"     default="0.0" />
+    <xacro:arg name="cam_yaw"       default="0.0" />
+    <xacro:arg name="rs_compat"     default="true" />
+    <xacro:include filename="$(find depthai_descriptions)/urdf/depthai_descr.urdf.xacro" />
+
+    <joint name="camera_base_joint" type="fixed">
+        <parent link="base_footprint"/>
+        <child link="oak_parent"/>
+        <origin xyz="0 -0.4 0.95" rpy="0 0 ${pi/2}"/>
+    </joint>
 
-    <xacro:include filename="$(find igvc_description)/urdf/sensor/lidar.urdf.xacro"/>
-    <xacro:include filename="$(find igvc_description)/urdf/sensor/camera.urdf.xacro"/>
-    <xacro:include filename="$(find igvc_description)/urdf/sensor/imu.urdf.xacro"/>
-    <xacro:include filename="$(find igvc_description)/urdf/sensor/gps.urdf.xacro"/>
+    <gazebo reference="oak_color_frame">
+        <sensor name="oak_depth" type="depth">
+            <pose> 0 0 0 0 0 0 </pose>
+            <topic>oak/depth</topic>
+            <update_rate>30</update_rate>
+            <always_on>1</always_on>
+            <gz_frame_id>oak_color_optical_frame</gz_frame_id>
+            <camera>
+                <horizontal_fov>1.51843645</horizontal_fov>
+                <image>
+                    <format>R_FLOAT32</format>
+                    <width>1280</width>
+                    <height>720</height>
+                </image>
+                <clip>
+                    <near>0.28</near>
+                    <far>10</far>
+                </clip>
+            </camera>
+            <plugin filename="libgz-sim8-sensors-system.so" name="gz::sim::v8::systems::Sensors"></plugin>
+        </sensor>
+    </gazebo>
+
+    <gazebo reference="oak_color_frame">
+        <sensor name="oak_color" type="camera">
+            <pose> 0 0 0 0 0 0 </pose>
+            <topic>oak/color</topic>
+            <update_rate>30</update_rate>
+            <always_on>1</always_on>
+            <gz_frame_id>oak_color_optical_frame</gz_frame_id>
+            <camera>
+                <horizontal_fov>1.51843645</horizontal_fov>
+                <image>
+                    <format>R8G8B8</format>
+                    <width>1280</width>
+                    <height>720</height>
+                </image>
+                <clip>
+                    <near>0.05</near>
+                    <far>100</far>
+                </clip>
+            </camera>
+        </sensor>
+    </gazebo>
+
+    <joint name="laser_joint" type="fixed">
+        <parent link="base_footprint"/>
+        <child link="laser_frame"/>
+        <origin xyz="0 0.2 0.75" rpy="0 0 0"/>
+    </joint>
+
+    <link name="laser_frame">
+        <visual>
+            <geometry>
+                <cylinder radius="0.0385" length="0.0385"/>
+            </geometry>
+        </visual>
+        <collision>
+            <geometry>
+                <cylinder radius="0.0385" length="0.0385"/>
+            </geometry>
+        </collision>
+
+        <inertial>
+            <origin xyz="-0.0368571 0.291615 0.214315" rpy="0 0 0"/>
+            <mass value="0.19"/>
+            <inertia ixx="3.60749" ixy="0" ixz="0" iyy="1.28472" iyz="0.159952" izz="3.84124"/>
+        </inertial>
+    </link>
+
+    <gazebo reference="laser_frame">
+        <sensor name="laser" type="gpu_lidar">
+            <pose> 0 0 0 0 0 0 </pose>
+            <visualize>true</visualize>
+            <update_rate>10</update_rate>
+            <lidar> 
+                <scan>
+                    <horizontal>
+                        <samples>3200</samples>
+                        <min_angle>-3.14</min_angle>
+                        <max_angle>3.14</max_angle>
+                    </horizontal>
+                </scan>
+                <range>
+                    <min>0.05</min>
+                    <max>30</max>
+                </range>
+            </lidar>
+            <topic>scan</topic>
+            <gz_frame_id>laser_frame</gz_frame_id>
+        </sensor>
+    </gazebo>
 
 </robot>