DuongTD/dock_planner
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update

Browse Source
This commit is contained in:
hoangson02
2026-03-26 12:05:20 +00:00
parent d51ecc0986
commit 03907b9613
2 changed files with 14 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/dock_calc.cpp
View File

@@ -108,11 +108,11 @@ namespace dock_planner
bool reverse) bool reverse)
{ {
constexpr double epsilon = 1e-6; constexpr double epsilon = 1e-6;
robot::log_error("[dock_calc] makePlanMoveToDock start"); robot::log_error("[dock_calc] makePlanMoveToDock start: (%f, %f), goal: (%f, %f), goal frame id: %s", start.pose.position.x, start.pose.position.y, goal.pose.position.x, goal.pose.position.y, goal.header.frame_id.c_str());
planMoveToDock.clear(); planMoveToDock.clear();
if(save_goal_.pose.position.x != goal.pose.position.x || if(save_goal_.pose.position.x != goal.pose.position.x ||
save_goal_.pose.position.y != goal.pose.position.x || save_goal_.pose.position.y != goal.pose.position.y ||
data_convert::getYaw(save_goal_.pose.orientation) != data_convert::getYaw(goal.pose.orientation)) data_convert::getYaw(save_goal_.pose.orientation) != data_convert::getYaw(goal.pose.orientation))
{ {
// robot::log_error("[dock_calc] DEBUG 1000"); // robot::log_error("[dock_calc] DEBUG 1000");

15
src/dock_planner.cpp
View File

@@ -14,7 +14,7 @@ namespace dock_planner
initialized_(false), initialized_(false),
check_free_space_(false), check_free_space_(false),
cost_threshold_(200), cost_threshold_(200),
calib_safety_distance_(0.05) {} calib_safety_distance_(0.0) {}
/** /**
* @brief Constructor với parameters * @brief Constructor với parameters
@@ -28,7 +28,7 @@ namespace dock_planner
initialized_(false), initialized_(false),
check_free_space_(false), check_free_space_(false),
cost_threshold_(200), cost_threshold_(200),
calib_safety_distance_(0.05) calib_safety_distance_(0.0)
{ {
initialize(name, costmap_robot); initialize(name, costmap_robot);
} }
@@ -90,7 +90,16 @@ namespace dock_planner
if(!initialized_) return false; if(!initialized_) return false;
if(!plan.empty()) plan.clear(); if(!plan.empty()) plan.clear();
vector<robot_geometry_msgs::Pose> planMoveToDock; vector<robot_geometry_msgs::Pose> planMoveToDock;
std::vector<robot_geometry_msgs::Point> footprint_robot = costmap_robot_->getRobotFootprint(); // std::vector<robot_geometry_msgs::Point> footprint_robot = costmap_robot_->getRobotFootprint();
std::vector<robot_geometry_msgs::Point> footprint_robot;
robot_geometry_msgs::Point p1; p1.x = 0.1; p1.y = -0.1;
robot_geometry_msgs::Point p2; p2.x = 0.1; p2.y = 0.1;
robot_geometry_msgs::Point p3; p3.x = -0.1; p3.y = 0.1;
robot_geometry_msgs::Point p4; p4.x = -0.1; p4.y = -0.1;
footprint_robot.push_back(p1);
footprint_robot.push_back(p2);
footprint_robot.push_back(p3);
footprint_robot.push_back(p4);
// footprint_dock_ = costmap_robot_->getRobotFootprint(); // footprint_dock_ = costmap_robot_->getRobotFootprint();
int degree = 2; int degree = 2;