diff --git a/.devcontainer/cyclonedds.xml b/.devcontainer/cyclonedds.xml new file mode 100644 index 00000000..5e4e7a4f --- /dev/null +++ b/.devcontainer/cyclonedds.xml @@ -0,0 +1,8 @@ + + + + + 512 + + + diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 88304096..0c5d8bee 100644 --- a/.devcontainer/devcontainer.json +++ b/.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": [ diff --git a/.devcontainer/start-gazebo-vnc.sh b/.devcontainer/start-gazebo-vnc.sh index fdefe2c5..50a516d4 100755 --- a/.devcontainer/start-gazebo-vnc.sh +++ b/.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 "" diff --git a/docs/MACOS_Gazebo_running.md b/docs/MACOS_Gazebo_running.md index d7791aae..f27ed90e 100644 --- a/docs/MACOS_Gazebo_running.md +++ b/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`. --- diff --git a/docs/RUN_NAV2.md b/docs/RUN_NAV2.md new file mode 100644 index 00000000..089219a0 --- /dev/null +++ b/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 } + } + } +}" +``` + diff --git a/src/igvc_bringup/launch/bringup.launch.py b/src/igvc_bringup/launch/bringup.launch.py index 8bbb70c7..d401f92b 100644 --- a/src/igvc_bringup/launch/bringup.launch.py +++ b/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 diff --git a/src/igvc_description/launch/publisher.launch.py b/src/igvc_description/launch/publisher.launch.py index bee0e333..f6ba3c63 100644 --- a/src/igvc_description/launch/publisher.launch.py +++ b/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 diff --git a/src/igvc_description/rviz/rviz_settings.rviz b/src/igvc_description/rviz/rviz_settings.rviz index 2db5104b..56da1f1a 100644 --- a/src/igvc_description/rviz/rviz_settings.rviz +++ b/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 diff --git a/src/igvc_description/urdf/sensors.urdf.xacro b/src/igvc_description/urdf/sensors.urdf.xacro index d621f7f3..66043aa7 100644 --- a/src/igvc_description/urdf/sensors.urdf.xacro +++ b/src/igvc_description/urdf/sensors.urdf.xacro @@ -1,9 +1,115 @@ + + + + + + + + + + + + + + + + + + - - - - + + + 0 0 0 0 0 0 + oak/depth + 30 + 1 + oak_color_optical_frame + + 1.51843645 + + R_FLOAT32 + 1280 + 720 + + + 0.28 + 10 + + + + + + + + + 0 0 0 0 0 0 + oak/color + 30 + 1 + oak_color_optical_frame + + 1.51843645 + + R8G8B8 + 1280 + 720 + + + 0.05 + 100 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0 0 0 0 0 0 + true + 10 + + + + 3200 + -3.14 + 3.14 + + + + 0.05 + 30 + + + scan + laser_frame + + \ No newline at end of file diff --git a/src/igvc_nav/CMakeLists.txt b/src/igvc_nav/CMakeLists.txt index 3ce78873..0631d478 100644 --- a/src/igvc_nav/CMakeLists.txt +++ b/src/igvc_nav/CMakeLists.txt @@ -7,6 +7,15 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) + +add_executable(cmd_vel_bridge src/cmd_vel_bridge.cpp) +ament_target_dependencies(cmd_vel_bridge rclcpp geometry_msgs) + +install( + TARGETS cmd_vel_bridge + DESTINATION lib/${PROJECT_NAME}) install( DIRECTORY launch diff --git a/src/igvc_nav/config/nav2_params.yaml b/src/igvc_nav/config/nav2_params.yaml index 22e5f6db..f15da4dc 100644 --- a/src/igvc_nav/config/nav2_params.yaml +++ b/src/igvc_nav/config/nav2_params.yaml @@ -1,5 +1,6 @@ amcl: ros__parameters: + enable_stamped_cmd_vel: true alpha1: 0.2 alpha2: 0.2 alpha3: 0.2 @@ -40,8 +41,9 @@ amcl: bt_navigator: ros__parameters: + enable_stamped_cmd_vel: true global_frame: map - robot_base_frame: base_link + robot_base_frame: base_footprint odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 @@ -67,6 +69,7 @@ bt_navigator: controller_server: ros__parameters: + enable_stamped_cmd_vel: true controller_frequency: 20.0 costmap_update_timeout: 0.30 min_x_velocity_threshold: 0.5 @@ -192,10 +195,11 @@ controller_server: local_costmap: local_costmap: ros__parameters: + enable_stamped_cmd_vel: true update_frequency: 5.0 publish_frequency: 2.0 global_frame: odom - robot_base_frame: base_link + robot_base_frame: base_footprint rolling_window: true width: 3 height: 3 @@ -234,10 +238,11 @@ local_costmap: global_costmap: global_costmap: ros__parameters: + enable_stamped_cmd_vel: true update_frequency: 1.0 publish_frequency: 1.0 global_frame: map - robot_base_frame: base_link + robot_base_frame: base_footprint robot_radius: 0.22 resolution: 0.05 track_unknown_space: true @@ -245,13 +250,13 @@ global_costmap: obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True - observation_sources: scan - scan: - topic: /scan + observation_sources: rtabmap + rtabmap: + topic: /rtabmap/odom_local_scan_map max_obstacle_height: 2.0 clearing: True marking: True - data_type: "LaserScan" + data_type: "PointCloud2" raytrace_max_range: 3.0 raytrace_min_range: 0.0 obstacle_max_range: 2.5 @@ -274,6 +279,7 @@ global_costmap: map_saver: ros__parameters: + enable_stamped_cmd_vel: true save_map_timeout: 5.0 free_thresh_default: 0.25 occupied_thresh_default: 0.65 @@ -281,6 +287,7 @@ map_saver: planner_server: ros__parameters: + enable_stamped_cmd_vel: true expected_planner_frequency: 20.0 planner_plugins: ["GridBased"] costmap_update_timeout: 1.0 @@ -292,6 +299,7 @@ planner_server: smoother_server: ros__parameters: + enable_stamped_cmd_vel: true smoother_plugins: ["simple_smoother"] simple_smoother: plugin: "nav2_smoother::SimpleSmoother" @@ -301,6 +309,7 @@ smoother_server: behavior_server: ros__parameters: + enable_stamped_cmd_vel: true local_costmap_topic: local_costmap/costmap_raw global_costmap_topic: global_costmap/costmap_raw local_footprint_topic: local_costmap/published_footprint @@ -319,7 +328,7 @@ behavior_server: plugin: "nav2_behaviors::AssistedTeleop" local_frame: odom global_frame: map - robot_base_frame: base_link + robot_base_frame: base_footprint transform_tolerance: 0.1 simulate_ahead_time: 2.0 max_rotational_vel: 1.0 @@ -328,6 +337,7 @@ behavior_server: waypoint_follower: ros__parameters: + enable_stamped_cmd_vel: true loop_rate: 20 stop_on_failure: false action_server_result_timeout: 900.0 @@ -339,6 +349,7 @@ waypoint_follower: velocity_smoother: ros__parameters: + enable_stamped_cmd_vel: true smoothing_frequency: 20.0 scale_velocities: False feedback: "OPEN_LOOP" @@ -350,13 +361,18 @@ velocity_smoother: odom_duration: 0.1 deadband_velocity: [0.5, 0.0, 0.5] velocity_timeout: 1.0 + # TB4-style pipeline: Nav2 -> smoother -> collision monitor + cmd_vel_topic: "cmd_vel_nav" + smoothed_cmd_vel_topic: "cmd_vel_smoothed" collision_monitor: ros__parameters: + enable_stamped_cmd_vel: true base_frame_id: "base_footprint" odom_frame_id: "odom" + # Take smoothed cmd_vel and send to the robot controller cmd_vel_in_topic: "cmd_vel_smoothed" - cmd_vel_out_topic: "cmd_vel" + cmd_vel_out_topic: "/bot_drive_controller/cmd_vel" state_topic: "collision_monitor_state" transform_tolerance: 0.2 source_timeout: 1.0 @@ -384,6 +400,7 @@ collision_monitor: docking_server: ros__parameters: + enable_stamped_cmd_vel: true controller_frequency: 50.0 initial_perception_timeout: 5.0 wait_charge_timeout: 5.0 @@ -391,7 +408,7 @@ docking_server: undock_linear_tolerance: 0.05 undock_angular_tolerance: 0.1 max_retries: 3 - base_frame: "base_link" + base_frame: "base_footprint" fixed_frame: "odom" dock_backwards: false dock_prestaging_tolerance: 0.5 @@ -437,6 +454,7 @@ docking_server: loopback_simulator: ros__parameters: + enable_stamped_cmd_vel: true base_frame_id: "base_footprint" odom_frame_id: "odom" map_frame_id: "map" diff --git a/src/igvc_nav/launch/igvc_nav.launch.py b/src/igvc_nav/launch/igvc_nav.launch.py index 221c536c..349e3c41 100644 --- a/src/igvc_nav/launch/igvc_nav.launch.py +++ b/src/igvc_nav/launch/igvc_nav.launch.py @@ -3,8 +3,10 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node def generate_launch_description(): nav2_bringup_package = get_package_share_directory('nav2_bringup') @@ -12,14 +14,29 @@ def generate_launch_description(): nav2_launch_path = os.path.join(nav2_bringup_package , 'launch', 'navigation_launch.py') config_path = os.path.join(igvc_nav_package, 'config', 'nav2_params.yaml') + + use_sim_time = LaunchConfiguration('use_sim_time') nav2 = IncludeLaunchDescription( PythonLaunchDescriptionSource(nav2_launch_path), launch_arguments={ - 'params_file' : config_path + 'params_file' : config_path, + 'use_sim_time' : use_sim_time, }.items() + ) return LaunchDescription([ - nav2 - ]) \ No newline at end of file + DeclareLaunchArgument( + 'use_sim_time', + default_value='true' + ), + + nav2, + Node( + package='igvc_nav', + executable='cmd_vel_bridge', + name='cmd_vel_bridge', + output='screen' + ) + ]) \ No newline at end of file diff --git a/src/igvc_nav/package.xml b/src/igvc_nav/package.xml index 4b915d5e..54a29bf6 100644 --- a/src/igvc_nav/package.xml +++ b/src/igvc_nav/package.xml @@ -14,6 +14,8 @@ nav2_bringup navigation2 + rclcpp + geometry_msgs ament_cmake diff --git a/src/igvc_nav/src/cmd_vel_bridge.cpp b/src/igvc_nav/src/cmd_vel_bridge.cpp new file mode 100644 index 00000000..475d1d0b --- /dev/null +++ b/src/igvc_nav/src/cmd_vel_bridge.cpp @@ -0,0 +1,33 @@ +#include +#include + +class CmdVelBridge : public rclcpp::Node +{ +public: + CmdVelBridge() + : Node("cmd_vel_bridge") + { + using geometry_msgs::msg::TwistStamped; + + pub_ = this->create_publisher("/bot_drive_controller/cmd_vel", 10); + sub_ = this->create_subscription( + "/cmd_vel_nav", 10, + [this](const TwistStamped::SharedPtr msg) + { + pub_->publish(*msg); + }); + } + +private: + rclcpp::Subscription::SharedPtr sub_; + rclcpp::Publisher::SharedPtr pub_; +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} +