diff --git a/.config/.yamllint.yaml b/.config/.yamllint.yaml index 3dc9900..9ac8c69 100644 --- a/.config/.yamllint.yaml +++ b/.config/.yamllint.yaml @@ -5,8 +5,13 @@ rules: line-length: max: 256 commas: false + document-start: disable + comments: + min-spaces-from-content: 1 + new-lines: disable truthy: ignore: | .github/workflows/build_base_images.yml .github/workflows/build_and_unitest.yml + .github/workflows/pre-commit.yml diff --git a/.gitignore b/.gitignore index 6b05ebb..ce4191f 100644 --- a/.gitignore +++ b/.gitignore @@ -6,3 +6,6 @@ __pycache__ watod-config.local.sh **/.DS_Store draft_* +*copy* +camera-info.txt +src/gazebo/launch/MR24-DT-A00.00 - URDF/ diff --git a/README.md b/README.md index 48985b6..cfc8fd0 100644 --- a/README.md +++ b/README.md @@ -8,3 +8,62 @@ These steps are to setup the repo to work on your own PC. We utilize docker to e 1. This repo is supported on Linux Ubuntu >= 22.04, Windows (WSL), and MacOS. This is standard practice that roboticists can't get around. To setup, you can either setup an [Ubuntu Virtual Machine](https://ubuntu.com/tutorials/how-to-run-ubuntu-desktop-on-a-virtual-machine-using-virtualbox#1-overview), setting up [WSL](https://learn.microsoft.com/en-us/windows/wsl/install), or setting up your computer to [dual boot](https://opensource.com/article/18/5/dual-boot-linux). You can find online resources for all three approaches. 2. Once inside Linux, [Download Docker Engine using the `apt` repository](https://docs.docker.com/engine/install/ubuntu/#install-using-the-repository) 3. You're all set! You can visit the [WATO Wiki](https://wiki.watonomous.ca/autonomous_software_general/monorepo_infrastructure) for more documentation on the WATO infrastructure. + +## Demo +https://github.com/user-attachments/assets/d4e821cd-8db7-4176-9e2e-1d9c62aab23d + +![Demo Screenshot](assets/demos/demo_nov30.png) + +## Simulation Environment + +We use Ignition Gazebo for physics simulation. The main world file is `robot_env.sdf`, which defines the entire simulation scene. + +### Robot Model (SDF) +A basic model of the rover is defined directly in the SDF with a differential drive base. Key components: +- **Chassis**: 2.0m x 1.0m x 0.5m box with inertial properties +- **Wheels**: Four cylindrical wheels with revolute joints +- **IMU**: Mounted on chassis, publishes to `/imu` +- **RGBD Camera**: Simulated RealSense D435 mounted on the front + +### Depth Camera Simulation +The depth camera is configured as an `rgbd_camera` sensor with realistic FOV, intrinsics, etc. + +It publishes: +- `/camera/image` ([Image](https://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/Image.html)) - RGB image (used by YOLO object detector) +- `/camera/points` ([PointCloud2](https://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/PointCloud2.html)) - 3D point cloud (used by costmap) + +### Mars Environment (URDF) +The environment is loaded from `mars_env.urdf`, which includes meshes for the mars terrain, mallet, and water bottle. + +The terrain has collision geometry so the rover can't drive through obstacles. + +### ROS2 Bridge +The `ros_gz_bridge` translates Ignition messages to ROS2 topics, bridging `/cmd_vel`, `/imu`, camera streams, and TF transforms. This way, our ROS2 autonomy stack interface seamlessly with the Gazebo sim. + +## Autonomy Architecture + +### Sensor Simulation +- **gps_sim**: Simulates GPS measurements by reading ground truth transforms from the simulator and injecting realistic noise and dropouts. Publishes [NavSatFix](https://docs.ros.org/en/kinetic/api/sensor_msgs/html/msg/NavSatFix.html) messages. +- **imu_sim**: Adds sensor noise and bias to clean IMU data to mimic real-world sensors. Takes ground truth IMU and outputs noisy orientation and angular velocity. + +### Localization +- **localization**: Runs an Extended Kalman Filter (EKF) to fuse (simulated) noisy GPS and IMU data into a smooth, filtered odometry estimate. Outputs robot pose for downstream modules. + +### Perception & Mapping +- **costmap**: Converts RGBD camera point clouds into a local 2D occupancy grid centered on the robot. Marks obstacles and inflates them for safety margins. +- **map_memory**: Stitches together local costmaps into a persistent global map as the rover explores. Only updates when the robot moves beyond a threshold distance to reduce computational load. +- **yolo_inference**: Runs YOLOv8 object detection (ONNX runtime) on camera images to detect objects like bottles and mallets. Publishes annotated images with bounding boxes. + +### Planning & Control +- **planner**: Implements A* search on the global occupancy grid. Takes the current robot pose and a goal point, outputs a collision-free path. Replans when new goals arrive. +- **control**: Pure pursuit controller that tracks the planned path. Uses a PID loop (configurable Kp, Ki, Kd gains) to compute steering corrections based on cross-track error. Outputs angular velocity to steer toward the target waypoint while maintaining constant forward velocity, publishing [Twist](https://docs.ros.org/en/jade/api/geometry_msgs/html/msg/Twist.html) commands to `/cmd_vel`. + +### Data Flow +The pipeline runs as follows: +- Sensor sims generate noisy data +- Localization fuses it into an estimate of the robot's position (odometry) +- Costmap builds local obstacles +- Map memory accumulates into global map +- Planner finds path to goal +- Control executes path +- Motor commands actuate the rover diff --git a/assets/demos/demo_nov30.mp4 b/assets/demos/demo_nov30.mp4 new file mode 100644 index 0000000..5b803a7 Binary files /dev/null and b/assets/demos/demo_nov30.mp4 differ diff --git a/assets/demos/demo_nov30.png b/assets/demos/demo_nov30.png new file mode 100644 index 0000000..59be99a Binary files /dev/null and b/assets/demos/demo_nov30.png differ diff --git a/docker/gazebo/gazeboserver.Dockerfile b/docker/gazebo/gazeboserver.Dockerfile index bccbd01..e3dcdd6 100644 --- a/docker/gazebo/gazeboserver.Dockerfile +++ b/docker/gazebo/gazeboserver.Dockerfile @@ -15,10 +15,10 @@ RUN apt-get update && apt-get install -y --no-install-recommends curl \ # Scan for rosdeps SHELL ["/bin/bash", "-o", "pipefail", "-c"] RUN apt-get -qq update && rosdep update && \ - (rosdep install --from-paths . --ignore-src -r -s \ - | grep 'apt-get install' \ + (rosdep install --from-paths . --ignore-src -r -s || true) \ + | (grep 'apt-get install' || true) \ | awk '{print $3}' \ - | sort > /tmp/colcon_install_list || echo "# No additional dependencies needed" > /tmp/colcon_install_list) + | sort > /tmp/colcon_install_list ################################# Dependencies ################################ FROM ${BASE_IMAGE} AS dependencies diff --git a/docker/robot/robot.Dockerfile b/docker/robot/robot.Dockerfile index 454ab5b..f11fc60 100644 --- a/docker/robot/robot.Dockerfile +++ b/docker/robot/robot.Dockerfile @@ -10,22 +10,25 @@ RUN apt-get update && apt-get install -y --no-install-recommends curl \ && rm -rf /var/lib/apt/lists/* # Copy in source code -COPY src/robot/yolo_inference ./yolo_inference -COPY src/robot/object_detection object_detection +# COPY src/robot/yolo_inference ./yolo_inference +# COPY src/robot/object_detection object_detection COPY src/robot/odometry_spoof odometry_spoof +COPY src/robot/gps_sim gps_sim +COPY src/robot/imu_sim imu_sim +COPY src/robot/localization localization +COPY src/robot/costmap costmap +COPY src/robot/map_memory map_memory +COPY src/robot/planner planner +COPY src/robot/control control COPY src/robot/bringup_robot bringup_robot -COPY src/robot/camera_fallback camera_fallback -COPY src/robot/arcade_driver arcade_driver -COPY src/robot/motor_speed_controller motor_speed_controller -COPY src/wato_msgs/drivetrain_msgs drivetrain_msgs # Scan for rosdeps SHELL ["/bin/bash", "-o", "pipefail", "-c"] RUN apt-get -qq update && rosdep update && \ - (rosdep install --from-paths . --ignore-src -r -s \ - | grep 'apt-get install' \ - | awk '{print $3}' \ - | sort > /tmp/colcon_install_list || echo "# No additional dependencies needed" > /tmp/colcon_install_list) + (rosdep install --from-paths . --ignore-src -r -s || true) \ + | (grep 'apt-get install' || true) \ + | awk '{print $3}' \ + | sort > /tmp/colcon_install_list || echo "# No additional dependencies needed" > /tmp/colcon_install_list ################################# Dependencies ################################ FROM ${BASE_IMAGE} AS dependencies @@ -50,8 +53,8 @@ RUN apt-get -qq update && \ rm -rf /var/lib/apt/lists/* # Copy in source code from source stage -WORKDIR ${AMENT_WS}/src -COPY src/robot/object_detection object_detection +# WORKDIR ${AMENT_WS}/src +# COPY src/robot/object_detection object_detection WORKDIR ${AMENT_WS} COPY --from=source ${AMENT_WS}/src src diff --git a/docker/vis_tools/foxglove.Dockerfile b/docker/vis_tools/foxglove.Dockerfile index 6589b13..4a07203 100644 --- a/docker/vis_tools/foxglove.Dockerfile +++ b/docker/vis_tools/foxglove.Dockerfile @@ -15,10 +15,10 @@ RUN apt-get update && apt-get install -y --no-install-recommends curl \ # Scan for rosdeps SHELL ["/bin/bash", "-o", "pipefail", "-c"] RUN apt-get -qq update && rosdep update && \ - (rosdep install --from-paths . --ignore-src -r -s \ - | grep 'apt-get install' \ + (rosdep install --from-paths . --ignore-src -r -s || true) \ + | (grep 'apt-get install' || true) \ | awk '{print $3}' \ - | sort > /tmp/colcon_install_list || echo "# No additional dependencies needed" > /tmp/colcon_install_list) + | sort > /tmp/colcon_install_list ################################# Dependencies ################################ FROM ${BASE_IMAGE} AS dependencies @@ -36,7 +36,6 @@ RUN apt-get update && apt-get install -y --no-install-recommends lsb-release sof apt-add-repository universe && \ rm -rf /var/lib/apt/lists/* -# Install Dependencies # Install Dependencies RUN apt-get update && \ apt-get install -y --no-install-recommends \ diff --git a/how 0bb5293d52599317a3e300c55897e34a48e5e289 --numstat b/how 0bb5293d52599317a3e300c55897e34a48e5e289 --numstat new file mode 100644 index 0000000..ea617de --- /dev/null +++ b/how 0bb5293d52599317a3e300c55897e34a48e5e289 --numstat @@ -0,0 +1,65 @@ +commit 0bb5293d52599317a3e300c55897e34a48e5e289 +Author: Barry Zhang <45883553+bluebarryz@users.noreply.github.com> +Date: Wed Jan 7 19:46:46 2026 -0500 + + refactor perception and integrate costmap, planner, and control modules + + .gitignore | 3 + + docker/gazebo/gazeboserver.Dockerfile | 10 +- + docker/robot/robot.Dockerfile | 29 +- + docker/vis_tools/foxglove.Dockerfile | 11 +- + modules/docker-compose.robot.yaml | 13 +- + src/gazebo/launch/env.urdf | 357 ++++++----- + src/gazebo/launch/mr24/robot.urdf | 365 +++++++++++ + src/gazebo/launch/robot_env.sdf | 668 +++++++++------------ + src/gazebo/launch/sim.launch.py | 60 +- + src/robot/bringup_robot/launch/robot.launch.py | 206 +++++-- + src/robot/bringup_robot/package.xml | 14 +- + src/robot/control/CMakeLists.txt | 7 +- + src/robot/control/config/params.yaml | 14 +- + src/robot/control/include/control_core.hpp | 47 ++ + src/robot/control/include/control_node.hpp | 63 ++ + src/robot/control/package.xml | 3 + + src/robot/control/src/control_core.cpp | 146 ++++- + src/robot/control/src/control_node.cpp | 101 +++- + src/robot/costmap/CMakeLists.txt | 5 +- + src/robot/costmap/config/params.yaml | 20 +- + src/robot/costmap/include/costmap_core.hpp | 50 ++ + src/robot/costmap/include/costmap_node.hpp | 46 ++ + src/robot/costmap/package.xml | 3 + + src/robot/costmap/src/costmap_core.cpp | 170 +++++- + src/robot/costmap/src/costmap_node.cpp | 88 ++- + src/robot/gps_sim/CMakeLists.txt | 47 ++ + src/robot/gps_sim/config/params.yaml | 13 + + src/robot/gps_sim/include/gps_sim_node.hpp | 46 ++ + src/robot/gps_sim/package.xml | 28 + + src/robot/gps_sim/src/gps_sim_node.cpp | 134 +++++ + src/robot/imu_sim/CMakeLists.txt | 41 ++ + src/robot/imu_sim/config/params.yaml | 18 + + src/robot/imu_sim/include/imu_sim_node.hpp | 53 ++ + src/robot/imu_sim/package.xml | 25 + + src/robot/imu_sim/src/imu_sim_node.cpp | 171 ++++++ + src/robot/localization/CMakeLists.txt | 56 ++ + src/robot/localization/config/params.yaml | 25 + + .../localization/include/localization_core.hpp | 106 ++++ + .../localization/include/localization_node.hpp | 73 +++ + src/robot/localization/package.xml | 29 + + src/robot/localization/src/localization_core.cpp | 501 ++++++++++++++++ + src/robot/localization/src/localization_node.cpp | 225 +++++++ + src/robot/map_memory/CMakeLists.txt | 8 +- + src/robot/map_memory/config/params.yaml | 26 +- + src/robot/map_memory/include/map_memory_core.hpp | 38 ++ + src/robot/map_memory/include/map_memory_node.hpp | 63 ++ + src/robot/map_memory/package.xml | 3 + + src/robot/map_memory/src/map_memory_core.cpp | 110 +++- + src/robot/map_memory/src/map_memory_node.cpp | 141 ++++- + .../odometry_spoof/include/odometry_spoof.hpp | 40 ++ + src/robot/odometry_spoof/src/odometry_spoof.cpp | 92 +-- + src/robot/planner/CMakeLists.txt | 4 +- + src/robot/planner/config/params.yaml | 16 +- + src/robot/planner/include/planner_core.hpp | 125 +++- + src/robot/planner/include/planner_node.hpp | 70 ++- + src/robot/planner/package.xml | 2 + + src/robot/planner/src/planner_core.cpp | 262 +++++++- + src/robot/planner/src/planner_node.cpp | 186 +++++- + 58 files changed, 4503 insertions(+), 773 deletions(-) diff --git a/modules/docker-compose.robot.yaml b/modules/docker-compose.robot.yaml index 20e37a8..8e6e7b9 100644 --- a/modules/docker-compose.robot.yaml +++ b/modules/docker-compose.robot.yaml @@ -12,11 +12,6 @@ services: profiles: - deploy command: /bin/bash -c "ros2 launch bringup_robot robot.launch.py" - devices: - - /dev/bus/usb:/dev/bus/usb - volumes: - - /dev:/dev - privileged: true robot_dev: build: *robot_build @@ -26,7 +21,3 @@ services: - develop volumes: - ${MONO_DIR}/src/robot:/root/ament_ws/src - - /dev:/dev - devices: - - /dev/bus/usb:/dev/bus/usb - privileged: true diff --git a/src/gazebo/launch/env.urdf b/src/gazebo/launch/env.urdf index 73d6a13..8a9b0b8 100644 --- a/src/gazebo/launch/env.urdf +++ b/src/gazebo/launch/env.urdf @@ -1,147 +1,210 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + 0.0 0.0 1.0 1 + 0.0 0.0 1.0 1 + 0.0 0.0 1.0 1 + + + + + + + + + + + + + + + + + 0.0 0.0 1.0 1 + 0.0 0.0 1.0 1 + 0.0 0.0 1.0 1 + + + + + + + + + + + + + + + + + 0.0 0.0 1.0 1 + 0.0 0.0 1.0 1 + 0.0 0.0 1.0 1 + + + + + + + + + + + + + + + + + 0.0 0.0 1.0 1 + 0.0 0.0 1.0 1 + 0.0 0.0 1.0 1 + + + + + + + + + + + + + + + + + 0.0 0.0 1.0 1 + 0.0 0.0 1.0 1 + 0.0 0.0 1.0 1 + + + + + + + + + + + + + + + + + 0.0 0.0 1.0 1 + 0.0 0.0 1.0 1 + 0.0 0.0 1.0 1 + + + + + + + + + + + + + + + + + 0.0 0.0 1.0 1 + 0.0 0.0 1.0 1 + 0.0 0.0 1.0 1 + + + + + + + + + + + + + + + + + 0.0 0.0 1.0 1 + 0.0 0.0 1.0 1 + 0.0 0.0 1.0 1 + + + + + + + + + + + + + + + + + 0.0 0.0 1.0 1 + 0.0 0.0 1.0 1 + 0.0 0.0 1.0 1 + + + + + + + + + + + + + + + + + 0.0 0.0 1.0 1 + 0.0 0.0 1.0 1 + 0.0 0.0 1.0 1 + + + + + + + + + + + + + + + + + 0.0 0.0 1.0 1 + 0.0 0.0 1.0 1 + 0.0 0.0 1.0 1 + + + diff --git a/src/gazebo/launch/mr24/robot.urdf b/src/gazebo/launch/mr24/robot.urdf new file mode 100644 index 0000000..e217949 --- /dev/null +++ b/src/gazebo/launch/mr24/robot.urdf @@ -0,0 +1,365 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/gazebo/launch/robot_env.sdf b/src/gazebo/launch/robot_env.sdf index e7a6961..8b0d497 100644 --- a/src/gazebo/launch/robot_env.sdf +++ b/src/gazebo/launch/robot_env.sdf @@ -1,4 +1,4 @@ - + @@ -18,263 +18,263 @@ name="ignition::gazebo::systems::SceneBroadcaster"> + name="ignition::gazebo::systems::Imu"> + name="ignition::gazebo::systems::Sensors"> ogre2 + name="ignition::gazebo::systems::Contact"> - - - - 3D View - false - docked - - - ogre2 - scene - 0.4 0.4 0.4 - 0.8 0.8 0.8 - -6 0 6 0 0.5 0 - - 0.25 - 25000 - - - - - - - floating - 5 - 5 - false - - - - - false - 5 - 5 - floating - false - - - - - false - 5 - 5 - floating - false - - - - - false - 5 - 5 - floating - false - - - - - false - 5 - 5 - floating - false - - - - - - - - - false - 5 - 5 - floating - false - - - - - false - 5 - 5 - floating - false - - - - - - - - - - false - 5 - 5 - floating - false - - - - - - - World control - false - false - 72 - 121 - 1 - - floating - - - - - - - true - true - true - true - - - - - - - World stats - false - false - 110 - 290 - 1 - - floating - - - - - - - true - true - true - true - - - - - - false - 0 - 0 - 250 - 50 - floating - false - #666666 - - - - - - - false - 250 - 0 - 150 - 50 - floating - false - #666666 - - - - - - - false - 0 - 50 - 250 - 50 - floating - false - #777777 - - - - - - - false - 250 - 50 - 50 - 50 - floating - false - #777777 - - - - - - - false - 300 - 50 - 50 - 50 - floating - false - #777777 - - - - true - false - 4000000 - - - - - - - docked_collapsed - - - - - - - docked_collapsed - - - - - - - docked_collapsed - - + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + 0.25 + 25000 + + + + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + + + + + false + 5 + 5 + floating + false + + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + true + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + + + + + false + 0 + 0 + 250 + 50 + floating + false + #666666 + + + + + + + false + 250 + 0 + 150 + 50 + floating + false + #666666 + + + + + + + false + 0 + 50 + 250 + 50 + floating + false + #777777 + + + + + + + false + 250 + 50 + 50 + 50 + floating + false + #777777 + + + + + + + false + 300 + 50 + 50 + 50 + floating + false + #777777 + + + + true + false + 4000000 + + + + + + + docked_collapsed + + + + + + + docked_collapsed + + + + + + + docked_collapsed + + - + @@ -292,6 +292,8 @@ -0.5 0.1 -0.9 + + true @@ -376,32 +378,24 @@ + - -6 0 3 0 0 0 + -6 0 1 0 0 0 - - - - - 0.5 0 0.5 0 0 0.7592182246 - - 0.5 0 0.5 0 0 -0.7592182246 + + + 0.8 0 0.5 0 0 0 - 0.5 0 0.4 0 0 0 - + 1.14395 0.126164 @@ -415,20 +409,17 @@ - 2.0 1.0 0.5 + 2.0 1.0 0.5 - + 0.0 0.0 1.0 1 0.0 0.0 1.0 1 0.0 0.0 1.0 1 - + 2.0 1.0 0.5 @@ -441,11 +432,9 @@ true imu - - - - 0 0 0 0 0 0 + + 0 0 0 0 0 0 30 1.51843645 @@ -488,64 +477,13 @@ 0.007 - /sim/realsense1/depth - - - - 0 0 0 0 0 0 - 30 - - 1.51843645 - - 320 - 240 - R8G8B8 - - - - 303.9691 - 303.7607 - 163.6526 - 119.2695 - 0 - - - - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 -
0.5 0.5
- - - 0.1 - 100 - - - - - - gaussian - 0 - 0.007 - - - /sim/realsense2/depth + camera + - - - - + - -0.5 0.6 0 -1.5707 0 0 + -0.5 0.6 0 -1.5707 0 0 2 @@ -580,11 +518,9 @@ - + - -0.5 -0.6 0 -1.5707 0 0 + -0.5 -0.6 0 -1.5707 0 0 1 @@ -623,10 +559,9 @@ 0.8 0 -0.2 0 0 0 - + - + 1 @@ -659,50 +594,40 @@ - - - + + + chassis left_wheel - 0 1 0 + 0 1 0 - -1.79769e+308 - 1.79769e+308 + -1.79769e+308 + 1.79769e+308 - + chassis right_wheel 0 1 0 - -1.79769e+308 - 1.79769e+308 + -1.79769e+308 + 1.79769e+308 - + chassis caster - + @@ -721,8 +646,7 @@ true false false - true + true true true 1000 @@ -731,7 +655,7 @@ + name="ignition::gazebo::systems::TriggeredPublisher"> 16777235 @@ -742,7 +666,7 @@ + name="ignition::gazebo::systems::TriggeredPublisher"> 16777237 @@ -753,7 +677,7 @@ + name="ignition::gazebo::systems::TriggeredPublisher"> 16777236 @@ -762,10 +686,9 @@ - + + name="ignition::gazebo::systems::TriggeredPublisher"> 16777234 @@ -774,10 +697,9 @@ - + + name="ignition::gazebo::systems::TriggeredPublisher"> data: true diff --git a/src/gazebo/launch/sim.launch.py b/src/gazebo/launch/sim.launch.py index a482542..243519b 100644 --- a/src/gazebo/launch/sim.launch.py +++ b/src/gazebo/launch/sim.launch.py @@ -39,16 +39,13 @@ def generate_launch_description(): "/model/robot/pose@tf2_msgs/msg/TFMessage@ignition.msgs.Pose_V", "/model/robot/pose_static@tf2_msgs/msg/TFMessage@ignition.msgs.Pose_V", "/cmd_vel@geometry_msgs/msg/Twist]ignition.msgs.Twist", - "/cmd_vel_stamped@geometry_msgs/msg/TwistStamped@ignition.msgs.Twist", "/imu@sensor_msgs/msg/Imu@ignition.msgs.IMU", + # '/lidar@sensor_msgs/msg/LaserScan@ignition.msgs.LaserScan', + "/camera/image@sensor_msgs/msg/Image@ignition.msgs.Image", + "/camera/depth_image@sensor_msgs/msg/Image@ignition.msgs.Image", + "/camera/points@sensor_msgs/msg/PointCloud2@ignition.msgs.PointCloudPacked", + "/camera/camera_info@sensor_msgs/msg/CameraInfo@ignition.msgs.CameraInfo", "/model/robot/odometry@nav_msgs/msg/Odometry@gz.msgs.Odometry", - "/sim/realsense1/depth/points@sensor_msgs/msg/PointCloud2@ignition.msgs.PointCloudPacked", - "/sim/realsense1/depth/image@sensor_msgs/msg/Image@ignition.msgs.Image", - "/sim/realsense1/depth/camera_info@sensor_msgs/msg/CameraInfo@ignition.msgs.CameraInfo", - "/sim/realsense2/depth/points@sensor_msgs/msg/PointCloud2@ignition.msgs.PointCloudPacked", - "/sim/realsense2/depth/image@sensor_msgs/msg/Image@ignition.msgs.Image", - "/sim/realsense2/depth/camera_info@sensor_msgs/msg/CameraInfo@ignition.msgs.CameraInfo", - "/tf@tf2_msgs/msg/TFMessage@ignition.msgs.Pose_V", ], parameters=[ {"qos_overrides./model/vehicle_blue.subscriber.reliability": "reliable"} @@ -57,6 +54,7 @@ def generate_launch_description(): remappings=[ ("/model/robot/pose", "/tf"), ("/model/robot/pose_static", "/tf"), + ("/camera/camera_info", "/camera_info"), ], ) diff --git a/src/robot/bringup_robot/launch/robot.launch.py b/src/robot/bringup_robot/launch/robot.launch.py index 3425961..f50c063 100644 --- a/src/robot/bringup_robot/launch/robot.launch.py +++ b/src/robot/bringup_robot/launch/robot.launch.py @@ -13,11 +13,9 @@ # limitations under the License. from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch_ros.actions import Node -from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node import os @@ -25,15 +23,94 @@ def generate_launch_description(): ld = LaunchDescription() # Begin building a launch description - yolo_pkg_share = get_package_share_directory("yolo_inference") - default_model_path = os.path.join(yolo_pkg_share, "models", "yolov8.onnx") + # Shared parameter: odom_topic (used by multiple nodes) + odom_topic_arg = DeclareLaunchArgument( + "odom_topic", + default_value="/odom/filtered", + description="Odometry topic name used by control, map_memory, and planner nodes", + ) + ld.add_action(odom_topic_arg) + + #################### Costmap Node ##################### + costmap_pkg_prefix = get_package_share_directory("costmap") + costmap_param_file = os.path.join(costmap_pkg_prefix, "config", "params.yaml") - model_path_arg = DeclareLaunchArgument( - "model_path", - default_value=default_model_path, - description="Absolute path to the YOLO ONNX/PT weights file", + costmap_param = DeclareLaunchArgument( + "costmap_param_file", + default_value=costmap_param_file, + description="Path to config file for producer node", ) - ld.add_action(model_path_arg) + costmap_node = Node( + package="costmap", + name="costmap_node", + executable="costmap_node", + parameters=[LaunchConfiguration("costmap_param_file")], + ) + ld.add_action(costmap_param) + ld.add_action(costmap_node) + + #################### Map Memory Node ##################### + map_memory_pkg_prefix = get_package_share_directory("map_memory") + map_memory_param_file = os.path.join(map_memory_pkg_prefix, "config", "params.yaml") + + map_memory_param = DeclareLaunchArgument( + "map_memory_param_file", + default_value=map_memory_param_file, + description="Path to config file for producer node", + ) + map_memory_node = Node( + package="map_memory", + name="map_memory_node", + executable="map_memory_node", + parameters=[ + LaunchConfiguration("map_memory_param_file"), + {"odom_topic": LaunchConfiguration("odom_topic")}, + ], + ) + ld.add_action(map_memory_param) + ld.add_action(map_memory_node) + + ##################### Planner Node ##################### + planner_pkg_prefix = get_package_share_directory("planner") + planner_param_file = os.path.join(planner_pkg_prefix, "config", "params.yaml") + + planner_param = DeclareLaunchArgument( + "planner_param_file", + default_value=planner_param_file, + description="Path to config file for producer node", + ) + planner_node = Node( + package="planner", + name="planner_node", + executable="planner_node", + parameters=[ + LaunchConfiguration("planner_param_file"), + {"odom_topic": LaunchConfiguration("odom_topic")}, + ], + ) + ld.add_action(planner_param) + ld.add_action(planner_node) + + ##################### Control Node ##################### + control_pkg_prefix = get_package_share_directory("control") + control_param_file = os.path.join(control_pkg_prefix, "config", "params.yaml") + + control_param = DeclareLaunchArgument( + "control_param_file", + default_value=control_param_file, + description="Path to config file for producer node", + ) + control_node = Node( + package="control", + name="control_node", + executable="control_node", + parameters=[ + LaunchConfiguration("control_param_file"), + {"odom_topic": LaunchConfiguration("odom_topic")}, + ], + ) + ld.add_action(control_param) + ld.add_action(control_node) #################### Odometry Spoof Node ##################### odometry_spoof_node = Node( @@ -43,61 +120,60 @@ def generate_launch_description(): ) ld.add_action(odometry_spoof_node) - #################### RealSense Node ##################### - realsense_launch_file = os.path.join( - get_package_share_directory("realsense2_camera"), "launch", "rs_launch.py" - ) - realsense_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource(realsense_launch_file), - launch_arguments={"pointcloud.enable": "true", "filters": "spatial"}.items(), - ) - ld.add_action(realsense_launch) - - #################### Camera Fallback Node ##################### - camera_fallback_node = Node( - package="camera_fallback", - name="camera_fallback_node", - executable="camera_fallback_node", - ) - ld.add_action(camera_fallback_node) - - #################### ArcadeDriver Node ##################### - arcade_driver_node = Node( - package="arcade_driver", - name="arcade_driver", - executable="arcade_driver", - ) - ld.add_action(arcade_driver_node) - - #################### MotorSpeedController Node ##################### - motor_speed_controller_node = Node( - package="motor_speed_controller", - name="motor_speed_controller", - executable="motor_speed_controller", - ) - ld.add_action(motor_speed_controller_node) - - #################### Object_detection Node ##################### - object_detection_node = Node( - package="object_detection", - name="object_detection_node", - executable="object_detection_node", - ) - ld.add_action(object_detection_node) - - ld.add_action( - Node( - package="yolo_inference", - executable="yolo_inference_node", - name="yolo_inference_node", - output="screen", - parameters=[ - { - "model_path": LaunchConfiguration("model_path"), - "input_size": 640, # TODO: adjust DEPENDING on model needs 416, 512, etc - } - ], - remappings=[("/image", "/sim/realsense1/depth/image")], - ) + #################### GPS Simulation Node ##################### + gps_sim_pkg_prefix = get_package_share_directory("gps_sim") + gps_sim_param_file = os.path.join(gps_sim_pkg_prefix, "config", "params.yaml") + + gps_sim_param = DeclareLaunchArgument( + "gps_sim_param_file", + default_value=gps_sim_param_file, + description="Path to config file for GPS simulation node", + ) + gps_sim_node = Node( + package="gps_sim", + name="gps_sim_node", + executable="gps_sim_node", + parameters=[LaunchConfiguration("gps_sim_param_file")], + ) + ld.add_action(gps_sim_param) + ld.add_action(gps_sim_node) + + #################### IMU Simulation Node ##################### + imu_sim_pkg_prefix = get_package_share_directory("imu_sim") + imu_sim_param_file = os.path.join(imu_sim_pkg_prefix, "config", "params.yaml") + + imu_sim_param = DeclareLaunchArgument( + "imu_sim_param_file", + default_value=imu_sim_param_file, + description="Path to config file for IMU simulation node", ) + imu_sim_node = Node( + package="imu_sim", + name="imu_sim_node", + executable="imu_sim_node", + parameters=[LaunchConfiguration("imu_sim_param_file")], + ) + ld.add_action(imu_sim_param) + ld.add_action(imu_sim_node) + + #################### Localization Node (EKF) ##################### + localization_pkg_prefix = get_package_share_directory("localization") + localization_param_file = os.path.join( + localization_pkg_prefix, "config", "params.yaml" + ) + + localization_param = DeclareLaunchArgument( + "localization_param_file", + default_value=localization_param_file, + description="Path to config file for localization node", + ) + localization_node = Node( + package="localization", + name="localization_node", + executable="localization_node", + parameters=[LaunchConfiguration("localization_param_file")], + ) + ld.add_action(localization_param) + ld.add_action(localization_node) + return ld diff --git a/src/robot/bringup_robot/package.xml b/src/robot/bringup_robot/package.xml index d224537..6d23fdc 100644 --- a/src/robot/bringup_robot/package.xml +++ b/src/robot/bringup_robot/package.xml @@ -10,17 +10,13 @@ ament_cmake odometry_spoof - arcade_driver - motor_speed_controller - realsense2_camera - realsense2_camera_msgs - realsense2_description - camera_fallback - - + control ament_cmake diff --git a/src/robot/control/CMakeLists.txt b/src/robot/control/CMakeLists.txt index dce8810..a4e7494 100644 --- a/src/robot/control/CMakeLists.txt +++ b/src/robot/control/CMakeLists.txt @@ -26,6 +26,9 @@ endif() # Search for dependencies required for building this package find_package(ament_cmake REQUIRED) # ROS2 build tool find_package(rclcpp REQUIRED) # ROS2 C++ package +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(tf2 REQUIRED) # Compiles source files into a library # A library is not executed, instead other executables can link @@ -42,7 +45,9 @@ target_include_directories(control_lib # Add ROS2 dependencies required by package ament_target_dependencies(control_lib rclcpp -) + geometry_msgs + nav_msgs + tf2) # Create ROS2 node executable from source files add_executable(control_node src/control_node.cpp) diff --git a/src/robot/control/config/params.yaml b/src/robot/control/config/params.yaml index dde97a4..4628205 100644 --- a/src/robot/control/config/params.yaml +++ b/src/robot/control/config/params.yaml @@ -1,2 +1,12 @@ -# control_node: -# ros__parameters: +control_node: + ros__parameters: + path_topic: "/path" + # odom_topic: "/odom/filtered" + cmd_vel_topic: "/cmd_vel" + + kp: 1.0 + ki: 0.01 + kd: 0.1 + + max_steering_angle: 1.0 # rad + linear_velocity: 1.0 # m/s diff --git a/src/robot/control/include/control_core.hpp b/src/robot/control/include/control_core.hpp new file mode 100644 index 0000000..34b9fab --- /dev/null +++ b/src/robot/control/include/control_core.hpp @@ -0,0 +1,56 @@ +// Copyright (c) 2025-present WATonomous. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTROL_CORE_HPP_ +#define CONTROL_CORE_HPP_ + +#include "geometry_msgs/msg/twist.hpp" +#include "nav_msgs/msg/path.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace robot +{ + +class ControlCore +{ +public: + explicit ControlCore(const rclcpp::Logger & logger); + + void initControlCore(double kp, double ki, double kd, double max_steering_angle, double linear_velocity); + + void updatePath(nav_msgs::msg::Path path); + + bool isPathEmpty(); + + unsigned int findClosestPoint(double robot_x, double robot_y); + + geometry_msgs::msg::Twist calculateControlCommand(double robot_x, double robot_y, double robot_theta, double dt); + +private: + nav_msgs::msg::Path path_; + rclcpp::Logger logger_; + + double kp_; + double ki_; + double kd_; + double max_steering_angle_; + double linear_velocity_; + + double prev_error_; + double integral_error_; +}; + +} // namespace robot + +#endif diff --git a/src/robot/control/include/control_node.hpp b/src/robot/control/include/control_node.hpp new file mode 100644 index 0000000..afd7a5e --- /dev/null +++ b/src/robot/control/include/control_node.hpp @@ -0,0 +1,79 @@ +// Copyright (c) 2025-present WATonomous. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTROL_NODE_HPP_ +#define CONTROL_NODE_HPP_ + +#include + +#include "control/control_core.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "nav_msgs/msg/path.hpp" +#include "rclcpp/rclcpp.hpp" + +class ControlNode : public rclcpp::Node +{ +public: + ControlNode(); + + // Read and load in ROS2 parameters + void processParameters(); + + // Utility: Convert quaternion to yaw + double quaternionToYaw(double x, double y, double z, double w); + + // Callback for path + void pathCallback(const nav_msgs::msg::Path::SharedPtr msg); + + // Callback for odometry + void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg); + + // Main loop to continuously follow the path + void followPath(); + + // Timer callback + void timerCallback(); + +private: + robot::ControlCore control_; + + // Subscriber and Publisher + rclcpp::Subscription::SharedPtr path_subscriber_; + rclcpp::Subscription::SharedPtr odom_subscriber_; + rclcpp::Publisher::SharedPtr cmd_vel_publisher_; + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + + // Path and robot state + double robot_x_; + double robot_y_; + double robot_theta_; + + // ROS2 params + std::string path_topic_; + std::string odom_topic_; + std::string cmd_vel_topic_; + + int control_period_ms_; + double kp_; + double ki_; + double kd_; + + double max_steering_angle_; + double linear_velocity_; +}; + +#endif diff --git a/src/robot/control/package.xml b/src/robot/control/package.xml index 7fb2721..cc87055 100644 --- a/src/robot/control/package.xml +++ b/src/robot/control/package.xml @@ -10,6 +10,9 @@ ament_cmake rclcpp + geometry_msgs + nav_msgs + tf2 ament_lint_auto ament_lint_common diff --git a/src/robot/control/src/control_core.cpp b/src/robot/control/src/control_core.cpp index 0003719..6f5f7a5 100644 --- a/src/robot/control/src/control_core.cpp +++ b/src/robot/control/src/control_core.cpp @@ -14,11 +14,152 @@ #include "control/control_core.hpp" +#include +#include +#include + namespace robot { ControlCore::ControlCore(const rclcpp::Logger & logger) -: logger_(logger) +: path_(nav_msgs::msg::Path()) +, logger_(logger) +, prev_error_(0.0) +, integral_error_(0.0) {} +void ControlCore::initControlCore(double kp, double ki, double kd, double max_steering_angle, double linear_velocity) +{ + kp_ = kp; + ki_ = ki; + kd_ = kd; + max_steering_angle_ = max_steering_angle; + linear_velocity_ = linear_velocity; +} + +void ControlCore::updatePath(nav_msgs::msg::Path path) +{ + RCLCPP_INFO(logger_, "Path Updated"); + path_ = path; + // Reset PID state when path changes + prev_error_ = 0.0; + integral_error_ = 0.0; +} + +bool ControlCore::isPathEmpty() +{ + return path_.poses.empty(); +} + +geometry_msgs::msg::Twist ControlCore::calculateControlCommand( + double robot_x, double robot_y, double robot_theta, double dt) +{ + geometry_msgs::msg::Twist twist; + + if (path_.poses.empty()) { + return twist; + } + + unsigned int closest_index = findClosestPoint(robot_x, robot_y); + + // Determine path segment for CTE calculation + // We need two points to define a line. + double p1_x, p1_y, p2_x, p2_y; + + if (closest_index < path_.poses.size() - 1) { + p1_x = path_.poses[closest_index].pose.position.x; + p1_y = path_.poses[closest_index].pose.position.y; + p2_x = path_.poses[closest_index + 1].pose.position.x; + p2_y = path_.poses[closest_index + 1].pose.position.y; + } else if (path_.poses.size() > 1) { + // If closest is the last point, use the previous segment + p1_x = path_.poses[closest_index - 1].pose.position.x; + p1_y = path_.poses[closest_index - 1].pose.position.y; + p2_x = path_.poses[closest_index].pose.position.x; + p2_y = path_.poses[closest_index].pose.position.y; + } else { + // Only one point in path, go to it + p1_x = path_.poses[0].pose.position.x; + p1_y = path_.poses[0].pose.position.y; + // Treat as point at same location, just use distance logic or stop + // For now, create a dummy small segment in direction of robot heading or just return 0 + // Simplest: treat p2 as p1 + some epsilon in x + p2_x = p1_x + 0.001; + p2_y = p1_y; + } + + // Calculate Cross Track Error (CTE) + // Vector V from p1 to p2 + double vx = p2_x - p1_x; + double vy = p2_y - p1_y; + + // Normalize V + double v_len = std::sqrt(vx * vx + vy * vy); + if (v_len > 0) { + vx /= v_len; + vy /= v_len; + } + + // Vector R from p1 to robot + double rx = robot_x - p1_x; + double ry = robot_y - p1_y; + + // Signed distance (cross product of normalized V and R) in 2D (z component) + // if V is along X, R is along Y, cross is positive (Left). + double cross_product = vx * ry - vy * rx; + + double error = cross_product; // Positive if robot is to the left of the path + + // PID Calculation + // Integral + integral_error_ += error * dt; + + // Derivative + double derivative = 0.0; + if (dt > 0) { + derivative = (error - prev_error_) / dt; + } + + // PID Output + // We want to steer OPPOSITE to the error. + // If error is positive (left), we want negative angular velocity (turn right). + double output = -(kp_ * error + ki_ * integral_error_ + kd_ * derivative); + + prev_error_ = error; + + // Clamp output to max steering/angular velocity + if (output > max_steering_angle_) output = max_steering_angle_; + if (output < -max_steering_angle_) output = -max_steering_angle_; + + // Set linear velocity + // Logic: slow down if error is high or turning sharp? + // For now, constant velocity unless very sharp turn required? + // Let's stick to the previous logic: if turning hard, slow down? + // Or just constant linear velocity as per prompt "make sure the robot follows the path" + + // Just use the configured linear velocity + twist.linear.x = linear_velocity_; + twist.angular.z = output; + + return twist; +} + +unsigned int ControlCore::findClosestPoint(double robot_x, double robot_y) +{ + double min_distance = std::numeric_limits::max(); + unsigned int closest_index = 0; + + for (size_t i = 0; i < path_.poses.size(); ++i) { + double dx = path_.poses[i].pose.position.x - robot_x; + double dy = path_.poses[i].pose.position.y - robot_y; + double distance = std::sqrt(dx * dx + dy * dy); + + if (distance < min_distance) { + min_distance = distance; + closest_index = i; + } + } + return closest_index; +} + } // namespace robot diff --git a/src/robot/control/src/control_node.cpp b/src/robot/control/src/control_node.cpp index b560494..1bed921 100644 --- a/src/robot/control/src/control_node.cpp +++ b/src/robot/control/src/control_node.cpp @@ -14,10 +14,102 @@ #include "control/control_node.hpp" +#include + +#include "tf2/LinearMath/Matrix3x3.h" +#include "tf2/LinearMath/Quaternion.h" + ControlNode::ControlNode() : Node("control") , control_(robot::ControlCore(this->get_logger())) -{} +{ + processParameters(); + + // Initialize subscribers and publisher + path_subscriber_ = this->create_subscription( + path_topic_, 10, std::bind(&ControlNode::pathCallback, this, std::placeholders::_1)); + + odom_subscriber_ = this->create_subscription( + odom_topic_, 10, std::bind(&ControlNode::odomCallback, this, std::placeholders::_1)); + + cmd_vel_publisher_ = this->create_publisher(cmd_vel_topic_, 10); + + timer_ = this->create_wall_timer( + std::chrono::milliseconds(control_period_ms_), std::bind(&ControlNode::timerCallback, this)); + + control_.initControlCore(kp_, ki_, kd_, max_steering_angle_, linear_velocity_); +} + +void ControlNode::processParameters() +{ + // Declare all ROS2 Parameters + this->declare_parameter("path_topic", "/path"); + this->declare_parameter("odom_topic", "/odom/filtered"); + this->declare_parameter("cmd_vel_topic", "/cmd_vel"); + this->declare_parameter("control_period_ms", 100); + this->declare_parameter("kp", 1.0); + this->declare_parameter("ki", 0.0); + this->declare_parameter("kd", 0.0); + this->declare_parameter("max_steering_angle", 1.5); + this->declare_parameter("linear_velocity", 1.5); + + // Retrieve parameters and store them in member variables + path_topic_ = this->get_parameter("path_topic").as_string(); + odom_topic_ = this->get_parameter("odom_topic").as_string(); + cmd_vel_topic_ = this->get_parameter("cmd_vel_topic").as_string(); + control_period_ms_ = this->get_parameter("control_period_ms").as_int(); + kp_ = this->get_parameter("kp").as_double(); + ki_ = this->get_parameter("ki").as_double(); + kd_ = this->get_parameter("kd").as_double(); + max_steering_angle_ = this->get_parameter("max_steering_angle").as_double(); + linear_velocity_ = this->get_parameter("linear_velocity").as_double(); +} + +void ControlNode::pathCallback(const nav_msgs::msg::Path::SharedPtr msg) +{ + control_.updatePath(*msg); +} + +void ControlNode::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) +{ + robot_x_ = msg->pose.pose.position.x; + robot_y_ = msg->pose.pose.position.y; + + // Get robot's orientation (yaw) from quaternion using utility function + robot_theta_ = quaternionToYaw( + msg->pose.pose.orientation.x, + msg->pose.pose.orientation.y, + msg->pose.pose.orientation.z, + msg->pose.pose.orientation.w); +} + +void ControlNode::followPath() +{ + if (control_.isPathEmpty()) { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 3000, "Path is empty. Waiting for new path."); + return; + } + + // Calculate control commands + double dt = control_period_ms_ / 1000.0; + geometry_msgs::msg::Twist cmd_vel = control_.calculateControlCommand(robot_x_, robot_y_, robot_theta_, dt); + cmd_vel_publisher_->publish(cmd_vel); +} + +void ControlNode::timerCallback() +{ + followPath(); +} + +double ControlNode::quaternionToYaw(double x, double y, double z, double w) +{ + // Using tf2 to convert to RPY + tf2::Quaternion q(x, y, z, w); + tf2::Matrix3x3 m(q); + double roll, pitch, yaw; + m.getRPY(roll, pitch, yaw); + return yaw; +} int main(int argc, char ** argv) { diff --git a/src/robot/costmap/CMakeLists.txt b/src/robot/costmap/CMakeLists.txt index 7957026..a52fce6 100644 --- a/src/robot/costmap/CMakeLists.txt +++ b/src/robot/costmap/CMakeLists.txt @@ -26,6 +26,9 @@ endif() # Search for dependencies required for building this package find_package(ament_cmake REQUIRED) # ROS2 build tool find_package(rclcpp REQUIRED) # ROS2 C++ package +find_package(geometry_msgs REQUIRED) # ROS2 C++ package +find_package(sensor_msgs REQUIRED) # ROS2 C++ package +find_package(nav_msgs REQUIRED) # ROS2 C++ package # Compiles source files into a library # A library is not executed, instead other executables can link @@ -38,7 +41,7 @@ add_library(costmap_lib target_include_directories(costmap_lib PUBLIC include) # Add ROS2 dependencies required by package -ament_target_dependencies(costmap_lib rclcpp) +ament_target_dependencies(costmap_lib rclcpp sensor_msgs nav_msgs geometry_msgs) # Create ROS2 node executable from source files add_executable(costmap_node src/costmap_node.cpp) diff --git a/src/robot/costmap/config/params.yaml b/src/robot/costmap/config/params.yaml index 1833ec5..7076c14 100644 --- a/src/robot/costmap/config/params.yaml +++ b/src/robot/costmap/config/params.yaml @@ -1,2 +1,18 @@ -# costmap_node: -# ros__parameters: +costmap_node: + ros__parameters: + laserscan_topic: "/lidar" # laserscan topic with which costmap node subscribes to + costmap_topic: "/costmap" # occupancy grid topic with which costmap node publishes the costmap to + + costmap: + resolution: 0.40 + width: 120 + height: 120 + + origin: + position: + x: -24.0 # given by -1 * width * resolution / 2 + y: -24.0 # given by -1 * height * resolution / 2 + orientation: + w: 1.0 + + inflation_radius: 1.5 diff --git a/src/robot/costmap/include/costmap_core.hpp b/src/robot/costmap/include/costmap_core.hpp new file mode 100644 index 0000000..1b57b22 --- /dev/null +++ b/src/robot/costmap/include/costmap_core.hpp @@ -0,0 +1,58 @@ +// Copyright (c) 2025-present WATonomous. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef COSTMAP_CORE_HPP_ +#define COSTMAP_CORE_HPP_ + +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" + +namespace robot +{ + +class CostmapCore +{ +public: + // Constructor, we pass in the node's RCLCPP logger to enable logging to terminal + explicit CostmapCore(const rclcpp::Logger & logger); + + // Initializes the Costmap with the parameters that we get from the params.yaml + void initCostmap(double resolution, int width, int height, geometry_msgs::msg::Pose origin, double inflation_radius); + + // Update the costmap based on the current laserscan reading + void updateCostmap(const sensor_msgs::msg::LaserScan::SharedPtr laserscan) const; + + // Update the costmap based on point cloud data from RGBD camera + void updateCostmapFromPointCloud(const sensor_msgs::msg::PointCloud2::SharedPtr cloud) const; + + // Inflate the obstacle in the laserscan on the costmap because we want of range of values + // where we can and cannot go + void inflateObstacle(int origin_x, int origin_y) const; + + // Retrieves costmap data + nav_msgs::msg::OccupancyGrid::SharedPtr getCostmapData() const; + +private: + nav_msgs::msg::OccupancyGrid::SharedPtr costmap_data_; + rclcpp::Logger logger_; + + double inflation_radius_; + int inflation_cells_; +}; + +} // namespace robot + +#endif diff --git a/src/robot/costmap/include/costmap_node.hpp b/src/robot/costmap/include/costmap_node.hpp new file mode 100644 index 0000000..1ce73f9 --- /dev/null +++ b/src/robot/costmap/include/costmap_node.hpp @@ -0,0 +1,61 @@ +// Copyright (c) 2025-present WATonomous. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef COSTMAP_NODE_HPP_ +#define COSTMAP_NODE_HPP_ + +#include + +#include "costmap/costmap_core.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" + +class CostmapNode : public rclcpp::Node +{ +public: + // Costmap Node constructor + CostmapNode(); + + // Retrieves all the parameters and their values in params.yaml + void processParameters(); + + // Given a laserscan, it will send it into CostmapCore for processing and then + // retrieve the costmap + void laserScanCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg) const; + + // Given a point cloud, it will send it into CostmapCore for processing and then + // retrieve the costmap + void pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) const; + +private: + robot::CostmapCore costmap_; + + rclcpp::Subscription::SharedPtr laser_scan_sub_; + rclcpp::Subscription::SharedPtr point_cloud_sub_; + rclcpp::Publisher::SharedPtr costmap_pub_; + + std::string laserscan_topic_; + std::string pointcloud_topic_; + std::string costmap_topic_; + + double resolution_; + int width_; + int height_; + geometry_msgs::msg::Pose origin_; + double inflation_radius_; +}; + +#endif diff --git a/src/robot/costmap/package.xml b/src/robot/costmap/package.xml index 2abc8b5..5f3b749 100644 --- a/src/robot/costmap/package.xml +++ b/src/robot/costmap/package.xml @@ -10,6 +10,9 @@ ament_cmake rclcpp + geometry_msgs + sensor_msgs + nav_msgs ament_lint_auto ament_lint_common diff --git a/src/robot/costmap/src/costmap_core.cpp b/src/robot/costmap/src/costmap_core.cpp index e12a494..14ae9f3 100644 --- a/src/robot/costmap/src/costmap_core.cpp +++ b/src/robot/costmap/src/costmap_core.cpp @@ -14,11 +14,188 @@ #include "costmap/costmap_core.hpp" +#include +#include +#include +#include + +#include "sensor_msgs/point_cloud2_iterator.hpp" + namespace robot { CostmapCore::CostmapCore(const rclcpp::Logger & logger) -: logger_(logger) +: costmap_data_(std::make_shared()) +, logger_(logger) {} +void CostmapCore::initCostmap( + double resolution, int width, int height, geometry_msgs::msg::Pose origin, double inflation_radius) +{ + costmap_data_->info.resolution = resolution; + costmap_data_->info.width = width; + costmap_data_->info.height = height; + costmap_data_->info.origin = origin; + costmap_data_->data.assign(width * height, -1); + + inflation_radius_ = inflation_radius; + inflation_cells_ = static_cast(inflation_radius / resolution); + + RCLCPP_INFO(logger_, "Costmap initialized with resolution: %.2f, width: %d, height: %d", resolution, width, height); +} + +void CostmapCore::updateCostmap(const sensor_msgs::msg::LaserScan::SharedPtr laserscan) const +{ + // Reset the costmap to free space + std::fill(costmap_data_->data.begin(), costmap_data_->data.end(), 0); + + double angle = laserscan->angle_min; + for (size_t i = 0; i < laserscan->ranges.size(); ++i, angle += laserscan->angle_increment) { + double range = laserscan->ranges[i]; + + // Check if the range is within the valid range + if (range >= laserscan->range_min && range <= laserscan->range_max) { + // Calculate obstacle position in the map frame + double x = range * std::cos(angle); + double y = range * std::sin(angle); + + // Convert to grid coordinates + int grid_x = static_cast((x - costmap_data_->info.origin.position.x) / costmap_data_->info.resolution); + int grid_y = static_cast((y - costmap_data_->info.origin.position.y) / costmap_data_->info.resolution); + + if ( + grid_x >= 0 && grid_x < static_cast(costmap_data_->info.width) && grid_y >= 0 && + grid_y < static_cast(costmap_data_->info.height)) + { + // Mark the cell as occupied + int index = grid_y * costmap_data_->info.width + grid_x; + costmap_data_->data[index] = 100; // 100 indicates an occupied cell + + // Inflate around the obstacle + inflateObstacle(grid_x, grid_y); + } + } + } +} + +void CostmapCore::updateCostmapFromPointCloud(const sensor_msgs::msg::PointCloud2::SharedPtr cloud) const +{ + // Reset the costmap to free space + std::fill(costmap_data_->data.begin(), costmap_data_->data.end(), 0); + + // Create iterators for x, y, z fields in the point cloud + sensor_msgs::PointCloud2ConstIterator iter_x(*cloud, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(*cloud, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(*cloud, "z"); + + // Take a thin horizontal slice at a specific height (like a 2D laser scan) + // Filter by z-height to get points at obstacle level (ignore ground) + const double min_height = 0.05; // Minimum height - ignore ground points below this + const double max_height = 0.15; // Maximum height - only 10cm band + + // Downsample: only process every Nth point to reduce computation + // A 640x480 cloud has 307k points, we only need ~256-500 for a laser-scan-like result + const int point_skip = 20; // Process every 20th point (307k/20 ≈ 15k points checked) + + int point_count = 0; + + // Iterate through points in the cloud with downsampling + for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { + // Downsample: skip most points + if (++point_count % point_skip != 0) { + continue; + } + + float x = *iter_x; + float y = *iter_y; + float z = *iter_z; + + // Skip invalid points (NaN or Inf) + if (!std::isfinite(x) || !std::isfinite(y) || !std::isfinite(z)) { + continue; + } + + // Only consider points in a thin horizontal slice at a specific height + // This creates a 2D laser-scan-like view, ignoring ground points + if (z < min_height || z > max_height) { + continue; + } + + // Use x and y directly from the point cloud (in the sensor's frame) + // This is exactly like the laser scan approach + double obstacle_x = x; + double obstacle_y = y; + + // Convert to grid coordinates (same logic as laser scan) + int grid_x = + static_cast((obstacle_x - costmap_data_->info.origin.position.x) / costmap_data_->info.resolution); + int grid_y = + static_cast((obstacle_y - costmap_data_->info.origin.position.y) / costmap_data_->info.resolution); + + // Check if within costmap bounds + if ( + grid_x >= 0 && grid_x < static_cast(costmap_data_->info.width) && grid_y >= 0 && + grid_y < static_cast(costmap_data_->info.height)) + { + // Mark the cell as occupied + int index = grid_y * costmap_data_->info.width + grid_x; + costmap_data_->data[index] = 100; // 100 indicates an occupied cell + + // Inflate around the obstacle + inflateObstacle(grid_x, grid_y); + } + } +} + +void CostmapCore::inflateObstacle(int origin_x, int origin_y) const +{ + // Use a simple breadth-first search (BFS) to mark cells within the inflation radius + std::queue> queue; + queue.emplace(origin_x, origin_y); + + std::vector> visited( + costmap_data_->info.width, std::vector(costmap_data_->info.height, false)); + visited[origin_x][origin_y] = true; + + while (!queue.empty()) { + auto [x, y] = queue.front(); + queue.pop(); + + // Iterate over neighboring cells + for (int dx = -1; dx <= 1; ++dx) { + for (int dy = -1; dy <= 1; ++dy) { + if (dx == 0 && dy == 0) continue; // Skip the center cell + + int nx = x + dx; + int ny = y + dy; + + // Ensure the neighbor cell is within bounds + if ( + nx >= 0 && nx < static_cast(costmap_data_->info.width) && ny >= 0 && + ny < static_cast(costmap_data_->info.height) && !visited[nx][ny]) + { + // Calculate the distance to the original obstacle cell + double distance = std::hypot(nx - origin_x, ny - origin_y) * costmap_data_->info.resolution; + + // If within inflation radius, mark as inflated and add to queue + if (distance <= inflation_radius_) { + int index = ny * costmap_data_->info.width + nx; + if (costmap_data_->data[index] < (1 - (distance / inflation_radius_)) * 100) { + costmap_data_->data[index] = (1 - (distance / inflation_radius_)) * 100; + } + queue.emplace(nx, ny); + } + + visited[nx][ny] = true; + } + } + } + } +} + +nav_msgs::msg::OccupancyGrid::SharedPtr CostmapCore::getCostmapData() const +{ + return costmap_data_; +} + } // namespace robot diff --git a/src/robot/costmap/src/costmap_node.cpp b/src/robot/costmap/src/costmap_node.cpp index 163fe33..3b46d39 100644 --- a/src/robot/costmap/src/costmap_node.cpp +++ b/src/robot/costmap/src/costmap_node.cpp @@ -16,11 +16,82 @@ #include #include +#include CostmapNode::CostmapNode() : Node("costmap") , costmap_(robot::CostmapCore(this->get_logger())) -{} +{ + // load ROS2 yaml parameters + processParameters(); + + // Subscribe to point cloud from RGBD camera + point_cloud_sub_ = this->create_subscription( + pointcloud_topic_, 10, std::bind(&CostmapNode::pointCloudCallback, this, std::placeholders::_1)); + + // Keep laser scan subscription for backwards compatibility (optional) + // laser_scan_sub_ = this->create_subscription( + // laserscan_topic_, 10, + // std::bind( + // &CostmapNode::laserScanCallback, this, + // std::placeholders::_1)); + + costmap_pub_ = this->create_publisher(costmap_topic_, 10); + + RCLCPP_INFO(this->get_logger(), "Initialized ROS Constructs"); + RCLCPP_INFO(this->get_logger(), "Subscribed to point cloud topic: %s", pointcloud_topic_.c_str()); + + costmap_.initCostmap(resolution_, width_, height_, origin_, inflation_radius_); + + RCLCPP_INFO(this->get_logger(), "Initialized Costmap Core"); +} + +void CostmapNode::processParameters() +{ + // Declare all ROS2 Parameters + this->declare_parameter("laserscan_topic", "/lidar"); + this->declare_parameter("pointcloud_topic", "/camera/points"); + this->declare_parameter("costmap_topic", "/costmap"); + this->declare_parameter("costmap.resolution", 0.1); + this->declare_parameter("costmap.width", 100); + this->declare_parameter("costmap.height", 100); + this->declare_parameter("costmap.origin.position.x", -5.0); + this->declare_parameter("costmap.origin.position.y", -5.0); + this->declare_parameter("costmap.origin.orientation.w", 1.0); + this->declare_parameter("costmap.inflation_radius", 1.0); + + // Retrieve parameters and store them in member variables + laserscan_topic_ = this->get_parameter("laserscan_topic").as_string(); + pointcloud_topic_ = this->get_parameter("pointcloud_topic").as_string(); + costmap_topic_ = this->get_parameter("costmap_topic").as_string(); + resolution_ = this->get_parameter("costmap.resolution").as_double(); + width_ = this->get_parameter("costmap.width").as_int(); + height_ = this->get_parameter("costmap.height").as_int(); + origin_.position.x = this->get_parameter("costmap.origin.position.x").as_double(); + origin_.position.y = this->get_parameter("costmap.origin.position.y").as_double(); + origin_.orientation.w = this->get_parameter("costmap.origin.orientation.w").as_double(); + inflation_radius_ = this->get_parameter("costmap.inflation_radius").as_double(); +} + +void CostmapNode::laserScanCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg) const +{ + // Update the costmap according to the laser scan + costmap_.updateCostmap(msg); + // publish the costmap + nav_msgs::msg::OccupancyGrid costmap_msg = *costmap_.getCostmapData(); + costmap_msg.header = msg->header; + costmap_pub_->publish(costmap_msg); +} + +void CostmapNode::pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) const +{ + // Update the costmap according to the point cloud + costmap_.updateCostmapFromPointCloud(msg); + // publish the costmap + nav_msgs::msg::OccupancyGrid costmap_msg = *costmap_.getCostmapData(); + costmap_msg.header = msg->header; + costmap_pub_->publish(costmap_msg); +} int main(int argc, char ** argv) { diff --git a/src/robot/gps_sim/CMakeLists.txt b/src/robot/gps_sim/CMakeLists.txt new file mode 100644 index 0000000..641ded5 --- /dev/null +++ b/src/robot/gps_sim/CMakeLists.txt @@ -0,0 +1,59 @@ +# Copyright (c) 2025-present WATonomous. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +cmake_minimum_required(VERSION 3.10) +project(gps_sim) + +# Set compiler to use C++ 17 standard +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Search for dependencies required for building this package +find_package(ament_cmake REQUIRED) # ROS2 build tool +find_package(rclcpp REQUIRED) # ROS2 C++ package +find_package(geometry_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) + +# Create ROS2 node executable from source files +add_executable(gps_sim_node src/gps_sim_node.cpp) +target_include_directories(gps_sim_node + PUBLIC include) +# Add ROS2 dependencies required by package +ament_target_dependencies(gps_sim_node + rclcpp + geometry_msgs + tf2 + tf2_ros + tf2_geometry_msgs + sensor_msgs +) + +# Copy executable to installation location +install(TARGETS + gps_sim_node + DESTINATION lib/${PROJECT_NAME}) + +# Copy config files to installation location +install(DIRECTORY + config + DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/src/robot/gps_sim/config/params.yaml b/src/robot/gps_sim/config/params.yaml new file mode 100644 index 0000000..76c4bc1 --- /dev/null +++ b/src/robot/gps_sim/config/params.yaml @@ -0,0 +1,12 @@ +gps_sim_node: + ros__parameters: + source_frame: "sim_world" + target_frame: "robot/chassis/camera" + + # GPS noise parameters + gps_noise_std: 0.2 # Standard deviation for GPS position noise (meters) + gps_update_rate: 1.0 # GPS update rate (Hz) + + # GPS dropout simulation + gps_dropout_probability: 0.0 # Probability of GPS dropout (0.0-1.0) + simulate_dropouts: false # Whether to simulate GPS dropouts diff --git a/src/robot/gps_sim/include/gps_sim_node.hpp b/src/robot/gps_sim/include/gps_sim_node.hpp new file mode 100644 index 0000000..7fcdb3f --- /dev/null +++ b/src/robot/gps_sim/include/gps_sim_node.hpp @@ -0,0 +1,62 @@ +// Copyright (c) 2025-present WATonomous. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GPS_SIM_NODE_HPP_ +#define GPS_SIM_NODE_HPP_ + +#include +#include +#include + +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/nav_sat_fix.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +class GpsSimNode : public rclcpp::Node +{ +public: + GpsSimNode(); + +private: + void timerCallback(); + void processParameters(); + double generateGaussianNoise(double mean, double stddev); + + // Publisher + rclcpp::Publisher::SharedPtr gps_pub_; + + // Transform Utilities + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + // Timer to periodically publish GPS data + rclcpp::TimerBase::SharedPtr timer_; + + // Random number generator for noise + std::random_device rd_; + std::mt19937 gen_; + std::normal_distribution gaussian_noise_; + + // Configuration parameters + std::string source_frame_; + std::string target_frame_; + double gps_noise_std_; // Standard deviation for GPS position noise (meters) + double gps_update_rate_; // GPS update rate (Hz) + double gps_dropout_probability_; // Probability of GPS dropout (0.0-1.0) + bool simulate_dropouts_; // Whether to simulate GPS dropouts +}; + +#endif diff --git a/src/robot/gps_sim/package.xml b/src/robot/gps_sim/package.xml new file mode 100644 index 0000000..ee5b362 --- /dev/null +++ b/src/robot/gps_sim/package.xml @@ -0,0 +1,27 @@ + + + gps_sim + 0.0.0 + Simulates GPS data by adding noise to ground truth transforms + + Eddy Zhou + Apache2.0 + + + ament_cmake + rclcpp + geometry_msgs + tf2 + tf2_ros + tf2_geometry_msgs + sensor_msgs + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + + + ament_cmake + + diff --git a/src/robot/gps_sim/src/gps_sim_node.cpp b/src/robot/gps_sim/src/gps_sim_node.cpp new file mode 100644 index 0000000..bae6d5a --- /dev/null +++ b/src/robot/gps_sim/src/gps_sim_node.cpp @@ -0,0 +1,154 @@ +// Copyright (c) 2025-present WATonomous. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gps_sim/gps_sim_node.hpp" + +#include +#include +#include +#include + +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +GpsSimNode::GpsSimNode() +: Node("gps_sim_node") +, gen_(rd_()) +, gaussian_noise_(0.0, 1.0) +{ + processParameters(); + + // Create publisher + gps_pub_ = this->create_publisher("gps/fix", 10); + + // Create TF buffer and listener + tf_buffer_ = std::make_shared(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + // Create timer to publish GPS at specified rate + int period_ms = static_cast(1000.0 / gps_update_rate_); + timer_ = this->create_wall_timer(std::chrono::milliseconds(period_ms), std::bind(&GpsSimNode::timerCallback, this)); + + RCLCPP_INFO(this->get_logger(), "GPS Simulation Node initialized"); + RCLCPP_INFO(this->get_logger(), " Update rate: %.1f Hz", gps_update_rate_); + RCLCPP_INFO(this->get_logger(), " Noise std dev: %.2f m", gps_noise_std_); + RCLCPP_INFO(this->get_logger(), " Dropout probability: %.2f", gps_dropout_probability_); +} + +void GpsSimNode::processParameters() +{ + // Declare parameters + this->declare_parameter("source_frame", "sim_world"); + this->declare_parameter("target_frame", "robot/chassis/camera"); + this->declare_parameter("gps_noise_std", 2.0); + this->declare_parameter("gps_update_rate", 1.0); + this->declare_parameter("gps_dropout_probability", 0.0); + this->declare_parameter("simulate_dropouts", false); + + // Get parameters + source_frame_ = this->get_parameter("source_frame").as_string(); + target_frame_ = this->get_parameter("target_frame").as_string(); + gps_noise_std_ = this->get_parameter("gps_noise_std").as_double(); + gps_update_rate_ = this->get_parameter("gps_update_rate").as_double(); + gps_dropout_probability_ = this->get_parameter("gps_dropout_probability").as_double(); + simulate_dropouts_ = this->get_parameter("simulate_dropouts").as_bool(); +} + +void GpsSimNode::timerCallback() +{ + // Look up transform from source_frame to target_frame + geometry_msgs::msg::TransformStamped transform_stamped; + try { + transform_stamped = tf_buffer_->lookupTransform( + source_frame_, + target_frame_, + tf2::TimePointZero // latest available transform + ); + } catch (const tf2::TransformException & ex) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), + *this->get_clock(), + 1000, + "Could not transform %s to %s: %s", + source_frame_.c_str(), + target_frame_.c_str(), + ex.what()); + return; + } + + // Check for GPS dropout + if (simulate_dropouts_) { + double random_value = static_cast(gen_()) / static_cast(gen_.max()); + if (random_value < gps_dropout_probability_) { + RCLCPP_DEBUG_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "GPS dropout simulated - not publishing fix"); + return; + } + } + + // Create GPS message + sensor_msgs::msg::NavSatFix gps_msg; + + // Fill header + gps_msg.header.stamp = transform_stamped.header.stamp; + gps_msg.header.frame_id = source_frame_; + + // Get ground truth position + double gt_x = transform_stamped.transform.translation.x; + double gt_y = transform_stamped.transform.translation.y; + double gt_z = transform_stamped.transform.translation.z; + + // Add noise to simulate GPS measurement + // Note: In a real system, you would convert world coordinates to lat/lon + // For simulation, we use x,y directly as lat/lon (scaled appropriately) + double noisy_x = gt_x + generateGaussianNoise(0.0, gps_noise_std_); + double noisy_y = gt_y + generateGaussianNoise(0.0, gps_noise_std_); + + // Set position (using x,y as lat/lon for simulation purposes) + // In real GPS: these would be actual latitude and longitude + gps_msg.latitude = noisy_x; + gps_msg.longitude = noisy_y; + gps_msg.altitude = gt_z; + + // Set status (assuming good fix when not dropped out) + gps_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX; + gps_msg.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GPS; + + // Set position covariance (3x3 matrix stored as 9-element array) + // Format: [xx, xy, xz, yx, yy, yz, zx, zy, zz] + gps_msg.position_covariance[0] = gps_noise_std_ * gps_noise_std_; // xx + gps_msg.position_covariance[1] = 0.0; // xy + gps_msg.position_covariance[2] = 0.0; // xz + gps_msg.position_covariance[3] = 0.0; // yx + gps_msg.position_covariance[4] = gps_noise_std_ * gps_noise_std_; // yy + gps_msg.position_covariance[5] = 0.0; // yz + gps_msg.position_covariance[6] = 0.0; // zx + gps_msg.position_covariance[7] = 0.0; // zy + gps_msg.position_covariance[8] = 1.0; // zz (altitude uncertainty) + gps_msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; + + // Publish GPS message + gps_pub_->publish(gps_msg); +} + +double GpsSimNode::generateGaussianNoise(double mean, double stddev) +{ + return mean + stddev * gaussian_noise_(gen_); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/robot/imu_sim/CMakeLists.txt b/src/robot/imu_sim/CMakeLists.txt new file mode 100644 index 0000000..bbe39eb --- /dev/null +++ b/src/robot/imu_sim/CMakeLists.txt @@ -0,0 +1,53 @@ +# Copyright (c) 2025-present WATonomous. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +cmake_minimum_required(VERSION 3.10) +project(imu_sim) + +# Set compiler to use C++ 17 standard +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Search for dependencies required for building this package +find_package(ament_cmake REQUIRED) # ROS2 build tool +find_package(rclcpp REQUIRED) # ROS2 C++ package +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) + +# Create ROS2 node executable from source files +add_executable(imu_sim_node src/imu_sim_node.cpp) +target_include_directories(imu_sim_node + PUBLIC include) +# Add ROS2 dependencies required by package +ament_target_dependencies(imu_sim_node + rclcpp + sensor_msgs + geometry_msgs +) + +# Copy executable to installation location +install(TARGETS + imu_sim_node + DESTINATION lib/${PROJECT_NAME}) + +# Copy config files to installation location +install(DIRECTORY + config + DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/src/robot/imu_sim/config/params.yaml b/src/robot/imu_sim/config/params.yaml new file mode 100644 index 0000000..bc3a765 --- /dev/null +++ b/src/robot/imu_sim/config/params.yaml @@ -0,0 +1,17 @@ +imu_sim_node: + ros__parameters: + imu_topic: "/imu" + imu_sim_topic: "/imu/simulated" + + # Noise parameters (standard deviations) + orientation_noise_std: 0.001 # Quaternion component noise + angular_vel_noise_std: 0.001 # rad/s + linear_accel_noise_std: 0.01 # m/s^2 + + # Bias parameters (simulates sensor drift) + orientation_bias_std: 0.0001 # Quaternion component bias + angular_vel_bias_std: 0.0001 # rad/s + linear_accel_bias_std: 0.001 # m/s^2 + + # Whether to add bias to measurements + add_bias: true diff --git a/src/robot/imu_sim/include/imu_sim_node.hpp b/src/robot/imu_sim/include/imu_sim_node.hpp new file mode 100644 index 0000000..dfa7d48 --- /dev/null +++ b/src/robot/imu_sim/include/imu_sim_node.hpp @@ -0,0 +1,68 @@ +// Copyright (c) 2025-present WATonomous. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef IMU_SIM_NODE_HPP_ +#define IMU_SIM_NODE_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/imu.hpp" + +class ImuSimNode : public rclcpp::Node +{ +public: + ImuSimNode(); + +private: + void imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg); + void processParameters(); + double generateGaussianNoise(double mean, double stddev); + + // Subscriber and Publisher + rclcpp::Subscription::SharedPtr imu_sub_; + rclcpp::Publisher::SharedPtr imu_sim_pub_; + + // Random number generators for noise + std::random_device rd_; + std::mt19937 gen_; + std::normal_distribution gaussian_noise_; + + // IMU bias (simulates sensor drift) + double orientation_bias_x_; + double orientation_bias_y_; + double orientation_bias_z_; + double orientation_bias_w_; + double angular_vel_bias_x_; + double angular_vel_bias_y_; + double angular_vel_bias_z_; + double linear_accel_bias_x_; + double linear_accel_bias_y_; + double linear_accel_bias_z_; + + // Configuration parameters + std::string imu_topic_; + std::string imu_sim_topic_; + double orientation_noise_std_; // Standard deviation for orientation noise + double angular_vel_noise_std_; // Standard deviation for angular velocity noise + double linear_accel_noise_std_; // Standard deviation for linear acceleration noise + double orientation_bias_std_; // Standard deviation for orientation bias + double angular_vel_bias_std_; // Standard deviation for angular velocity bias + double linear_accel_bias_std_; // Standard deviation for linear acceleration bias + bool add_bias_; // Whether to add bias to measurements + bool initialized_; // Whether biases have been initialized +}; + +#endif diff --git a/src/robot/imu_sim/package.xml b/src/robot/imu_sim/package.xml new file mode 100644 index 0000000..53c13c9 --- /dev/null +++ b/src/robot/imu_sim/package.xml @@ -0,0 +1,24 @@ + + + imu_sim + 0.0.0 + Simulates IMU data by adding noise and bias to ground truth IMU measurements + + Eddy Zhou + Apache2.0 + + + ament_cmake + rclcpp + sensor_msgs + geometry_msgs + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + + + ament_cmake + + diff --git a/src/robot/imu_sim/src/imu_sim_node.cpp b/src/robot/imu_sim/src/imu_sim_node.cpp new file mode 100644 index 0000000..ed0c450 --- /dev/null +++ b/src/robot/imu_sim/src/imu_sim_node.cpp @@ -0,0 +1,175 @@ +// Copyright (c) 2025-present WATonomous. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "imu_sim/imu_sim_node.hpp" + +#include +#include +#include +#include + +ImuSimNode::ImuSimNode() +: Node("imu_sim_node") +, gen_(rd_()) +, gaussian_noise_(0.0, 1.0) +, initialized_(false) +{ + processParameters(); + + // Initialize biases to zero (will be set on first message if add_bias_ is true) + orientation_bias_x_ = 0.0; + orientation_bias_y_ = 0.0; + orientation_bias_z_ = 0.0; + orientation_bias_w_ = 0.0; + angular_vel_bias_x_ = 0.0; + angular_vel_bias_y_ = 0.0; + angular_vel_bias_z_ = 0.0; + linear_accel_bias_x_ = 0.0; + linear_accel_bias_y_ = 0.0; + linear_accel_bias_z_ = 0.0; + + // Create subscriber + imu_sub_ = this->create_subscription( + imu_topic_, 10, std::bind(&ImuSimNode::imuCallback, this, std::placeholders::_1)); + + // Create publisher + imu_sim_pub_ = this->create_publisher(imu_sim_topic_, 10); + + RCLCPP_INFO(this->get_logger(), "IMU Simulation Node initialized"); + RCLCPP_INFO(this->get_logger(), " Subscribing to: %s", imu_topic_.c_str()); + RCLCPP_INFO(this->get_logger(), " Publishing to: %s", imu_sim_topic_.c_str()); + RCLCPP_INFO(this->get_logger(), " Orientation noise std: %.4f", orientation_noise_std_); + RCLCPP_INFO(this->get_logger(), " Angular velocity noise std: %.4f", angular_vel_noise_std_); + RCLCPP_INFO(this->get_logger(), " Linear acceleration noise std: %.4f", linear_accel_noise_std_); + RCLCPP_INFO(this->get_logger(), " Add bias: %s", add_bias_ ? "true" : "false"); +} + +void ImuSimNode::processParameters() +{ + // Declare parameters + this->declare_parameter("imu_topic", "/imu"); + this->declare_parameter("imu_sim_topic", "/imu/simulated"); + this->declare_parameter("orientation_noise_std", 0.01); + this->declare_parameter("angular_vel_noise_std", 0.01); + this->declare_parameter("linear_accel_noise_std", 0.1); + this->declare_parameter("orientation_bias_std", 0.001); + this->declare_parameter("angular_vel_bias_std", 0.001); + this->declare_parameter("linear_accel_bias_std", 0.01); + this->declare_parameter("add_bias", true); + + // Get parameters + imu_topic_ = this->get_parameter("imu_topic").as_string(); + imu_sim_topic_ = this->get_parameter("imu_sim_topic").as_string(); + orientation_noise_std_ = this->get_parameter("orientation_noise_std").as_double(); + angular_vel_noise_std_ = this->get_parameter("angular_vel_noise_std").as_double(); + linear_accel_noise_std_ = this->get_parameter("linear_accel_noise_std").as_double(); + orientation_bias_std_ = this->get_parameter("orientation_bias_std").as_double(); + angular_vel_bias_std_ = this->get_parameter("angular_vel_bias_std").as_double(); + linear_accel_bias_std_ = this->get_parameter("linear_accel_bias_std").as_double(); + add_bias_ = this->get_parameter("add_bias").as_bool(); +} + +void ImuSimNode::imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg) +{ + // Initialize biases on first message if enabled + if (add_bias_ && !initialized_) { + orientation_bias_x_ = generateGaussianNoise(0.0, orientation_bias_std_); + orientation_bias_y_ = generateGaussianNoise(0.0, orientation_bias_std_); + orientation_bias_z_ = generateGaussianNoise(0.0, orientation_bias_std_); + orientation_bias_w_ = generateGaussianNoise(0.0, orientation_bias_std_); + angular_vel_bias_x_ = generateGaussianNoise(0.0, angular_vel_bias_std_); + angular_vel_bias_y_ = generateGaussianNoise(0.0, angular_vel_bias_std_); + angular_vel_bias_z_ = generateGaussianNoise(0.0, angular_vel_bias_std_); + linear_accel_bias_x_ = generateGaussianNoise(0.0, linear_accel_bias_std_); + linear_accel_bias_y_ = generateGaussianNoise(0.0, linear_accel_bias_std_); + linear_accel_bias_z_ = generateGaussianNoise(0.0, linear_accel_bias_std_); + initialized_ = true; + RCLCPP_INFO(this->get_logger(), "IMU biases initialized"); + } + + // Create simulated IMU message + sensor_msgs::msg::Imu imu_sim_msg = *msg; + + // Add noise and bias to orientation + imu_sim_msg.orientation.x = + msg->orientation.x + generateGaussianNoise(0.0, orientation_noise_std_) + (add_bias_ ? orientation_bias_x_ : 0.0); + imu_sim_msg.orientation.y = + msg->orientation.y + generateGaussianNoise(0.0, orientation_noise_std_) + (add_bias_ ? orientation_bias_y_ : 0.0); + imu_sim_msg.orientation.z = + msg->orientation.z + generateGaussianNoise(0.0, orientation_noise_std_) + (add_bias_ ? orientation_bias_z_ : 0.0); + imu_sim_msg.orientation.w = + msg->orientation.w + generateGaussianNoise(0.0, orientation_noise_std_) + (add_bias_ ? orientation_bias_w_ : 0.0); + + // Normalize quaternion (important for valid orientation) + double norm = std::sqrt( + imu_sim_msg.orientation.x * imu_sim_msg.orientation.x + imu_sim_msg.orientation.y * imu_sim_msg.orientation.y + + imu_sim_msg.orientation.z * imu_sim_msg.orientation.z + imu_sim_msg.orientation.w * imu_sim_msg.orientation.w); + if (norm > 0.0) { + imu_sim_msg.orientation.x /= norm; + imu_sim_msg.orientation.y /= norm; + imu_sim_msg.orientation.z /= norm; + imu_sim_msg.orientation.w /= norm; + } + + // Add noise and bias to angular velocity + imu_sim_msg.angular_velocity.x = msg->angular_velocity.x + generateGaussianNoise(0.0, angular_vel_noise_std_) + + (add_bias_ ? angular_vel_bias_x_ : 0.0); + imu_sim_msg.angular_velocity.y = msg->angular_velocity.y + generateGaussianNoise(0.0, angular_vel_noise_std_) + + (add_bias_ ? angular_vel_bias_y_ : 0.0); + imu_sim_msg.angular_velocity.z = msg->angular_velocity.z + generateGaussianNoise(0.0, angular_vel_noise_std_) + + (add_bias_ ? angular_vel_bias_z_ : 0.0); + + // Add noise and bias to linear acceleration + imu_sim_msg.linear_acceleration.x = msg->linear_acceleration.x + generateGaussianNoise(0.0, linear_accel_noise_std_) + + (add_bias_ ? linear_accel_bias_x_ : 0.0); + imu_sim_msg.linear_acceleration.y = msg->linear_acceleration.y + generateGaussianNoise(0.0, linear_accel_noise_std_) + + (add_bias_ ? linear_accel_bias_y_ : 0.0); + imu_sim_msg.linear_acceleration.z = msg->linear_acceleration.z + generateGaussianNoise(0.0, linear_accel_noise_std_) + + (add_bias_ ? linear_accel_bias_z_ : 0.0); + + // Update covariance matrices to reflect added noise + // Orientation covariance (3x3 matrix for roll, pitch, yaw) + double orientation_cov = orientation_noise_std_ * orientation_noise_std_; + imu_sim_msg.orientation_covariance[0] = orientation_cov; // roll-roll + imu_sim_msg.orientation_covariance[4] = orientation_cov; // pitch-pitch + imu_sim_msg.orientation_covariance[8] = orientation_cov; // yaw-yaw + + // Angular velocity covariance + double angular_vel_cov = angular_vel_noise_std_ * angular_vel_noise_std_; + imu_sim_msg.angular_velocity_covariance[0] = angular_vel_cov; // x-x + imu_sim_msg.angular_velocity_covariance[4] = angular_vel_cov; // y-y + imu_sim_msg.angular_velocity_covariance[8] = angular_vel_cov; // z-z + + // Linear acceleration covariance + double linear_accel_cov = linear_accel_noise_std_ * linear_accel_noise_std_; + imu_sim_msg.linear_acceleration_covariance[0] = linear_accel_cov; // x-x + imu_sim_msg.linear_acceleration_covariance[4] = linear_accel_cov; // y-y + imu_sim_msg.linear_acceleration_covariance[8] = linear_accel_cov; // z-z + + // Publish simulated IMU message + imu_sim_pub_->publish(imu_sim_msg); +} + +double ImuSimNode::generateGaussianNoise(double mean, double stddev) +{ + return mean + stddev * gaussian_noise_(gen_); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/robot/localization/CMakeLists.txt b/src/robot/localization/CMakeLists.txt new file mode 100644 index 0000000..a30ed44 --- /dev/null +++ b/src/robot/localization/CMakeLists.txt @@ -0,0 +1,68 @@ +# Copyright (c) 2025-present WATonomous. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +cmake_minimum_required(VERSION 3.10) +project(localization) + +# Set compiler to use C++ 17 standard +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Search for dependencies required for building this package +find_package(ament_cmake REQUIRED) # ROS2 build tool +find_package(rclcpp REQUIRED) # ROS2 C++ package +find_package(geometry_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) + +# Compiles source files into a library +add_library(localization_lib + src/localization_core.cpp) +# Indicate to compiler where to search for header files +target_include_directories(localization_lib + PUBLIC include) +# Add ROS2 dependencies required by package +ament_target_dependencies(localization_lib + rclcpp + geometry_msgs + sensor_msgs + nav_msgs + tf2 + tf2_ros + tf2_geometry_msgs +) + +# Create ROS2 node executable from source files +add_executable(localization_node src/localization_node.cpp) +# Link to the previously built library +target_link_libraries(localization_node localization_lib) + +# Copy executable to installation location +install(TARGETS + localization_node + DESTINATION lib/${PROJECT_NAME}) + +# Copy config files to installation location +install(DIRECTORY + config + DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/src/robot/localization/config/params.yaml b/src/robot/localization/config/params.yaml new file mode 100644 index 0000000..0fa4098 --- /dev/null +++ b/src/robot/localization/config/params.yaml @@ -0,0 +1,24 @@ +localization_node: + ros__parameters: + gps_topic: "/gps/fix" + imu_topic: "/imu/simulated" + cmd_vel_topic: "/cmd_vel" + odom_topic: "/odom/ekf_filtered" + source_frame: "sim_world" + target_frame: "robot/chassis/camera" + + # EKF process noise (uncertainty from motion model) + process_noise_position: 0.1 # m + process_noise_orientation: 0.05 # rad + process_noise_velocity: 0.1 # m/s + + # EKF measurement noise (from sensor characteristics) + gps_measurement_noise: 0.5 # m (should match gps_noise_std) + imu_measurement_noise_orientation: 0.001 # rad + imu_measurement_noise_angular_vel: 0.001 # rad/s + + # EKF update rate + ekf_update_rate: 10.0 # Hz + + # Use TF for initialization + use_tf_for_init: true diff --git a/src/robot/localization/include/localization_core.hpp b/src/robot/localization/include/localization_core.hpp new file mode 100644 index 0000000..e527e47 --- /dev/null +++ b/src/robot/localization/include/localization_core.hpp @@ -0,0 +1,171 @@ +// Copyright (c) 2025-present WATonomous. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LOCALIZATION_CORE_HPP_ +#define LOCALIZATION_CORE_HPP_ + +#include +#include +#include + +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/imu.hpp" +#include "sensor_msgs/msg/nav_sat_fix.hpp" + +namespace robot +{ + +// Simple 6x6 matrix class for EKF operations +class Matrix6x6 +{ +public: + Matrix6x6(); + explicit Matrix6x6(double value); + Matrix6x6(const Matrix6x6 & other); + + double & operator()(int i, int j) + { + return data_[i * 6 + j]; + } + + const double & operator()(int i, int j) const + { + return data_[i * 6 + j]; + } + + Matrix6x6 operator+(const Matrix6x6 & other) const; + Matrix6x6 operator-(const Matrix6x6 & other) const; + Matrix6x6 operator*(const Matrix6x6 & other) const; + Matrix6x6 transpose() const; + Matrix6x6 inverse() const; + + static Matrix6x6 identity(); + +private: + std::array data_; +}; + +// Simple 6x1 vector class +class Vector6 +{ +public: + Vector6(); + Vector6(double x, double y, double theta, double vx, double vy, double omega); + + double & operator()(int i) + { + return data_[i]; + } + + const double & operator()(int i) const + { + return data_[i]; + } + + Vector6 operator+(const Vector6 & other) const; + Vector6 operator-(const Vector6 & other) const; + Vector6 operator*(double scalar) const; + + double norm() const; + + double x() const + { + return data_[0]; + } + + double y() const + { + return data_[1]; + } + + double theta() const + { + return data_[2]; + } + + double vx() const + { + return data_[3]; + } + + double vy() const + { + return data_[4]; + } + + double omega() const + { + return data_[5]; + } + +private: + std::array data_; +}; + +class ExtendedKalmanFilter +{ +public: + explicit ExtendedKalmanFilter(const rclcpp::Logger & logger); + + // Initialize EKF with initial pose + void initialize(double x, double y, double theta); + + // Prediction step (process model) - using wheel odometry + void predict( + double vx, + double vy, + double omega, + double dt, + double process_noise_pos, + double process_noise_theta, + double process_noise_vel); + + // Update steps (measurement models) + void updateGPS(double gps_x, double gps_y, double measurement_noise); + void updateIMUOrientation(double theta, double measurement_noise); + void updateIMUAngularVel(double omega, double measurement_noise); + + // Get current state estimate + void getState(double & x, double & y, double & theta, double & vx, double & vy, double & omega); + + // Get state with covariance for odometry message + void getOdometry( + nav_msgs::msg::Odometry & odom_msg, + const std::string & frame_id, + const std::string & child_frame_id, + const rclcpp::Time & stamp); + + // Check if EKF is initialized + bool isInitialized() const + { + return initialized_; + } + +private: + // State vector: [x, y, theta, vx, vy, omega] + Vector6 state_; + Matrix6x6 P_; // 6x6 covariance matrix + + bool initialized_; + rclcpp::Logger logger_; + + // Helper functions + Matrix6x6 computeProcessJacobian(double vx, double vy, double omega, double dt); + void normalizeTheta(); +}; + +} // namespace robot + +#endif diff --git a/src/robot/localization/include/localization_node.hpp b/src/robot/localization/include/localization_node.hpp new file mode 100644 index 0000000..bf8c70c --- /dev/null +++ b/src/robot/localization/include/localization_node.hpp @@ -0,0 +1,89 @@ +// Copyright (c) 2025-present WATonomous. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LOCALIZATION_NODE_HPP_ +#define LOCALIZATION_NODE_HPP_ + +#include +#include + +#include "geometry_msgs/msg/twist.hpp" +#include "localization/localization_core.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/imu.hpp" +#include "sensor_msgs/msg/nav_sat_fix.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +class LocalizationNode : public rclcpp::Node +{ +public: + LocalizationNode(); + +private: + void processParameters(); + void timerCallback(); + void gpsCallback(const sensor_msgs::msg::NavSatFix::SharedPtr msg); + void imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg); + void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg); + + double quaternionToYaw(double x, double y, double z, double w); + + // EKF instance + robot::ExtendedKalmanFilter ekf_; + + // Publishers + rclcpp::Publisher::SharedPtr odom_pub_; + + // Subscribers + rclcpp::Subscription::SharedPtr gps_sub_; + rclcpp::Subscription::SharedPtr imu_sub_; + rclcpp::Subscription::SharedPtr cmd_vel_sub_; + + // Transform utilities (for initialization) + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + + // Latest sensor data + geometry_msgs::msg::Twist latest_cmd_vel_; + sensor_msgs::msg::Imu latest_imu_; + bool has_cmd_vel_; + bool has_imu_; + rclcpp::Time last_cmd_vel_time_; + rclcpp::Time last_update_time_; + + // Configuration parameters + std::string gps_topic_; + std::string imu_topic_; + std::string cmd_vel_topic_; + std::string odom_topic_; + std::string source_frame_; + std::string target_frame_; + + double process_noise_position_; + double process_noise_orientation_; + double process_noise_velocity_; + double gps_measurement_noise_; + double imu_measurement_noise_orientation_; + double imu_measurement_noise_angular_vel_; + + double ekf_update_rate_; + bool use_tf_for_init_; +}; + +#endif diff --git a/src/robot/localization/package.xml b/src/robot/localization/package.xml new file mode 100644 index 0000000..0547949 --- /dev/null +++ b/src/robot/localization/package.xml @@ -0,0 +1,28 @@ + + + localization + 0.0.0 + EKF-based localization using GPS, IMU, and wheel encoders + + Eddy Zhou + Apache2.0 + + + ament_cmake + rclcpp + geometry_msgs + sensor_msgs + nav_msgs + tf2 + tf2_ros + tf2_geometry_msgs + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + + + ament_cmake + + diff --git a/src/robot/localization/src/localization_core.cpp b/src/robot/localization/src/localization_core.cpp new file mode 100644 index 0000000..2188c9e --- /dev/null +++ b/src/robot/localization/src/localization_core.cpp @@ -0,0 +1,548 @@ +// Copyright (c) 2025-present WATonomous. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "localization/localization_core.hpp" + +#include +#include +#include +#include + +#include "tf2/LinearMath/Matrix3x3.h" +#include "tf2/LinearMath/Quaternion.h" + +namespace robot +{ + +// ============================================================================ +// Matrix6x6 Implementation +// ============================================================================ + +Matrix6x6::Matrix6x6() +{ + data_.fill(0.0); +} + +Matrix6x6::Matrix6x6(double value) +{ + data_.fill(0.0); + for (int i = 0; i < 6; i++) { + data_[i * 6 + i] = value; + } +} + +Matrix6x6::Matrix6x6(const Matrix6x6 & other) +: data_(other.data_) +{} + +Matrix6x6 Matrix6x6::operator+(const Matrix6x6 & other) const +{ + Matrix6x6 result; + for (size_t i = 0; i < 36; i++) { + result.data_[i] = data_[i] + other.data_[i]; + } + return result; +} + +Matrix6x6 Matrix6x6::operator-(const Matrix6x6 & other) const +{ + Matrix6x6 result; + for (size_t i = 0; i < 36; i++) { + result.data_[i] = data_[i] - other.data_[i]; + } + return result; +} + +Matrix6x6 Matrix6x6::operator*(const Matrix6x6 & other) const +{ + Matrix6x6 result; + for (int i = 0; i < 6; i++) { + for (int j = 0; j < 6; j++) { + double sum = 0.0; + for (int k = 0; k < 6; k++) { + sum += (*this)(i, k) * other(k, j); + } + result(i, j) = sum; + } + } + return result; +} + +Matrix6x6 Matrix6x6::transpose() const +{ + Matrix6x6 result; + for (int i = 0; i < 6; i++) { + for (int j = 0; j < 6; j++) { + result(j, i) = (*this)(i, j); + } + } + return result; +} + +Matrix6x6 Matrix6x6::inverse() const +{ + // Simple Gaussian elimination for 6x6 matrix + Matrix6x6 augmented = *this; + Matrix6x6 identity = Matrix6x6::identity(); + + // Create augmented matrix [A|I] + std::array aug_data; + for (int i = 0; i < 6; i++) { + for (int j = 0; j < 6; j++) { + aug_data[i * 12 + j] = (*this)(i, j); + aug_data[i * 12 + j + 6] = (i == j) ? 1.0 : 0.0; + } + } + + // Forward elimination + for (int i = 0; i < 6; i++) { + // Find pivot + int max_row = i; + double max_val = std::abs(aug_data[i * 12 + i]); + for (int k = i + 1; k < 6; k++) { + if (std::abs(aug_data[k * 12 + i]) > max_val) { + max_val = std::abs(aug_data[k * 12 + i]); + max_row = k; + } + } + + // Swap rows + if (max_row != i) { + for (int j = 0; j < 12; j++) { + std::swap(aug_data[i * 12 + j], aug_data[max_row * 12 + j]); + } + } + + // Make diagonal element 1 + double pivot = aug_data[i * 12 + i]; + if (std::abs(pivot) < 1e-10) { + // Singular matrix, return identity (fallback) + return Matrix6x6::identity(); + } + + for (int j = 0; j < 12; j++) { + aug_data[i * 12 + j] /= pivot; + } + + // Eliminate column + for (int k = 0; k < 6; k++) { + if (k != i) { + double factor = aug_data[k * 12 + i]; + for (int j = 0; j < 12; j++) { + aug_data[k * 12 + j] -= factor * aug_data[i * 12 + j]; + } + } + } + } + + // Extract inverse + Matrix6x6 result; + for (int i = 0; i < 6; i++) { + for (int j = 0; j < 6; j++) { + result(i, j) = aug_data[i * 12 + j + 6]; + } + } + + return result; +} + +Matrix6x6 Matrix6x6::identity() +{ + Matrix6x6 result; + for (int i = 0; i < 6; i++) { + result(i, i) = 1.0; + } + return result; +} + +// ============================================================================ +// Vector6 Implementation +// ============================================================================ + +Vector6::Vector6() +{ + data_.fill(0.0); +} + +Vector6::Vector6(double x, double y, double theta, double vx, double vy, double omega) +{ + data_[0] = x; + data_[1] = y; + data_[2] = theta; + data_[3] = vx; + data_[4] = vy; + data_[5] = omega; +} + +Vector6 Vector6::operator+(const Vector6 & other) const +{ + Vector6 result; + for (size_t i = 0; i < 6; i++) { + result.data_[i] = data_[i] + other.data_[i]; + } + return result; +} + +Vector6 Vector6::operator-(const Vector6 & other) const +{ + Vector6 result; + for (size_t i = 0; i < 6; i++) { + result.data_[i] = data_[i] - other.data_[i]; + } + return result; +} + +Vector6 Vector6::operator*(double scalar) const +{ + Vector6 result; + for (size_t i = 0; i < 6; i++) { + result.data_[i] = data_[i] * scalar; + } + return result; +} + +double Vector6::norm() const +{ + double sum = 0.0; + for (size_t i = 0; i < 6; i++) { + sum += data_[i] * data_[i]; + } + return std::sqrt(sum); +} + +// ============================================================================ +// ExtendedKalmanFilter Implementation +// ============================================================================ + +ExtendedKalmanFilter::ExtendedKalmanFilter(const rclcpp::Logger & logger) +: logger_(logger) +, initialized_(false) +{ + state_ = Vector6(); + P_ = Matrix6x6::identity() * 0.1; // Initial uncertainty +} + +void ExtendedKalmanFilter::initialize(double x, double y, double theta) +{ + state_ = Vector6(x, y, theta, 0.0, 0.0, 0.0); + + // Initialize covariance with high uncertainty + P_ = Matrix6x6::identity(); + P_(0, 0) = 1.0; // x uncertainty + P_(1, 1) = 1.0; // y uncertainty + P_(2, 2) = 0.1; // theta uncertainty + P_(3, 3) = 0.1; // vx uncertainty + P_(4, 4) = 0.1; // vy uncertainty + P_(5, 5) = 0.1; // omega uncertainty + + initialized_ = true; +} + +void ExtendedKalmanFilter::predict( + double vx, + double vy, + double omega, + double dt, + double process_noise_pos, + double process_noise_theta, + double process_noise_vel) +{ + if (!initialized_) { + return; + } + + // Process model: constant velocity model + double theta = state_.theta(); + double cos_theta = std::cos(theta); + double sin_theta = std::sin(theta); + + // Predict state + double x_new = state_.x() + (vx * cos_theta - vy * sin_theta) * dt; + double y_new = state_.y() + (vx * sin_theta + vy * cos_theta) * dt; + double theta_new = state_.theta() + omega * dt; + + state_ = Vector6(x_new, y_new, theta_new, vx, vy, omega); + normalizeTheta(); + + // Compute process Jacobian + Matrix6x6 F = computeProcessJacobian(vx, vy, omega, dt); + + // Process noise covariance + Matrix6x6 Q; + Q(0, 0) = process_noise_pos * process_noise_pos; + Q(1, 1) = process_noise_pos * process_noise_pos; + Q(2, 2) = process_noise_theta * process_noise_theta; + Q(3, 3) = process_noise_vel * process_noise_vel; + Q(4, 4) = process_noise_vel * process_noise_vel; + Q(5, 5) = process_noise_vel * process_noise_vel; + + // Update covariance: P = F * P * F^T + Q + Matrix6x6 F_transpose = F.transpose(); + P_ = F * P_ * F_transpose + Q; +} + +void ExtendedKalmanFilter::updateGPS(double gps_x, double gps_y, double measurement_noise) +{ + if (!initialized_) { + return; + } + + // Measurement model: h(x) = [x, y] + Vector6 h; + h(0) = state_.x(); + h(1) = state_.y(); + + // Measurement + Vector6 z; + z(0) = gps_x; + z(1) = gps_y; + + // Innovation + Vector6 y = z - h; + + // Measurement Jacobian (2x6) + // H = [1 0 0 0 0 0] + // [0 1 0 0 0 0] + + // Measurement noise covariance (2x2) + // R = [σ² 0 ] + // [0 σ²] + double R_val = measurement_noise * measurement_noise; + + // Innovation covariance: S = H * P * H^T + R + // For 2x6 H and 6x6 P, we need to compute H*P*H^T + // H*P is 2x6, then (H*P)*H^T is 2x2 + double S_00 = P_(0, 0) + R_val; + double S_01 = P_(0, 1); + double S_10 = P_(1, 0); + double S_11 = P_(1, 1) + R_val; + + // S inverse (2x2) + double det_S = S_00 * S_11 - S_01 * S_10; + if (std::abs(det_S) < 1e-10) { + return; // Singular, skip update + } + + double S_inv_00 = S_11 / det_S; + double S_inv_01 = -S_01 / det_S; + double S_inv_10 = -S_10 / det_S; + double S_inv_11 = S_00 / det_S; + + // Kalman gain: K = P * H^T * S^-1 + // P*H^T is 6x2, then multiply by S^-1 (2x2) gives 6x2 + Vector6 K_x, K_y; + for (int i = 0; i < 6; i++) { + K_x(i) = P_(i, 0) * S_inv_00 + P_(i, 1) * S_inv_10; + K_y(i) = P_(i, 0) * S_inv_01 + P_(i, 1) * S_inv_11; + } + + // Update state: x = x + K * y + state_ = state_ + K_x * y(0) + K_y * y(1); + normalizeTheta(); + + // Update covariance: P = (I - K*H) * P + // K*H is 6x6, where (K*H)(i,j) = K_x(i)*H(0,j) + K_y(i)*H(1,j) + Matrix6x6 KH; + for (int i = 0; i < 6; i++) { + KH(i, 0) = K_x(i); + KH(i, 1) = K_y(i); + } + + Matrix6x6 I = Matrix6x6::identity(); + Matrix6x6 I_minus_KH = I - KH; + P_ = I_minus_KH * P_; +} + +void ExtendedKalmanFilter::updateIMUOrientation(double theta, double measurement_noise) +{ + if (!initialized_) { + return; + } + + // Measurement model: h(x) = theta + double h = state_.theta(); + double z = theta; + + // Innovation (normalize angle difference) + double y = z - h; + while (y > M_PI) y -= 2.0 * M_PI; + while (y < -M_PI) y += 2.0 * M_PI; + + // Measurement Jacobian: H = [0 0 1 0 0 0] + // Innovation covariance: S = H * P * H^T + R + double S = P_(2, 2) + measurement_noise * measurement_noise; + + if (std::abs(S) < 1e-10) { + return; + } + + // Kalman gain: K = P * H^T / S + Vector6 K; + for (int i = 0; i < 6; i++) { + K(i) = P_(i, 2) / S; + } + + // Update state + state_ = state_ + K * y; + normalizeTheta(); + + // Update covariance: P = (I - K*H) * P + Matrix6x6 KH; + for (int i = 0; i < 6; i++) { + KH(i, 2) = K(i); + } + + Matrix6x6 I = Matrix6x6::identity(); + Matrix6x6 I_minus_KH = I - KH; + P_ = I_minus_KH * P_; +} + +void ExtendedKalmanFilter::updateIMUAngularVel(double omega, double measurement_noise) +{ + if (!initialized_) { + return; + } + + // Measurement model: h(x) = omega + double h = state_.omega(); + double z = omega; + double y = z - h; + + // Measurement Jacobian: H = [0 0 0 0 0 1] + // Innovation covariance: S = H * P * H^T + R + double S = P_(5, 5) + measurement_noise * measurement_noise; + + if (std::abs(S) < 1e-10) { + return; + } + + // Kalman gain: K = P * H^T / S + Vector6 K; + for (int i = 0; i < 6; i++) { + K(i) = P_(i, 5) / S; + } + + // Update state + state_ = state_ + K * y; + + // Update covariance: P = (I - K*H) * P + Matrix6x6 KH; + for (int i = 0; i < 6; i++) { + KH(i, 5) = K(i); + } + + Matrix6x6 I = Matrix6x6::identity(); + Matrix6x6 I_minus_KH = I - KH; + P_ = I_minus_KH * P_; +} + +void ExtendedKalmanFilter::getState(double & x, double & y, double & theta, double & vx, double & vy, double & omega) +{ + x = state_.x(); + y = state_.y(); + theta = state_.theta(); + vx = state_.vx(); + vy = state_.vy(); + omega = state_.omega(); +} + +void ExtendedKalmanFilter::getOdometry( + nav_msgs::msg::Odometry & odom_msg, + const std::string & frame_id, + const std::string & child_frame_id, + const rclcpp::Time & stamp) +{ + odom_msg.header.stamp = stamp; + odom_msg.header.frame_id = frame_id; + odom_msg.child_frame_id = child_frame_id; + + // Set pose + odom_msg.pose.pose.position.x = state_.x(); + odom_msg.pose.pose.position.y = state_.y(); + odom_msg.pose.pose.position.z = 0.0; + + // Convert theta to quaternion + tf2::Quaternion q; + q.setRPY(0, 0, state_.theta()); + odom_msg.pose.pose.orientation.x = q.x(); + odom_msg.pose.pose.orientation.y = q.y(); + odom_msg.pose.pose.orientation.z = q.z(); + odom_msg.pose.pose.orientation.w = q.w(); + + // Set twist + odom_msg.twist.twist.linear.x = state_.vx(); + odom_msg.twist.twist.linear.y = state_.vy(); + odom_msg.twist.twist.linear.z = 0.0; + odom_msg.twist.twist.angular.x = 0.0; + odom_msg.twist.twist.angular.y = 0.0; + odom_msg.twist.twist.angular.z = state_.omega(); + + // Set covariance (6x6 matrix flattened to 36 elements) + // Pose covariance (6x6: x, y, z, roll, pitch, yaw) + for (int i = 0; i < 6; i++) { + for (int j = 0; j < 6; j++) { + if (i < 3 && j < 3) { + // Position covariance + odom_msg.pose.covariance[i * 6 + j] = P_(i, j); + } else if (i == 5 && j == 5) { + // Yaw covariance (theta is index 2 in state) + odom_msg.pose.covariance[35] = P_(2, 2); + } + } + } + + // Twist covariance + for (int i = 0; i < 6; i++) { + for (int j = 0; j < 6; j++) { + if (i < 3 && j < 3) { + // Linear velocity covariance + odom_msg.twist.covariance[i * 6 + j] = P_(i + 3, j + 3); + } else if (i == 5 && j == 5) { + // Angular z velocity covariance + odom_msg.twist.covariance[35] = P_(5, 5); + } + } + } +} + +Matrix6x6 ExtendedKalmanFilter::computeProcessJacobian(double vx, double vy, double omega, double dt) +{ + Matrix6x6 F = Matrix6x6::identity(); + double theta = state_.theta(); + double cos_theta = std::cos(theta); + double sin_theta = std::sin(theta); + + // Partial derivatives of process model + F(0, 2) = (-vx * sin_theta - vy * cos_theta) * dt; // dx/dtheta + F(0, 3) = cos_theta * dt; // dx/dvx + F(0, 4) = -sin_theta * dt; // dx/dvy + + F(1, 2) = (vx * cos_theta - vy * sin_theta) * dt; // dy/dtheta + F(1, 3) = sin_theta * dt; // dy/dvx + F(1, 4) = cos_theta * dt; // dy/dvy + + F(2, 5) = dt; // dtheta/domega + + return F; +} + +void ExtendedKalmanFilter::normalizeTheta() +{ + double theta = state_.theta(); + while (theta > M_PI) theta -= 2.0 * M_PI; + while (theta < -M_PI) theta += 2.0 * M_PI; + state_(2) = theta; +} + +} // namespace robot diff --git a/src/robot/localization/src/localization_node.cpp b/src/robot/localization/src/localization_node.cpp new file mode 100644 index 0000000..8a55c30 --- /dev/null +++ b/src/robot/localization/src/localization_node.cpp @@ -0,0 +1,236 @@ +// Copyright (c) 2025-present WATonomous. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "localization/localization_node.hpp" + +#include +#include +#include +#include + +#include "tf2/LinearMath/Matrix3x3.h" +#include "tf2/LinearMath/Quaternion.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +LocalizationNode::LocalizationNode() +: Node("localization_node") +, ekf_(this->get_logger()) +, has_cmd_vel_(false) +, has_imu_(false) +{ + processParameters(); + + // Create publisher + odom_pub_ = this->create_publisher(odom_topic_, 10); + + // Create subscribers + gps_sub_ = this->create_subscription( + gps_topic_, 10, std::bind(&LocalizationNode::gpsCallback, this, std::placeholders::_1)); + + imu_sub_ = this->create_subscription( + imu_topic_, 10, std::bind(&LocalizationNode::imuCallback, this, std::placeholders::_1)); + + cmd_vel_sub_ = this->create_subscription( + cmd_vel_topic_, 10, std::bind(&LocalizationNode::cmdVelCallback, this, std::placeholders::_1)); + + // Create TF buffer and listener (for initialization) + if (use_tf_for_init_) { + tf_buffer_ = std::make_shared(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + } + + // Create timer for EKF updates + int period_ms = static_cast(1000.0 / ekf_update_rate_); + timer_ = + this->create_wall_timer(std::chrono::milliseconds(period_ms), std::bind(&LocalizationNode::timerCallback, this)); + + last_update_time_ = this->now(); + + RCLCPP_INFO(this->get_logger(), "Localization Node initialized"); + RCLCPP_INFO(this->get_logger(), " GPS topic: %s", gps_topic_.c_str()); + RCLCPP_INFO(this->get_logger(), " IMU topic: %s", imu_topic_.c_str()); + RCLCPP_INFO(this->get_logger(), " Cmd vel topic: %s", cmd_vel_topic_.c_str()); + RCLCPP_INFO(this->get_logger(), " EKF update rate: %.1f Hz", ekf_update_rate_); +} + +void LocalizationNode::processParameters() +{ + // Declare parameters + this->declare_parameter("gps_topic", "/gps/fix"); + this->declare_parameter("imu_topic", "/imu/simulated"); + this->declare_parameter("cmd_vel_topic", "/cmd_vel"); + this->declare_parameter("odom_topic", "/odom/ekf_filtered"); + this->declare_parameter("source_frame", "sim_world"); + this->declare_parameter("target_frame", "robot/chassis/camera"); + this->declare_parameter("process_noise_position", 0.1); + this->declare_parameter("process_noise_orientation", 0.05); + this->declare_parameter("process_noise_velocity", 0.1); + this->declare_parameter("gps_measurement_noise", 0.5); + this->declare_parameter("imu_measurement_noise_orientation", 0.001); + this->declare_parameter("imu_measurement_noise_angular_vel", 0.001); + this->declare_parameter("ekf_update_rate", 10.0); + this->declare_parameter("use_tf_for_init", true); + + // Get parameters + gps_topic_ = this->get_parameter("gps_topic").as_string(); + imu_topic_ = this->get_parameter("imu_topic").as_string(); + cmd_vel_topic_ = this->get_parameter("cmd_vel_topic").as_string(); + odom_topic_ = this->get_parameter("odom_topic").as_string(); + source_frame_ = this->get_parameter("source_frame").as_string(); + target_frame_ = this->get_parameter("target_frame").as_string(); + process_noise_position_ = this->get_parameter("process_noise_position").as_double(); + process_noise_orientation_ = this->get_parameter("process_noise_orientation").as_double(); + process_noise_velocity_ = this->get_parameter("process_noise_velocity").as_double(); + gps_measurement_noise_ = this->get_parameter("gps_measurement_noise").as_double(); + imu_measurement_noise_orientation_ = this->get_parameter("imu_measurement_noise_orientation").as_double(); + imu_measurement_noise_angular_vel_ = this->get_parameter("imu_measurement_noise_angular_vel").as_double(); + ekf_update_rate_ = this->get_parameter("ekf_update_rate").as_double(); + use_tf_for_init_ = this->get_parameter("use_tf_for_init").as_bool(); +} + +void LocalizationNode::timerCallback() +{ + // Initialize EKF from TF if not initialized and TF is enabled + if (!ekf_.isInitialized() && use_tf_for_init_ && tf_buffer_) { + geometry_msgs::msg::TransformStamped transform_stamped; + try { + transform_stamped = tf_buffer_->lookupTransform(source_frame_, target_frame_, tf2::TimePointZero); + + double x = transform_stamped.transform.translation.x; + double y = transform_stamped.transform.translation.y; + double theta = quaternionToYaw( + transform_stamped.transform.rotation.x, + transform_stamped.transform.rotation.y, + transform_stamped.transform.rotation.z, + transform_stamped.transform.rotation.w); + + ekf_.initialize(x, y, theta); + last_update_time_ = this->now(); + RCLCPP_INFO(this->get_logger(), "EKF initialized from TF: x=%.2f, y=%.2f, theta=%.2f", x, y, theta); + return; + } catch (const tf2::TransformException & ex) { + RCLCPP_DEBUG_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, "Waiting for TF transform for initialization: %s", ex.what()); + return; + } + } + + if (!ekf_.isInitialized()) { + return; + } + + // Compute time delta + rclcpp::Time current_time = this->now(); + double dt = (current_time - last_update_time_).seconds(); + if (dt <= 0.0) { + return; + } + + // Predict using wheel odometry (cmd_vel) + if (has_cmd_vel_) { + double vx = latest_cmd_vel_.linear.x; + double vy = latest_cmd_vel_.linear.y; + double omega = latest_cmd_vel_.angular.z; + + ekf_.predict(vx, vy, omega, dt, process_noise_position_, process_noise_orientation_, process_noise_velocity_); + } + + // Publish filtered odometry + nav_msgs::msg::Odometry filtered_odom; + ekf_.getOdometry(filtered_odom, source_frame_, target_frame_, current_time); + odom_pub_->publish(filtered_odom); + + last_update_time_ = current_time; +} + +void LocalizationNode::gpsCallback(const sensor_msgs::msg::NavSatFix::SharedPtr msg) +{ + if (!ekf_.isInitialized()) { + return; + } + + // Check GPS status + if (msg->status.status != sensor_msgs::msg::NavSatStatus::STATUS_FIX) { + return; + } + + // Extract measurement noise from covariance if available + double measurement_noise = gps_measurement_noise_; + if ( + msg->position_covariance_type == sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN || + msg->position_covariance_type == sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_KNOWN) + { + double variance = msg->position_covariance[0]; + if (variance > 0.0) { + measurement_noise = std::sqrt(variance); + } + } + + // Update EKF with GPS + ekf_.updateGPS(msg->latitude, msg->longitude, measurement_noise); +} + +void LocalizationNode::imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg) +{ + latest_imu_ = *msg; + has_imu_ = true; + + if (!ekf_.isInitialized()) { + return; + } + + // Extract orientation + double theta = quaternionToYaw(msg->orientation.x, msg->orientation.y, msg->orientation.z, msg->orientation.w); + + // Extract measurement noise from covariance + double orientation_noise = imu_measurement_noise_orientation_; + if (msg->orientation_covariance[8] > 0.0) { + orientation_noise = std::sqrt(msg->orientation_covariance[8]); + } + + // Update EKF with IMU orientation + ekf_.updateIMUOrientation(theta, orientation_noise); + + // Update EKF with IMU angular velocity + double angular_vel_noise = imu_measurement_noise_angular_vel_; + if (msg->angular_velocity_covariance[8] > 0.0) { + angular_vel_noise = std::sqrt(msg->angular_velocity_covariance[8]); + } + + ekf_.updateIMUAngularVel(msg->angular_velocity.z, angular_vel_noise); +} + +void LocalizationNode::cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg) +{ + latest_cmd_vel_ = *msg; + has_cmd_vel_ = true; + last_cmd_vel_time_ = this->now(); +} + +double LocalizationNode::quaternionToYaw(double x, double y, double z, double w) +{ + tf2::Quaternion q(x, y, z, w); + tf2::Matrix3x3 m(q); + double roll, pitch, yaw; + m.getRPY(roll, pitch, yaw); + return yaw; +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/robot/map_memory/CMakeLists.txt b/src/robot/map_memory/CMakeLists.txt index 9807196..a8c426d 100644 --- a/src/robot/map_memory/CMakeLists.txt +++ b/src/robot/map_memory/CMakeLists.txt @@ -26,6 +26,9 @@ endif() # Search for dependencies required for building this package find_package(ament_cmake REQUIRED) # ROS2 build tool find_package(rclcpp REQUIRED) # ROS2 C++ package +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2 REQUIRED) # Compiles source files into a library # A library is not executed, instead other executables can link @@ -40,6 +43,9 @@ target_include_directories(map_memory_lib # Add ROS2 dependencies required by package ament_target_dependencies(map_memory_lib rclcpp + nav_msgs + geometry_msgs + tf2 ) # Create ROS2 node executable from source files diff --git a/src/robot/map_memory/config/params.yaml b/src/robot/map_memory/config/params.yaml index 0808832..40c9c93 100644 --- a/src/robot/map_memory/config/params.yaml +++ b/src/robot/map_memory/config/params.yaml @@ -1,2 +1,24 @@ -# map_memory_node: -# ros__parameters: +map_memory_node: + ros__parameters: + local_costmap_topic: "/costmap" # local costmap topic with which map memory node subscribes to + # odom_topic: "/odom/filtered" # odometry topic with which map memory node subscribes to + map_topic: "/map" # map topic with which the map will be published to + + # ms, rate at which the map should be published + # for simplicity, this is also the rate the A* planner plans at + map_pub_rate: 3000 + # m, distance travelled by the robot at which the map will update + update_distance: 1.5 + + global_map: + resolution: 0.5 + width: 60 + height: 60 + + origin: + position: + x: -15.0 # given by -1 * width * resolution / 2 + y: -15.0 # given by -1 * height * resolution / 2 + z: 0.3 # height offset to render map above URDF mesh (in meters) + orientation: + w: 1.0 diff --git a/src/robot/map_memory/include/map_memory_core.hpp b/src/robot/map_memory/include/map_memory_core.hpp new file mode 100644 index 0000000..dbecac6 --- /dev/null +++ b/src/robot/map_memory/include/map_memory_core.hpp @@ -0,0 +1,46 @@ +// Copyright (c) 2025-present WATonomous. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MAP_MEMORY_CORE_HPP_ +#define MAP_MEMORY_CORE_HPP_ + +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace robot +{ + +class MapMemoryCore +{ +public: + explicit MapMemoryCore(const rclcpp::Logger & logger); + + void initMapMemory(double resolution, int width, int height, geometry_msgs::msg::Pose origin); + + void updateMap( + nav_msgs::msg::OccupancyGrid::SharedPtr local_costmap, double robot_x, double robot_y, double robot_theta); + + bool robotToMap(double rx, double ry, int & mx, int & my); + + // Retrieves map data + nav_msgs::msg::OccupancyGrid::SharedPtr getMapData() const; + +private: + nav_msgs::msg::OccupancyGrid::SharedPtr global_map_; + rclcpp::Logger logger_; +}; + +} // namespace robot + +#endif diff --git a/src/robot/map_memory/include/map_memory_node.hpp b/src/robot/map_memory/include/map_memory_node.hpp new file mode 100644 index 0000000..991f2a5 --- /dev/null +++ b/src/robot/map_memory/include/map_memory_node.hpp @@ -0,0 +1,78 @@ +// Copyright (c) 2025-present WATonomous. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MAP_MEMORY_NODE_HPP_ +#define MAP_MEMORY_NODE_HPP_ + +#include +#include +#include +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "map_memory/map_memory_core.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp/rclcpp.hpp" + +class MapMemoryNode : public rclcpp::Node +{ +public: + MapMemoryNode(); + + void localCostmapCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg); + void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg); + void timerCallback(); + + // Internal utility to convert quaternion to yaw + double quaternionToYaw(double x, double y, double z, double w); + + void processParameters(); + +private: + // core logic for processing the global costmap + robot::MapMemoryCore map_memory_; + + // ROS2 Constructs + rclcpp::Subscription::SharedPtr local_costmap_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Publisher::SharedPtr global_costmap_pub_; + rclcpp::TimerBase::SharedPtr timer_; + + // ROS2 Parameters + std::string local_costmap_topic_; + std::string odom_topic_; + std::string map_topic_; + + int map_pub_rate_; + + double resolution_; + int width_; + int height_; + geometry_msgs::msg::Pose origin_; + + double update_distance_; + + // To keep track of robot pose in sim frame + double robot_x_; // [m] + double robot_y_; // [m] + double robot_theta_; // [rad] + + // Last position used to check if robot moved > distance_ + double last_robot_x_; + double last_robot_y_; +}; + +#endif diff --git a/src/robot/map_memory/package.xml b/src/robot/map_memory/package.xml index 748c2da..cd09449 100644 --- a/src/robot/map_memory/package.xml +++ b/src/robot/map_memory/package.xml @@ -10,6 +10,9 @@ ament_cmake rclcpp + geometry_msgs + nav_msgs + tf2 ament_lint_auto ament_lint_common diff --git a/src/robot/map_memory/src/map_memory_core.cpp b/src/robot/map_memory/src/map_memory_core.cpp index 7c6c230..c63614f 100644 --- a/src/robot/map_memory/src/map_memory_core.cpp +++ b/src/robot/map_memory/src/map_memory_core.cpp @@ -14,11 +14,112 @@ #include "map_memory/map_memory_core.hpp" +#include + namespace robot { MapMemoryCore::MapMemoryCore(const rclcpp::Logger & logger) -: logger_(logger) +: global_map_(std::make_shared()) +, logger_(logger) {} +void MapMemoryCore::initMapMemory(double resolution, int width, int height, geometry_msgs::msg::Pose origin) +{ + global_map_->info.resolution = resolution; + global_map_->info.width = width; + global_map_->info.height = height; + global_map_->info.origin = origin; + global_map_->data.assign(width * height, 0); + + RCLCPP_INFO( + logger_, "Global Map initialized with resolution: %.2f, width: %d, height: %d", resolution, width, height); +} + +void MapMemoryCore::updateMap( + nav_msgs::msg::OccupancyGrid::SharedPtr local_costmap, double robot_x, double robot_y, double robot_theta) +{ + // Get local costmap specs + double local_res = local_costmap->info.resolution; + double local_origin_x = local_costmap->info.origin.position.x; + double local_origin_y = local_costmap->info.origin.position.y; + unsigned int local_w = local_costmap->info.width; + unsigned int local_h = local_costmap->info.height; + const auto & local_data = local_costmap->data; + + // For each cell in local costmap, transform to sim_world + for (unsigned int j = 0; j < local_h; ++j) { + for (unsigned int i = 0; i < local_w; ++i) { + int8_t occ_val = local_data[j * local_w + i]; + if (occ_val < 0) { + // Unknown => skip or handle differently + continue; + } + // Convert (i,j) to local metric coords relative to "robot" frame + double lx = local_origin_x + (i + 0.5) * local_res; // center of cell + double ly = local_origin_y + (j + 0.5) * local_res; + + // Now transform (lx, ly) from "robot" frame to "sim_world" frame + // using the robot pose (robot_x_, robot_y_, robot_theta_). + // Basic 2D transform: + // wx = robot_x + cos(theta)*lx - sin(theta)*ly + // wy = robot_y + sin(theta)*lx + cos(theta)*ly + double cos_t = std::cos(robot_theta); + double sin_t = std::sin(robot_theta); + double wx = robot_x + (lx * cos_t - ly * sin_t); + double wy = robot_y + (lx * sin_t + ly * cos_t); + + // Convert (wx, wy) to indices in the global map + int gx, gy; + if (!robotToMap(wx, wy, gx, gy)) { + // Out of global map bounds + continue; + } + + // --- Take the max cost instead of direct overwrite --- + int8_t & global_val = global_map_->data[gy * global_map_->info.width + gx]; + + // If the global cell is unknown (-1), treat that as 0 when taking max. + int current_global_cost = (global_val < 0) ? 0 : global_val; + int local_cost = static_cast(occ_val); + + // Merge by taking the maximum: + int merged_cost = std::max(current_global_cost, local_cost); + + // If merged_cost is still 0 but occ_val is not, it means local cost was 0 or unknown + // In that case, we might want to handle it differently; for simple max, we just do this: + global_val = static_cast(merged_cost); + } + } +} + +bool MapMemoryCore::robotToMap(double rx, double ry, int & mx, int & my) +{ + double origin_x = global_map_->info.origin.position.x; + double origin_y = global_map_->info.origin.position.y; + double resolution = global_map_->info.resolution; + + // offset from origin + if (rx < origin_x || ry < origin_y) { + return false; + } + + my = static_cast((ry - origin_y) / resolution); + mx = static_cast((rx - origin_x) / resolution); + + if ( + mx < 0 || mx >= static_cast(global_map_->info.width) || my < 0 || + my >= static_cast(global_map_->info.height)) + { + return false; + } + return true; +} + +// Retrieves map data +nav_msgs::msg::OccupancyGrid::SharedPtr MapMemoryCore::getMapData() const +{ + return global_map_; +} + } // namespace robot diff --git a/src/robot/map_memory/src/map_memory_node.cpp b/src/robot/map_memory/src/map_memory_node.cpp index fa58e9d..62aed53 100644 --- a/src/robot/map_memory/src/map_memory_node.cpp +++ b/src/robot/map_memory/src/map_memory_node.cpp @@ -14,10 +14,129 @@ #include "map_memory/map_memory_node.hpp" +#include + +#include "tf2/LinearMath/Matrix3x3.h" +#include "tf2/LinearMath/Quaternion.h" + MapMemoryNode::MapMemoryNode() : Node("map_memory") , map_memory_(robot::MapMemoryCore(this->get_logger())) -{} +{ + // load ROS2 yaml parameters + processParameters(); + + // Subscribe to local costmap + local_costmap_sub_ = this->create_subscription( + local_costmap_topic_, 10, std::bind(&MapMemoryNode::localCostmapCallback, this, std::placeholders::_1)); + + // Subscribe to odometry + odom_sub_ = this->create_subscription( + odom_topic_, 10, std::bind(&MapMemoryNode::odomCallback, this, std::placeholders::_1)); + + // Publish a global costmap for downstream path planning + global_costmap_pub_ = this->create_publisher(map_topic_, 10); + + timer_ = + this->create_wall_timer(std::chrono::milliseconds(map_pub_rate_), std::bind(&MapMemoryNode::timerCallback, this)); + + map_memory_.initMapMemory(resolution_, width_, height_, origin_); + + RCLCPP_INFO(this->get_logger(), "Initialized Map Memory Core"); +} + +void MapMemoryNode::processParameters() +{ + // Declare all ROS2 Parameters + this->declare_parameter("local_costmap_topic", "/costmap"); + this->declare_parameter("odom_topic", "/odom/filtered"); + this->declare_parameter("map_topic", "/map"); + this->declare_parameter("map_pub_rate", 500); + this->declare_parameter("update_distance", 1.0); + this->declare_parameter("global_map.resolution", 0.1); + this->declare_parameter("global_map.width", 100); + this->declare_parameter("global_map.height", 100); + this->declare_parameter("global_map.origin.position.x", -5.0); + this->declare_parameter("global_map.origin.position.y", -5.0); + this->declare_parameter("global_map.origin.position.z", 0.1); + this->declare_parameter("global_map.origin.orientation.w", 1.0); + + // Retrieve parameters and store them in member variables + local_costmap_topic_ = this->get_parameter("local_costmap_topic").as_string(); + odom_topic_ = this->get_parameter("odom_topic").as_string(); + map_topic_ = this->get_parameter("map_topic").as_string(); + map_pub_rate_ = this->get_parameter("map_pub_rate").as_int(); + update_distance_ = this->get_parameter("update_distance").as_double(); + resolution_ = this->get_parameter("global_map.resolution").as_double(); + width_ = this->get_parameter("global_map.width").as_int(); + height_ = this->get_parameter("global_map.height").as_int(); + origin_.position.x = this->get_parameter("global_map.origin.position.x").as_double(); + origin_.position.y = this->get_parameter("global_map.origin.position.y").as_double(); + origin_.position.z = this->get_parameter("global_map.origin.position.z").as_double(); + origin_.orientation.w = this->get_parameter("global_map.origin.orientation.w").as_double(); +} + +void MapMemoryNode::localCostmapCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) +{ + bool all_zero = std::all_of(msg->data.begin(), msg->data.end(), [](int8_t val) { return val == 0; }); + if (all_zero) { + RCLCPP_INFO(this->get_logger(), "All elements in the array are zero."); + return; + } + + // Check how far the robot has moved since last update + if (!std::isnan(last_robot_x_)) { + double dist = std::hypot(robot_x_ - last_robot_x_, robot_y_ - last_robot_y_); + if (dist < update_distance_) { + // Robot hasn’t moved enough, skip updating the global map + return; + } + } + + // Update last position + last_robot_x_ = robot_x_; + last_robot_y_ = robot_y_; + + // Update the global map + map_memory_.updateMap(msg, robot_x_, robot_y_, robot_theta_); +} + +void MapMemoryNode::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) +{ + // Extract the robot’s position and orientation from the Odometry message. + // Assume this odometry is in the "sim_world" frame or a frame equivalent to it. + robot_x_ = msg->pose.pose.position.x; + robot_y_ = msg->pose.pose.position.y; + + // Convert quaternion to yaw + double qx = msg->pose.pose.orientation.x; + double qy = msg->pose.pose.orientation.y; + double qz = msg->pose.pose.orientation.z; + double qw = msg->pose.pose.orientation.w; + robot_theta_ = quaternionToYaw(qx, qy, qz, qw); +} + +void MapMemoryNode::timerCallback() +{ + // Publish the map every map_pub_rate [ms] + nav_msgs::msg::OccupancyGrid map_msg = *map_memory_.getMapData(); + map_msg.header.stamp = this->now(); + map_msg.header.frame_id = "sim_world"; + // Set z-offset to render map slightly above ground plane (on top of URDF) + map_msg.info.origin.position.z = origin_.position.z; + global_costmap_pub_->publish(map_msg); +} + +// Utility: Convert quaternion to yaw +double MapMemoryNode::quaternionToYaw(double x, double y, double z, double w) +{ + // Using tf2 to convert to RPY + tf2::Quaternion q(x, y, z, w); + tf2::Matrix3x3 m(q); + double roll, pitch, yaw; + m.getRPY(roll, pitch, yaw); + return yaw; +} int main(int argc, char ** argv) { diff --git a/src/robot/odometry_spoof/include/odometry_spoof.hpp b/src/robot/odometry_spoof/include/odometry_spoof.hpp new file mode 100644 index 0000000..13dc62e --- /dev/null +++ b/src/robot/odometry_spoof/include/odometry_spoof.hpp @@ -0,0 +1,56 @@ +// Copyright (c) 2025-present WATonomous. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ODOMETRY_SPOOF_NODE_HPP_ +#define ODOMETRY_SPOOF_NODE_HPP_ + +#include + +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2/LinearMath/Matrix3x3.h" +#include "tf2/LinearMath/Quaternion.h" +#include "tf2/LinearMath/Vector3.h" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +class OdometrySpoofNode : public rclcpp::Node +{ +public: + OdometrySpoofNode(); + +private: + void timerCallback(); + + // Odom Publisher + rclcpp::Publisher::SharedPtr odom_pub_; + + // Transform Utilities + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + // Timer to periodically lookup transforms + rclcpp::TimerBase::SharedPtr timer_; + + // Check if last transform was found + bool has_last_transform_; + + // Vars to store previous transform + rclcpp::Time last_time_; + tf2::Vector3 last_position_; + tf2::Quaternion last_orientation_; +}; + +#endif diff --git a/src/robot/odometry_spoof/src/odometry_spoof.cpp b/src/robot/odometry_spoof/src/odometry_spoof.cpp index 392ef7c..6a19a80 100644 --- a/src/robot/odometry_spoof/src/odometry_spoof.cpp +++ b/src/robot/odometry_spoof/src/odometry_spoof.cpp @@ -36,10 +36,10 @@ OdometrySpoofNode::OdometrySpoofNode() void OdometrySpoofNode::timerCallback() { - // We'll look up the transform from sim_world -> robot/chassis/lidar, - // note robot frame is usually not the lidar sensor, but we do so to make - // this assignment easier - const std::string target_frame = "robot/chassis"; + // We'll look up the transform from sim_world -> robot/chassis/camera, + // note robot frame is usually not the camera sensor, but we do so to make this + // assignment easier + const std::string target_frame = "robot/chassis/camera"; const std::string source_frame = "sim_world"; geometry_msgs::msg::TransformStamped transform_stamped; diff --git a/src/robot/planner/CMakeLists.txt b/src/robot/planner/CMakeLists.txt index c82ea07..a2574eb 100644 --- a/src/robot/planner/CMakeLists.txt +++ b/src/robot/planner/CMakeLists.txt @@ -26,6 +26,8 @@ endif() # Search for dependencies required for building this package find_package(ament_cmake REQUIRED) # ROS2 build tool find_package(rclcpp REQUIRED) # ROS2 C++ package +find_package(nav_msgs REQUIRED) # ROS2 C++ package +find_package(geometry_msgs REQUIRED) # ROS2 C++ package # Compiles source files into a library # A library is not executed, instead other executables can link @@ -38,7 +40,7 @@ add_library(planner_lib target_include_directories(planner_lib PUBLIC include) # Add ROS2 dependencies required by package -ament_target_dependencies(planner_lib rclcpp) +ament_target_dependencies(planner_lib rclcpp nav_msgs geometry_msgs) # Create ROS2 node executable from source files add_executable(planner_node src/planner_node.cpp) diff --git a/src/robot/planner/config/params.yaml b/src/robot/planner/config/params.yaml index b86127e..c5d1176 100644 --- a/src/robot/planner/config/params.yaml +++ b/src/robot/planner/config/params.yaml @@ -1,2 +1,14 @@ -# planner_node: -# ros__parameters: +planner_node: + ros__parameters: + map_topic: "/map" + goal_topic: "/goal_point" + # odom_topic: "/odom/filtered" + path_topic: "/path" + + # Path planning params + smoothing_factor: 0.2 + iterations: 20 + + # Path start-end params + goal_tolerance: 1.5 + plan_timeout_seconds: 60.0 diff --git a/src/robot/planner/include/planner_core.hpp b/src/robot/planner/include/planner_core.hpp index 013d409..24a39bb 100644 --- a/src/robot/planner/include/planner_core.hpp +++ b/src/robot/planner/include/planner_core.hpp @@ -15,18 +15,134 @@ #ifndef PLANNER_CORE_HPP_ #define PLANNER_CORE_HPP_ +#include +#include +#include +#include +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav_msgs/msg/path.hpp" #include "rclcpp/rclcpp.hpp" namespace robot { +// ------------------- Supporting Structures ------------------- + +// 2D grid index +struct CellIndex +{ + int x; + int y; + + CellIndex(int xx, int yy) + : x(xx) + , y(yy) + {} + + CellIndex() + : x(0) + , y(0) + {} + + bool operator==(const CellIndex & other) const + { + return (x == other.x && y == other.y); + } + + bool operator!=(const CellIndex & other) const + { + return (x != other.x || y != other.y); + } +}; + +// Hash function for CellIndex so it can be used in std::unordered_map +struct CellIndexHash +{ + std::size_t operator()(const CellIndex & idx) const + { + // A simple hash combining x and y + return std::hash()(idx.x) ^ (std::hash()(idx.y) << 1); + } +}; + +// Structure representing a node in the A* open set +struct AStarNode +{ + CellIndex index; + double f_score; // f = g + h + + AStarNode(CellIndex idx, double f) + : index(idx) + , f_score(f) + {} +}; + +// Comparator for the priority queue (min-heap by f_score) +struct CompareF +{ + bool operator()(const AStarNode & a, const AStarNode & b) + { + // We want the node with the smallest f_score on top + return a.f_score > b.f_score; + } +}; + class PlannerCore { public: explicit PlannerCore(const rclcpp::Logger & logger); + void initPlanner(double smoothing_factor, int iterations); + + bool planPath( + double start_world_x, + double start_world_y, + double goal_x, + double goal_y, + nav_msgs::msg::OccupancyGrid::SharedPtr map); + + bool doAStar(const CellIndex & start_idx, const CellIndex & goal_idx, std::vector & out_path); + + // Reconstructs path by backtracking cameFrom + void reconstructPath( + const std::unordered_map & cameFrom, + const CellIndex & current, + std::vector & out_path); + + // 8-direction neighbors + std::vector getNeighbors8(const CellIndex & c); + + // Euclidean heuristic + double euclideanHeuristic(const CellIndex & a, const CellIndex & b); + + // Step cost for orth vs diagonal moves + double stepDistance(const CellIndex & a, const CellIndex & b); + + void lineOfSightSmoothing(std::vector & path_cells); + + // Convert world coordinates to map indices + bool poseToMap(double wx, double wy, CellIndex & out_idx); + + // Convert map cell to world coordinates + void mapToPose(const CellIndex & idx, double & wx, double & wy); + + bool lineOfSight(const CellIndex & start, const CellIndex & end); + + bool isCellFree(int x, int y); + + nav_msgs::msg::Path::SharedPtr getPath() const; + private: + double smoothing_factor_; + int iterations_; + rclcpp::Logger logger_; + nav_msgs::msg::OccupancyGrid::SharedPtr map_; + nav_msgs::msg::Path::SharedPtr path_; }; } // namespace robot diff --git a/src/robot/planner/include/planner_node.hpp b/src/robot/planner/include/planner_node.hpp index 1df5499..5e16a04 100644 --- a/src/robot/planner/include/planner_node.hpp +++ b/src/robot/planner/include/planner_node.hpp @@ -15,6 +15,13 @@ #ifndef PLANNER_NODE_HPP_ #define PLANNER_NODE_HPP_ +#include +#include + +#include "geometry_msgs/msg/point_stamped.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "nav_msgs/msg/path.hpp" #include "planner/planner_core.hpp" #include "rclcpp/rclcpp.hpp" @@ -23,8 +30,55 @@ class PlannerNode : public rclcpp::Node public: PlannerNode(); + void processParameters(); + + void mapCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr map_msg); + void goalCallback(const geometry_msgs::msg::PointStamped::SharedPtr goal_msg); + void odomCallback(const nav_msgs::msg::Odometry::SharedPtr odom_msg); + void timerCallback(); + + void publishPath(); + void resetGoal(); + private: robot::PlannerCore planner_; + + // Subscriptions + rclcpp::Subscription::SharedPtr map_sub_; + rclcpp::Subscription::SharedPtr goal_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + + // Publisher + rclcpp::Publisher::SharedPtr path_pub_; + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + + // Costmap & Mutex + nav_msgs::msg::OccupancyGrid::SharedPtr map_; + std::mutex map_mutex_; + + // Current goal + geometry_msgs::msg::PointStamped current_goal_; + bool active_goal_; + rclcpp::Time plan_start_time_; + + // Robot odometry (x,y). For simplicity, ignoring orientation usage here. + bool have_odom_; + double odom_x_; + double odom_y_; + + // Parameters + std::string map_topic_; + std::string goal_topic_; + std::string odom_topic_; + std::string path_topic_; + + double smoothing_factor_; + int iterations_; + + double goal_tolerance_; + double plan_timeout_; }; #endif diff --git a/src/robot/planner/package.xml b/src/robot/planner/package.xml index db62085..f5475b0 100644 --- a/src/robot/planner/package.xml +++ b/src/robot/planner/package.xml @@ -10,6 +10,8 @@ ament_cmake rclcpp + geometry_msgs + nav_msgs ament_lint_auto ament_lint_common diff --git a/src/robot/planner/src/planner_core.cpp b/src/robot/planner/src/planner_core.cpp index 2c1f921..ed98b83 100644 --- a/src/robot/planner/src/planner_core.cpp +++ b/src/robot/planner/src/planner_core.cpp @@ -14,11 +14,264 @@ #include "planner/planner_core.hpp" +#include +#include +#include +#include + namespace robot { PlannerCore::PlannerCore(const rclcpp::Logger & logger) -: logger_(logger) +: path_(std::make_shared()) +, map_(std::make_shared()) +, logger_(logger) {} +void PlannerCore::initPlanner(double smoothing_factor, int iterations) +{ + smoothing_factor_ = smoothing_factor; + iterations_ = iterations; +} + +bool PlannerCore::planPath( + double start_world_x, double start_world_y, double goal_x, double goal_y, nav_msgs::msg::OccupancyGrid::SharedPtr map) +{ + map_ = map; + + // Convert current goal to map indices + CellIndex goal_idx; + if (!poseToMap(goal_x, goal_y, goal_idx)) { + RCLCPP_WARN(logger_, "Goal is out of costmap bounds. Aborting."); + return false; + } + + CellIndex start_idx; + if (!poseToMap(start_world_x, start_world_y, start_idx)) { + RCLCPP_WARN(logger_, "Start is out of costmap bounds. Aborting."); + return false; + } + + RCLCPP_INFO( + logger_, + "Planning from odom start (%0.2f, %0.2f) => cell (%d, %d) to goal (%d, %d).", + start_world_x, + start_world_y, + start_idx.x, + start_idx.y, + goal_idx.x, + goal_idx.y); + + // Run A* + std::vector path_cells; + bool success = doAStar(start_idx, goal_idx, path_cells); + + if (!success) { + RCLCPP_WARN(logger_, "A* failed to find a path."); + return false; + } + + // Convert path cells to nav_msgs::Path + if (path_->poses.size() > 0) { + path_->poses.clear(); + } + + for (auto & cell : path_cells) { + geometry_msgs::msg::PoseStamped pose_stamped; + pose_stamped.header = map_->header; + + double wx, wy; + mapToPose(cell, wx, wy); + + pose_stamped.pose.position.x = wx; + pose_stamped.pose.position.y = wy; + pose_stamped.pose.orientation.w = 1.0; // simple orientation + + path_->poses.push_back(pose_stamped); + } + + return true; +} + +bool PlannerCore::doAStar(const CellIndex & start_idx, const CellIndex & goal_idx, std::vector & out_path) +{ + const int width = map_->info.width; + const int height = map_->info.height; + + // Data structures for A* + std::unordered_map gScore; + std::unordered_map cameFrom; + std::unordered_map fScore; + + auto setScore = [&](auto & storage, const CellIndex & idx, double val) { storage[idx] = val; }; + auto getScore = [&](auto & storage, const CellIndex & idx) { + auto it = storage.find(idx); + if (it == storage.end()) { + return std::numeric_limits::infinity(); + } + return it->second; + }; + + // Helper to retrieve cell cost from costmap + auto cellCost = [&](const CellIndex & idx) { + if (idx.x < 0 || idx.x >= width || idx.y < 0 || idx.y >= height) { + return 127; // out of bounds => treat as high cost + } + int map_index = idx.y * width + idx.x; + int8_t val = map_->data[map_index]; + // Unknown => treat as high cost + if (val < 0) { + val = 100; + } + return static_cast(val); + }; + + // Initialize start node + setScore(gScore, start_idx, 0.0); + double h_start = euclideanHeuristic(start_idx, goal_idx); + setScore(fScore, start_idx, h_start); + + // Open set (min-heap by f_score) + std::priority_queue, CompareF> openSet; + openSet.push(AStarNode(start_idx, h_start)); + + while (!openSet.empty()) { + AStarNode current = openSet.top(); + openSet.pop(); + CellIndex cidx = current.index; + + // Goal check + if (cidx == goal_idx) { + reconstructPath(cameFrom, cidx, out_path); + return true; + } + + double current_g = getScore(gScore, cidx); + + // Check neighbors (8-way) + auto neighbors = getNeighbors8(cidx); + for (auto & nb : neighbors) { + // Skip out of bounds quickly + if (nb.x < 0 || nb.x >= width || nb.y < 0 || nb.y >= height) { + continue; + } + + // If cell cost is too high => treat as obstacle + int cost_val = cellCost(nb); + if (cost_val > 90) { + continue; + } + + // Step cost: 1.0 orth, sqrt(2) diag + double step_cost = stepDistance(cidx, nb); + // Add a penalty from costmap cell value (simple scale) + double penalty = cost_val / 25.0; + + double tentative_g = current_g + step_cost + penalty; + double old_g = getScore(gScore, nb); + + if (tentative_g < old_g) { + setScore(gScore, nb, tentative_g); + double h = euclideanHeuristic(nb, goal_idx); + double f = tentative_g + h; + setScore(fScore, nb, f); + + cameFrom[nb] = cidx; + openSet.push(AStarNode(nb, f)); + } + } + } + + return false; // No path found +} + +void PlannerCore::reconstructPath( + const std::unordered_map & cameFrom, + const CellIndex & current, + std::vector & out_path) +{ + out_path.clear(); + CellIndex c = current; + out_path.push_back(c); + + auto it = cameFrom.find(c); + while (it != cameFrom.end()) { + c = it->second; + out_path.push_back(c); + it = cameFrom.find(c); + } + std::reverse(out_path.begin(), out_path.end()); +} + +std::vector PlannerCore::getNeighbors8(const CellIndex & c) +{ + std::vector result; + result.reserve(8); + for (int dx = -1; dx <= 1; dx++) { + for (int dy = -1; dy <= 1; dy++) { + if (dx == 0 && dy == 0) { + continue; + } + result.push_back(CellIndex(c.x + dx, c.y + dy)); + } + } + return result; +} + +double PlannerCore::euclideanHeuristic(const CellIndex & a, const CellIndex & b) +{ + double dx = static_cast(a.x - b.x); + double dy = static_cast(a.y - b.y); + return std::sqrt(dx * dx + dy * dy); +} + +double PlannerCore::stepDistance(const CellIndex & a, const CellIndex & b) +{ + int dx = std::abs(a.x - b.x); + int dy = std::abs(a.y - b.y); + if (dx + dy == 2) { + // diagonal + return std::sqrt(2.0); + } else { + // orth + return 1.0; + } +} + +bool PlannerCore::poseToMap(double wx, double wy, CellIndex & out_idx) +{ + double origin_x = map_->info.origin.position.x; + double origin_y = map_->info.origin.position.y; + double res = map_->info.resolution; + + double mx = (wx - origin_x) / res; + double my = (wy - origin_y) / res; + + int ix = static_cast(std::floor(mx)); + int iy = static_cast(std::floor(my)); + + if (ix < 0 || ix >= static_cast(map_->info.width) || iy < 0 || iy >= static_cast(map_->info.height)) { + return false; + } + + out_idx.x = ix; + out_idx.y = iy; + return true; +} + +void PlannerCore::mapToPose(const CellIndex & idx, double & wx, double & wy) +{ + double origin_x = map_->info.origin.position.x; + double origin_y = map_->info.origin.position.y; + double res = map_->info.resolution; + + wx = origin_x + (idx.x + 0.5) * res; + wy = origin_y + (idx.y + 0.5) * res; +} + +nav_msgs::msg::Path::SharedPtr PlannerCore::getPath() const +{ + return path_; +} + } // namespace robot diff --git a/src/robot/planner/src/planner_node.cpp b/src/robot/planner/src/planner_node.cpp index 656c240..4f1e9a9 100644 --- a/src/robot/planner/src/planner_node.cpp +++ b/src/robot/planner/src/planner_node.cpp @@ -14,10 +14,179 @@ #include "planner/planner_node.hpp" +#include + PlannerNode::PlannerNode() : Node("planner") , planner_(robot::PlannerCore(this->get_logger())) -{} +{ + // load ROS2 yaml parameters + processParameters(); + + // Subscribers + map_sub_ = this->create_subscription( + map_topic_, 10, std::bind(&PlannerNode::mapCallback, this, std::placeholders::_1)); + + goal_sub_ = this->create_subscription( + goal_topic_, 10, std::bind(&PlannerNode::goalCallback, this, std::placeholders::_1)); + + // Subscribe to odometry from /odom/filtered + odom_sub_ = this->create_subscription( + odom_topic_, 10, std::bind(&PlannerNode::odomCallback, this, std::placeholders::_1)); + + // Publisher + path_pub_ = this->create_publisher(path_topic_, 10); + + // Timer to check goal/timeout status periodically (500 ms) + timer_ = this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&PlannerNode::timerCallback, this)); + + planner_.initPlanner(smoothing_factor_, iterations_); +} + +void PlannerNode::processParameters() +{ + // Declare and get parameters + this->declare_parameter("map_topic", "/map"); + this->declare_parameter("goal_topic", "/goal_pose"); + this->declare_parameter("odom_topic", "/odom/filtered"); + this->declare_parameter("path_topic", "/path"); + this->declare_parameter("smoothing_factor", 0.2); + this->declare_parameter("iterations", 20); + this->declare_parameter("goal_tolerance", 0.3); + this->declare_parameter("plan_timeout_seconds", 10.0); + + map_topic_ = this->get_parameter("map_topic").as_string(); + goal_topic_ = this->get_parameter("goal_topic").as_string(); + odom_topic_ = this->get_parameter("odom_topic").as_string(); + path_topic_ = this->get_parameter("path_topic").as_string(); + smoothing_factor_ = this->get_parameter("smoothing_factor").as_double(); + iterations_ = this->get_parameter("iterations").as_int(); + goal_tolerance_ = this->get_parameter("goal_tolerance").as_double(); + plan_timeout_ = this->get_parameter("plan_timeout_seconds").as_double(); +} + +void PlannerNode::mapCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) +{ + { + std::lock_guard lock(map_mutex_); + map_ = msg; + } + + // If we have an active goal, re-run plan + if (active_goal_) { + double elapsed = (now() - plan_start_time_).seconds(); + if (elapsed <= plan_timeout_) { + RCLCPP_INFO(this->get_logger(), "Map updated => Replanning for current goal (time elapsed: %.2f).", elapsed); + publishPath(); + } + } +} + +void PlannerNode::goalCallback(const geometry_msgs::msg::PointStamped::SharedPtr goal_msg) +{ + if (active_goal_) { + RCLCPP_WARN(this->get_logger(), "Ignoring new goal; a goal is already active."); + return; + } + + if (!map_) { + RCLCPP_WARN(this->get_logger(), "No costmap available yet. Cannot set goal."); + return; + } + + current_goal_ = *goal_msg; + active_goal_ = true; + plan_start_time_ = now(); + + RCLCPP_INFO(this->get_logger(), "Received new goal: (%.2f, %.2f)", goal_msg->point.x, goal_msg->point.y); + + publishPath(); +} + +void PlannerNode::odomCallback(const nav_msgs::msg::Odometry::SharedPtr odom_msg) +{ + // Store the latest odometry in (odom_x_, odom_y_) + // For simplicity, ignoring orientation or z + odom_x_ = odom_msg->pose.pose.position.x; + odom_y_ = odom_msg->pose.pose.position.y; + have_odom_ = true; +} + +void PlannerNode::timerCallback() +{ + if (!active_goal_) { + return; + } + + // Check if we've timed out + double elapsed = (now() - plan_start_time_).seconds(); + // if (elapsed > plan_timeout_) { + // RCLCPP_WARN(this->get_logger(), "Plan timed out after %.2f seconds. Resetting goal.", elapsed); + // resetGoal(); + // return; + // } + + // Check if we reached the goal + double distance = sqrt(pow(odom_x_ - current_goal_.point.x, 2) + pow(odom_y_ - current_goal_.point.y, 2)); + if (distance < goal_tolerance_) { + RCLCPP_WARN(this->get_logger(), "Plan succeeded! Elapsed Time: %.2f", elapsed); + resetGoal(); + return; + } +} + +void PlannerNode::publishPath() +{ + if (!have_odom_) { + RCLCPP_WARN(this->get_logger(), "No odometry received yet. Cannot plan."); + resetGoal(); + return; + } + + // Use the robot's odometry as the start pose + double start_world_x = odom_x_; + double start_world_y = odom_y_; + + { + std::lock_guard lock(map_mutex_); + if (!planner_.planPath(start_world_x, start_world_y, current_goal_.point.x, current_goal_.point.y, map_)) { + RCLCPP_ERROR(this->get_logger(), "Plan Failed."); + resetGoal(); + return; + } + } + + nav_msgs::msg::Path path_msg = *planner_.getPath(); + path_msg.header.stamp = this->now(); + path_msg.header.frame_id = map_->header.frame_id; + + path_pub_->publish(path_msg); +} + +void PlannerNode::resetGoal() +{ + active_goal_ = false; + RCLCPP_INFO(this->get_logger(), "Resetting active goal."); + + // Publish an empty path, which should tell the robot to stop or have no path + nav_msgs::msg::Path empty_path; + empty_path.header.stamp = this->now(); + + // Use the costmap frame if available; otherwise a default like "sim_world" + { + std::lock_guard lock(map_mutex_); + if (map_) { + empty_path.header.frame_id = map_->header.frame_id; + } else { + empty_path.header.frame_id = "sim_world"; + } + } + + // Publish the empty path + path_pub_->publish(empty_path); + + RCLCPP_INFO(this->get_logger(), "Published empty path to stop the robot."); +} int main(int argc, char ** argv) {