diff --git a/src/robot/control/include/control/control_core.hpp b/src/robot/control/include/control/control_core.hpp index 2c1b9c2..34b9fab 100644 --- a/src/robot/control/include/control/control_core.hpp +++ b/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 diff --git a/src/robot/control/include/control/control_node.hpp b/src/robot/control/include/control/control_node.hpp index 8fc6eeb..afd7a5e 100644 --- a/src/robot/control/include/control/control_node.hpp +++ b/src/robot/control/include/control/control_node.hpp @@ -15,7 +15,12 @@ #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 @@ -23,8 +28,52 @@ 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/include/control_core.hpp b/src/robot/control/include/control_core.hpp deleted file mode 100644 index 34b9fab..0000000 --- a/src/robot/control/include/control_core.hpp +++ /dev/null @@ -1,56 +0,0 @@ -// 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 deleted file mode 100644 index afd7a5e..0000000 --- a/src/robot/control/include/control_node.hpp +++ /dev/null @@ -1,79 +0,0 @@ -// 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/costmap/include/costmap/costmap_core.hpp b/src/robot/costmap/include/costmap/costmap_core.hpp index e583b2b..1b57b22 100644 --- a/src/robot/costmap/include/costmap/costmap_core.hpp +++ b/src/robot/costmap/include/costmap/costmap_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 diff --git a/src/robot/costmap/include/costmap/costmap_node.hpp b/src/robot/costmap/include/costmap/costmap_node.hpp index 2080a1c..1ce73f9 100644 --- a/src/robot/costmap/include/costmap/costmap_node.hpp +++ b/src/robot/costmap/include/costmap/costmap_node.hpp @@ -15,16 +15,47 @@ #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/include/costmap_core.hpp b/src/robot/costmap/include/costmap_core.hpp deleted file mode 100644 index 1b57b22..0000000 --- a/src/robot/costmap/include/costmap_core.hpp +++ /dev/null @@ -1,58 +0,0 @@ -// 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 deleted file mode 100644 index 1ce73f9..0000000 --- a/src/robot/costmap/include/costmap_node.hpp +++ /dev/null @@ -1,61 +0,0 @@ -// 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/gps_sim/include/gps_sim_node.hpp b/src/robot/gps_sim/include/gps_sim/gps_sim_node.hpp similarity index 100% rename from src/robot/gps_sim/include/gps_sim_node.hpp rename to src/robot/gps_sim/include/gps_sim/gps_sim_node.hpp diff --git a/src/robot/imu_sim/include/imu_sim_node.hpp b/src/robot/imu_sim/include/imu_sim/imu_sim_node.hpp similarity index 100% rename from src/robot/imu_sim/include/imu_sim_node.hpp rename to src/robot/imu_sim/include/imu_sim/imu_sim_node.hpp diff --git a/src/robot/localization/include/localization_core.hpp b/src/robot/localization/include/localization/localization_core.hpp similarity index 97% rename from src/robot/localization/include/localization_core.hpp rename to src/robot/localization/include/localization/localization_core.hpp index e527e47..796076a 100644 --- a/src/robot/localization/include/localization_core.hpp +++ b/src/robot/localization/include/localization/localization_core.hpp @@ -48,9 +48,12 @@ class Matrix6x6 Matrix6x6 operator+(const Matrix6x6 & other) const; Matrix6x6 operator-(const Matrix6x6 & other) const; Matrix6x6 operator*(const Matrix6x6 & other) const; + Matrix6x6 operator*(double scalar) const; Matrix6x6 transpose() const; Matrix6x6 inverse() const; + Matrix6x6 & operator=(const Matrix6x6 & other); + static Matrix6x6 identity(); private: diff --git a/src/robot/localization/include/localization_node.hpp b/src/robot/localization/include/localization/localization_node.hpp similarity index 100% rename from src/robot/localization/include/localization_node.hpp rename to src/robot/localization/include/localization/localization_node.hpp diff --git a/src/robot/localization/src/localization_core.cpp b/src/robot/localization/src/localization_core.cpp index 2188c9e..3646c5d 100644 --- a/src/robot/localization/src/localization_core.cpp +++ b/src/robot/localization/src/localization_core.cpp @@ -79,6 +79,23 @@ Matrix6x6 Matrix6x6::operator*(const Matrix6x6 & other) const return result; } +Matrix6x6 Matrix6x6::operator*(double scalar) const +{ + Matrix6x6 result; + for (size_t i = 0; i < 36; i++) { + result.data_[i] = data_[i] * scalar; + } + return result; +} + +Matrix6x6 & Matrix6x6::operator=(const Matrix6x6 & other) +{ + if (this != &other) { + data_ = other.data_; + } + return *this; +} + Matrix6x6 Matrix6x6::transpose() const { Matrix6x6 result; @@ -93,8 +110,6 @@ Matrix6x6 Matrix6x6::transpose() const 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; @@ -226,8 +241,8 @@ double Vector6::norm() const // ============================================================================ ExtendedKalmanFilter::ExtendedKalmanFilter(const rclcpp::Logger & logger) -: logger_(logger) -, initialized_(false) +: initialized_(false) +, logger_(logger) { state_ = Vector6(); P_ = Matrix6x6::identity() * 0.1; // Initial uncertainty @@ -518,6 +533,7 @@ void ExtendedKalmanFilter::getOdometry( Matrix6x6 ExtendedKalmanFilter::computeProcessJacobian(double vx, double vy, double omega, double dt) { + (void)omega; Matrix6x6 F = Matrix6x6::identity(); double theta = state_.theta(); double cos_theta = std::cos(theta); diff --git a/src/robot/map_memory/include/map_memory/map_memory_core.hpp b/src/robot/map_memory/include/map_memory/map_memory_core.hpp index 2186aee..dbecac6 100644 --- a/src/robot/map_memory/include/map_memory/map_memory_core.hpp +++ b/src/robot/map_memory/include/map_memory/map_memory_core.hpp @@ -15,6 +15,7 @@ #ifndef MAP_MEMORY_CORE_HPP_ #define MAP_MEMORY_CORE_HPP_ +#include "nav_msgs/msg/occupancy_grid.hpp" #include "rclcpp/rclcpp.hpp" namespace robot @@ -25,7 +26,18 @@ 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_; }; diff --git a/src/robot/map_memory/include/map_memory/map_memory_node.hpp b/src/robot/map_memory/include/map_memory/map_memory_node.hpp index 6724402..991f2a5 100644 --- a/src/robot/map_memory/include/map_memory/map_memory_node.hpp +++ b/src/robot/map_memory/include/map_memory/map_memory_node.hpp @@ -15,7 +15,16 @@ #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 @@ -23,8 +32,47 @@ 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/include/map_memory_core.hpp b/src/robot/map_memory/include/map_memory_core.hpp deleted file mode 100644 index dbecac6..0000000 --- a/src/robot/map_memory/include/map_memory_core.hpp +++ /dev/null @@ -1,46 +0,0 @@ -// 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 deleted file mode 100644 index 991f2a5..0000000 --- a/src/robot/map_memory/include/map_memory_node.hpp +++ /dev/null @@ -1,78 +0,0 @@ -// 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/odometry_spoof/include/odometry_spoof.hpp b/src/robot/odometry_spoof/include/odometry_spoof.hpp deleted file mode 100644 index 13dc62e..0000000 --- a/src/robot/odometry_spoof/include/odometry_spoof.hpp +++ /dev/null @@ -1,56 +0,0 @@ -// 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/planner/include/planner_core.hpp b/src/robot/planner/include/planner/planner_core.hpp similarity index 100% rename from src/robot/planner/include/planner_core.hpp rename to src/robot/planner/include/planner/planner_core.hpp diff --git a/src/robot/planner/include/planner_node.hpp b/src/robot/planner/include/planner/planner_node.hpp similarity index 100% rename from src/robot/planner/include/planner_node.hpp rename to src/robot/planner/include/planner/planner_node.hpp diff --git a/watod-config.sh b/watod-config.sh index e1276d2..7a758f1 100755 --- a/watod-config.sh +++ b/watod-config.sh @@ -52,4 +52,4 @@ export ACTIVE_MODULES="robot gazebo vis_tools" ## Platform in which to build the docker images with. ## Either arm64 (apple silicon, raspberry pi) or amd64 (most computers) -export PLATFORM="amd64" +export PLATFORM="arm64"