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:
HiepLM
2025-12-30 09:11:02 +07:00
parent c5c55147b7
commit 92264eaffc
8 changed files with 141 additions and 141 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
src/dock_planner.cpp
View File

@@ -65,11 +65,11 @@ namespace dock_planner
// plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
nav_2d_msgs::Polygon2D footprint_dock ;//= nav_2d_utils::footprintFromParams(private_nh,false);
robot_nav_2d_msgs::Polygon2D footprint_dock ;//= robot_nav_2d_utils::footprintFromParams(private_nh,false);
for(auto pt : footprint_dock.points)
{
geometry_msgs::Point footprint_point;
robot_geometry_msgs::Point footprint_point;
footprint_point.x = pt.x;
footprint_point.y = pt.y;
footprint_point.z = 0.0;
@@ -81,14 +81,14 @@ namespace dock_planner
}
}
bool DockPlanner::makePlan(const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal,
std::vector<geometry_msgs::PoseStamped>& plan)
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<geometry_msgs::Pose> planMoveToDock;
std::vector<geometry_msgs::Point> footprint_robot = costmap_robot_->getRobotFootprint();
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;
@@ -99,7 +99,7 @@ namespace dock_planner
if(!status_makePlanMoveToDock) return false;
for(int i = 0; i < (int)planMoveToDock.size(); i++)
{
geometry_msgs::PoseStamped pose;
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;
@@ -112,7 +112,7 @@ namespace dock_planner
return true;
}
// void DockPlanner::publishPlan(const std::vector<geometry_msgs::PoseStamped>& plan)
// void DockPlanner::publishPlan(const std::vector<robot_geometry_msgs::PoseStamped>& plan)
// {
// nav_msgs::Path path_msg;
// path_msg.header.stamp = robot::Time::now();