135 lines
5.1 KiB
C++
135 lines
5.1 KiB
C++
#include "dock_planner/dock_planner.h"
|
|
#include <cmath>
|
|
#include <algorithm>
|
|
#include <boost/dll/alias.hpp>
|
|
|
|
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, robot_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, robot_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<robot_nav_msgs::Path>("plan", 1);
|
|
|
|
robot_nav_2d_msgs::Polygon2D footprint_dock ;//= robot_nav_2d_utils::footprintFromParams(private_nh,false);
|
|
|
|
for(auto pt : footprint_dock.points)
|
|
{
|
|
robot_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 robot_geometry_msgs::PoseStamped& start,
|
|
const robot_geometry_msgs::PoseStamped& goal,
|
|
std::vector<robot_geometry_msgs::PoseStamped>& plan)
|
|
{
|
|
if(!initialized_) return false;
|
|
if(!plan.empty()) plan.clear();
|
|
vector<robot_geometry_msgs::Pose> planMoveToDock;
|
|
std::vector<robot_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++)
|
|
{
|
|
robot_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<robot_geometry_msgs::PoseStamped>& plan)
|
|
// {
|
|
// robot_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);
|
|
// }
|
|
|
|
// Export factory function
|
|
static boost::shared_ptr<robot_nav_core::BaseGlobalPlanner> dock_planner_plugin() {
|
|
return boost::make_shared<dock_planner::DockPlanner>();
|
|
}
|
|
|
|
|
|
} // namespace dock_planner
|
|
|
|
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
|
|
BOOST_DLL_ALIAS(dock_planner::dock_planner_plugin, DockPlanner)
|