first commit

src/dock_planner.cpp

@@ -0,0 +1,124 @@
+#include "dock_planner/dock_planner.h"
+#include <cmath>
+#include <algorithm>
+
+namespace dock_planner 
+{
+    /**
+     * @brief Constructor mặc định
+     * Khởi tạo tất cả các pointer về nullptr và trạng thái chưa initialized
+     */
+    DockPlanner::DockPlanner() : costmap_robot_(nullptr), 
+                                costmap_(nullptr), 
+                                initialized_(false), 
+                                check_free_space_(false),
+                                cost_threshold_(200),
+                                calib_safety_distance_(0.05) {}
+
+    /**
+     * @brief Constructor với parameters
+     * @param name Tên của planner
+     * @param costmap_robot Pointer tới costmap ROS wrapper
+     * Tự động gọi initialize() nếu được cung cấp costmap
+     */
+    DockPlanner::DockPlanner(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot) 
+        : costmap_robot_(nullptr), 
+        costmap_(nullptr), 
+        initialized_(false), 
+        check_free_space_(false),
+        cost_threshold_(200),
+        calib_safety_distance_(0.05)
+    {
+        initialize(name, costmap_robot);
+    }
+
+    /**
+     * @brief Khởi tạo global planner
+     * @param name Tên của planner instance
+     * @param costmap_robot Pointer tới costmap ROS wrapper
+     * 
+     * Thiết lập:
+     * - Liên kết với costmap
+     * - Tạo publisher cho visualization
+     * - Load parameters từ ROS parameter server
+     * - Đánh dấu trạng thái initialized
+     */
+    bool DockPlanner::initialize(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot) 
+    {
+        if (!initialized_) 
+        {
+            costmap_robot_ = costmap_robot;
+            costmap_ = costmap_robot_->getCostmap();
+            frame_id_ = costmap_robot_->getGlobalFrameID();
+            calc_plan_to_dock_.costmap_ = costmap_robot_->getCostmap();
+            
+            robot::NodeHandle private_nh("~/" + name);
+
+            // Load parameters from the private namespace
+            private_nh.getParam("check_free_space", check_free_space_);
+            private_nh.getParam("cost_threshold", cost_threshold_);
+            private_nh.getParam("calib_safety_distance", calib_safety_distance_);
+
+            calc_plan_to_dock_.cost_threshold_ = cost_threshold_;
+            calc_plan_to_dock_.calib_safety_distance_ = calib_safety_distance_;
+
+
+            // plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
+
+            nav_2d_msgs::Polygon2D footprint_dock ;//= nav_2d_utils::footprintFromParams(private_nh,false);
+            
+            for(auto pt : footprint_dock.points)
+            {
+                geometry_msgs::Point footprint_point;
+                footprint_point.x = pt.x;
+                footprint_point.y = pt.y;
+                footprint_point.z = 0.0;
+                footprint_dock_.push_back(footprint_point);
+            }
+            
+            initialized_ = true;
+            // ROS_WARN("[dock_planner] check_free_space_(%d)", check_free_space_);
+        }
+    }
+    
+    bool DockPlanner::makePlan(const geometry_msgs::PoseStamped& start,
+                                const geometry_msgs::PoseStamped& goal,
+                                std::vector<geometry_msgs::PoseStamped>& plan)
+    {
+        if(!initialized_) return false;
+        if(!plan.empty()) plan.clear();
+        vector<geometry_msgs::Pose> planMoveToDock;
+        std::vector<geometry_msgs::Point> footprint_robot = costmap_robot_->getRobotFootprint();
+
+        // footprint_dock_ = costmap_robot_->getRobotFootprint();
+        int degree = 2;
+        double step = 0.02;
+        bool status_makePlanMoveToDock = calc_plan_to_dock_.makePlanMoveToDock(start, goal, footprint_robot, footprint_dock_, degree, step, planMoveToDock, true);
+        robot::Time plan_time = robot::Time::now();
+
+        if(!status_makePlanMoveToDock) return false;
+        for(int i = 0; i < (int)planMoveToDock.size(); i++)
+        {
+          geometry_msgs::PoseStamped pose;
+          pose.header.stamp = plan_time;
+          pose.header.frame_id = costmap_robot_->getGlobalFrameID();
+          pose.pose.position.x = planMoveToDock[i].position.x;
+          pose.pose.position.y = planMoveToDock[i].position.y;
+          pose.pose.position.z = 0;
+          pose.pose.orientation = planMoveToDock[i].orientation;
+          plan.push_back(pose);
+        }
+        publishPlan(plan);
+        return true;
+    }
+
+    // void DockPlanner::publishPlan(const std::vector<geometry_msgs::PoseStamped>& plan) 
+    // {
+    //     nav_msgs::Path path_msg;
+    //     path_msg.header.stamp = robot::Time::now();
+    //     path_msg.header.frame_id = frame_id_;
+    //     path_msg.poses = plan;
+    //     plan_pub_.publish(path_msg);
+    // }
+
+} // namespace dock_planner