Ani and Ninja - WATO onboarding

src/robot/control/CMakeLists.txt

@@ -13,6 +13,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)
+find_package(geometry_msgs REQUIRED)
 
 # Compiles source files into a library
 # A library is not executed, instead other executables can link
@@ -31,10 +33,13 @@ ament_target_dependencies(control_lib
   rclcpp
 )
 
+ament_target_dependencies(control_lib rclcpp nav_msgs geometry_msgs)
+
 # Create ROS2 node executable from source files
 add_executable(control_node src/control_node.cpp)
 # Link to the previously built library to access Transformer classes and methods
 target_link_libraries(control_node control_lib)
+ament_target_dependencies(control_node rclcpp nav_msgs geometry_msgs)
 
 # Copy executable to installation location
 install(TARGETS

src/robot/control/include/control_node.hpp

@@ -1,6 +1,51 @@
 #ifndef CONTROL_NODE_HPP_
 #define CONTROL_NODE_HPP_
 
+#include "rclcpp/rclcpp.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include <optional>
+#include <mutex>
+
+class PurePursuitController : public rclcpp::Node {
+public:
+  PurePursuitController();
+
+private:
+  void pathCallback(const nav_msgs::msg::Path::SharedPtr msg);
+  void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg);
+  void controlLoop();
+
+  std::optional<geometry_msgs::msg::PoseStamped> findLookaheadPoint();
+  geometry_msgs::msg::Twist computeVelocity(const geometry_msgs::msg::PoseStamped &target);
+  double computeDistance(const geometry_msgs::msg::Point &a, const geometry_msgs::msg::Point &b);
+  double extractYaw(const geometry_msgs::msg::Quaternion &quat);
+
+  // ROS interfaces
+  rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr path_sub_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
+  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
+  rclcpp::TimerBase::SharedPtr control_timer_;
+
+  // Data
+  nav_msgs::msg::Path::SharedPtr current_path_;
+  nav_msgs::msg::Odometry::SharedPtr robot_odom_;
+  std::mutex mutex_;
+
+  // Parameters
+  double lookahead_distance_ = 1.0;  // meters
+  double goal_tolerance_ = 0.1;      // meters
+  double linear_speed_ = 0.5;        // m/s
+  double max_angular_speed_ = 1.5;   // rad/s
+  double control_rate_hz_ = 10.0;    // Hz
+};
+
+#endif
+#ifndef CONTROL_NODE_HPP_
+#define CONTROL_NODE_HPP_
+
 #include "rclcpp/rclcpp.hpp"
 
 #include "control_core.hpp"

src/robot/control/package.xml

@@ -10,6 +10,8 @@
   <!--https://www.ros.org/reps/rep-0149.html#dependency-tags-->
   <buildtool_depend>ament_cmake</buildtool_depend>
   <depend>rclcpp</depend>
+  <depend>nav_msgs</depend>
+  <depend>geometry_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

src/robot/control/src/control_node.cpp

@@ -1,11 +1,138 @@
 #include "control_node.hpp"
+#include <cmath>
 
-ControlNode::ControlNode(): Node("control"), control_(robot::ControlCore(this->get_logger())) {}
-
-int main(int argc, char ** argv)
+PurePursuitController::PurePursuitController()
+  : Node("pure_pursuit_controller")
 {
+  path_sub_ = this->create_subscription<nav_msgs::msg::Path>(
+    "/path", 10, std::bind(&PurePursuitController::pathCallback, this, std::placeholders::_1));
+
+  odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
+    "/odom/filtered", 10, std::bind(&PurePursuitController::odomCallback, this, std::placeholders::_1));
+
+  cmd_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
+
+  auto period = std::chrono::duration<double>(1.0 / control_rate_hz_);
+  control_timer_ = this->create_wall_timer(period, std::bind(&PurePursuitController::controlLoop, this));
+
+  RCLCPP_INFO(this->get_logger(), "PurePursuitController started (lookahead=%.2f m)", lookahead_distance_);
+}
+
+void PurePursuitController::pathCallback(const nav_msgs::msg::Path::SharedPtr msg) {
+  std::lock_guard<std::mutex> lock(mutex_);
+  current_path_ = msg;
+}
+
+void PurePursuitController::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) {
+  std::lock_guard<std::mutex> lock(mutex_);
+  robot_odom_ = msg;
+}
+
+void PurePursuitController::controlLoop() {
+  std::lock_guard<std::mutex> lock(mutex_);
+  if (!current_path_ || !robot_odom_) return;
+
+  // Check if the path has poses
+  if (current_path_->poses.empty()) return;
+
+  // If final goal is within tolerance, stop
+  const auto &goal_pose = current_path_->poses.back().pose.position;
+  double dxg = goal_pose.x - robot_odom_->pose.pose.position.x;
+  double dyg = goal_pose.y - robot_odom_->pose.pose.position.y;
+  double goal_dist = std::sqrt(dxg*dxg + dyg*dyg);
+  if (goal_dist < goal_tolerance_) {
+    geometry_msgs::msg::Twist stop_msg;
+    cmd_vel_pub_->publish(stop_msg);
+    return;
+  }
+
+  auto lookahead = findLookaheadPoint();
+  if (!lookahead) return;
+
+  auto cmd = computeVelocity(*lookahead);
+  cmd_vel_pub_->publish(cmd);
+}
+
+std::optional<geometry_msgs::msg::PoseStamped> PurePursuitController::findLookaheadPoint() {
+  if (!current_path_ || current_path_->poses.empty() || !robot_odom_) return std::nullopt;
+
+  double rx = robot_odom_->pose.pose.position.x;
+  double ry = robot_odom_->pose.pose.position.y;
+  double yaw = extractYaw(robot_odom_->pose.pose.orientation);
+
+  // Search along path for first point at least lookahead_distance away
+  for (const auto &pose_stamped : current_path_->poses) {
+    double px = pose_stamped.pose.position.x;
+    double py = pose_stamped.pose.position.y;
+    double dx = px - rx;
+    double dy = py - ry;
+    double dist = std::sqrt(dx*dx + dy*dy);
+    if (dist >= lookahead_distance_) {
+      return pose_stamped;
+    }
+  }
+
+  // If none found, return the last pose as the lookahead (close to goal)
+  return current_path_->poses.back();
+}
+
+geometry_msgs::msg::Twist PurePursuitController::computeVelocity(const geometry_msgs::msg::PoseStamped &target) {
+  geometry_msgs::msg::Twist cmd;
+  if (!robot_odom_) return cmd;
+
+  double rx = robot_odom_->pose.pose.position.x;
+  double ry = robot_odom_->pose.pose.position.y;
+  double yaw = extractYaw(robot_odom_->pose.pose.orientation);
+
+  double tx = target.pose.position.x;
+  double ty = target.pose.position.y;
+
+  // Transform target to robot frame (world -> robot)
+  double dx = tx - rx;
+  double dy = ty - ry;
+  double x_r =  std::cos(yaw) * dx + std::sin(yaw) * dy;
+  double y_r = -std::sin(yaw) * dx + std::cos(yaw) * dy;
+
+  // If lookahead point is behind the robot, rotate in place toward it
+  if (x_r < 0.01) {
+    cmd.linear.x = 0.0;
+    double angle_to_target = std::atan2(y_r, x_r);
+    cmd.angular.z = std::clamp(angle_to_target * 1.0, -max_angular_speed_, max_angular_speed_);
+    return cmd;
+  }
+
+  // Pure pursuit curvature
+  double L = lookahead_distance_;
+  double curvature = 2.0 * y_r / (L * L);
+  double angular = linear_speed_ * curvature;
+  angular = std::clamp(angular, -max_angular_speed_, max_angular_speed_);
+
+  cmd.linear.x = linear_speed_;
+  cmd.angular.z = angular;
+  return cmd;
+}
+
+double PurePursuitController::computeDistance(const geometry_msgs::msg::Point &a, const geometry_msgs::msg::Point &b) {
+  double dx = a.x - b.x;
+  double dy = a.y - b.y;
+  return std::sqrt(dx*dx + dy*dy);
+}
+
+double PurePursuitController::extractYaw(const geometry_msgs::msg::Quaternion &quat) {
+  // yaw (z) from quaternion
+  double x = quat.x;
+  double y = quat.y;
+  double z = quat.z;
+  double w = quat.w;
+  double siny_cosp = 2.0 * (w * z + x * y);
+  double cosy_cosp = 1.0 - 2.0 * (y * y + z * z);
+  return std::atan2(siny_cosp, cosy_cosp);
+}
+
+int main(int argc, char ** argv) {
   rclcpp::init(argc, argv);
-  rclcpp::spin(std::make_shared<ControlNode>());
+  rclcpp::spin(std::make_shared<PurePursuitController>());
   rclcpp::shutdown();
   return 0;
 }
+

src/robot/costmap/CMakeLists.txt

@@ -1,19 +1,22 @@
 cmake_minimum_required(VERSION 3.10)
 project(costmap)
-
+ 
 # 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(std_msgs REQUIRED)    # YOU ARE ONLY ADDING THIS TO THE FILE
+find_package(sensor_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+
 # Compiles source files into a library
 # A library is not executed, instead other executables can link
 # against it to access defined methods and classes.
@@ -25,21 +28,27 @@ 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
+  std_msgs # YOU ARE ONLY ADDING THIS TO THE FILE
+  sensor_msgs
+  nav_msgs
+)
+
 # Create ROS2 node executable from source files
 add_executable(costmap_node src/costmap_node.cpp)
 # Link to the previously built library to access costmap classes and methods
 target_link_libraries(costmap_node costmap_lib)
-
+ament_target_dependencies(costmap_node rclcpp sensor_msgs nav_msgs)
+
 # Copy executable to installation location
 install(TARGETS
   costmap_node
   DESTINATION lib/${PROJECT_NAME})
-
+ 
 # Copy launch and config files to installation location
 install(DIRECTORY
   config
   DESTINATION share/${PROJECT_NAME})
-
+ 
 ament_package()

src/robot/costmap/include/costmap_node.hpp

@@ -1,16 +1,54 @@
 #ifndef COSTMAP_NODE_HPP_
 #define COSTMAP_NODE_HPP_
-
+ 
 #include "rclcpp/rclcpp.hpp"
-
+#include "std_msgs/msg/string.hpp"
+
 #include "costmap_core.hpp"
-
+#include "sensor_msgs/msg/laser_scan.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
+#include <vector>
+#include <mutex>
+
 class CostmapNode : public rclcpp::Node {
   public:
     CostmapNode();
+
+    // Place callback function here
+    void publishMessage();
 
+    // New: laser scan callback will process incoming scans
+    void laserCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg);
+
   private:
     robot::CostmapCore costmap_;
-};
+    // Place these constructs here
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr string_pub_;
+    rclcpp::TimerBase::SharedPtr timer_;
+
+    // New: lidar subscription and costmap publisher
+    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr lidar_sub_;
+    rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr costmap_pub_;
 
+    // Costmap parameters
+    double resolution_ = 0.1; // meters per cell
+    unsigned int width_ = 200; // cells
+    unsigned int height_ = 200; // cells
+    double origin_x_ = 0.0; // meters (world frame origin of grid)
+    double origin_y_ = 0.0; // meters
+
+    // Inflation parameters
+    double inflation_radius_ = 1.0; // meters
+    int max_cost_ = 100;
+
+    // Grid storage in row-major (y * width + x)
+    std::vector<int8_t> grid_data_;
+    std::mutex grid_mutex_;
+
+    // Helpers
+    bool worldToGrid(double wx, double wy, int &gx, int &gy);
+    void inflateObstacles();
+    void publishCostmap(const rclcpp::Time &stamp);
+};
+
 #endif 

src/robot/costmap/package.xml

@@ -3,18 +3,23 @@
   <name>costmap</name>
   <version>0.0.0</version>
   <description>A sample ROS package for pubsub communication</description>
-
+ 
   <maintainer email="[email protected]">Owen Leather</maintainer>
   <license>Apache2.0</license>
-
+ 
   <!--https://www.ros.org/reps/rep-0149.html#dependency-tags-->
   <buildtool_depend>ament_cmake</buildtool_depend>
   <depend>rclcpp</depend>
-
+
+  <!--YOU ARE ADDING THIS MAINLY-->
+  <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>nav_msgs</depend>
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_cmake_gtest</test_depend>
-
+ 
   <!--https://www.ros.org/reps/rep-0149.html#export-->
   <export>
     <build_type>ament_cmake</build_type>