refactor: reimplement control, costmap, and map memory nodes and cores with expanded functionality and remove odometry spoofing.

Author: vedantgrover

src/robot/control/include/control/control_core.hpp

@@ -15,6 +15,8 @@
 #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
@@ -23,11 +25,30 @@ namespace robot
 class ControlCore
 {
 public:
-  // Constructor, we pass in the node's RCLCPP logger to enable logging to terminal
   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

src/robot/control/include/control/control_node.hpp

@@ -15,16 +15,65 @@
 #ifndef CONTROL_NODE_HPP_
 #define CONTROL_NODE_HPP_
 
+#include <string>
+
 #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<nav_msgs::msg::Path>::SharedPtr path_subscriber_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_subscriber_;
+  rclcpp::Publisher<geometry_msgs::msg::Twist>::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

src/robot/control/include/control_core.hpp

@@ -15,7 +15,10 @@
 #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
 {
@@ -26,8 +29,28 @@ class CostmapCore
   // 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

src/robot/control/include/control_node.hpp

@@ -15,16 +15,47 @@
 #ifndef COSTMAP_NODE_HPP_
 #define COSTMAP_NODE_HPP_
 
+#include <string>
+
 #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<sensor_msgs::msg::LaserScan>::SharedPtr laser_scan_sub_;
+  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr point_cloud_sub_;
+  rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::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