HiepLM/pnkx_nav_core
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'origin/3.0' into awc-devel

Browse Source
This commit is contained in:
hoangson02
2026-03-23 08:16:18 +00:00
parent 587e2deb60 c1e00fe76d
commit b4fee6c417
3 changed files with 15 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp
View File

@@ -176,7 +176,9 @@ namespace two_points_planner
const double dy = goal.pose.position.y - start.pose.position.y;
const double distance = std::sqrt(dx * dx + dy * dy);
double theta;
// Lấy độ phân giải của costmap
double resolution = costmap_robot_->getCostmap()->getResolution();
if(fabs(dx) > 1e-9 || fabs(dy) > 1e-9)
{
theta = std::atan2(dy, dx);
@@ -189,16 +191,12 @@ namespace two_points_planner
else
{
robot_geometry_msgs::PoseStamped pose = start;
pose.pose.position.x += 0.01 * cos(theta);
pose.pose.position.y += 0.01 * sin(theta);
pose.pose.position.x += resolution * cos(theta);
pose.pose.position.y += resolution * sin(theta);
plan.push_back(pose);
plan.push_back(goal);
return true;
}
// Lấy độ phân giải của costmap
double resolution = costmap_robot_->getCostmap()->getResolution();
// Tính số điểm cần chia
int num_points = std::ceil(distance / resolution);

9
src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp
View File

@@ -250,17 +250,12 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::reset()
if(rotate_algorithm_) rotate_algorithm_->reset();
ret_nav_ = ret_angle_ = false;
robot::log_info_at(__FILE__, __LINE__, "Debug");
parent_.printParams();
std::string algorithm_nav_name;
planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
parent_.setParam(algorithm_nav_name, original_papams_);
robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name);
nh_algorithm.setParam("allow_rotate", false);
robot::log_info_at(__FILE__, __LINE__, "Debug ở đây");
parent_.printParams();
}
void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity)
@@ -455,7 +450,6 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::isGoalReached(const robot_nav_
{
robot::log_info_at(__FILE__, __LINE__, "local_pose %f %f %f", local_pose.pose.x, local_pose.pose.y, local_pose.pose.theta);
robot::log_info_at(__FILE__, __LINE__, "local_goal %f %f %f", local_goal.pose.x, local_goal.pose.y, local_goal.pose.theta);
robot::log_info_at(__FILE__, __LINE__, "goal_pose_ %f %f %f", goal_pose_.pose.x, goal_pose_.pose.y, goal_pose_.pose.theta);
}
}
@@ -801,7 +795,8 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::getLocalPath(
robot_geometry_msgs::PoseStamped start = robot_nav_2d_utils::pose2DToPoseStamped(local_pose);
robot_geometry_msgs::PoseStamped goal = robot_nav_2d_utils::pose2DToPoseStamped(local_goal);
std::vector<robot_geometry_msgs::PoseStamped> docking_plan;
robot::log_info_at(__FILE__, __LINE__, "start %s %f %f", start.header.frame_id.c_str(), start.pose.position.x, start.pose.position.y);
robot::log_info_at(__FILE__, __LINE__, "goal %s %f %f", goal.header.frame_id.c_str(), goal.pose.position.x, goal.pose.position.y);
if (!docking_planner_->makePlan(start, goal, docking_plan))
{
throw robot_nav_core2::LocalPlannerException("Making plan from goal maker is failed");

26
src/Navigations/Packages/move_base/src/move_base.cpp
View File

@@ -3144,15 +3144,6 @@ robot_geometry_msgs::PoseStamped move_base::MoveBase::goalToGlobalFrame(const ro
}
std::string global_frame = planner_costmap_robot_->getGlobalFrameID();
// robot_geometry_msgs::PoseStamped goal_pose, global_pose;
// goal_pose = goal_pose_msg;
// goal_pose.header.stamp = robot::Time(); // latest available
// try
// {
// tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, tf3::Time());
// tf3::doTransform(goal_pose, global_pose, transform);
// }
robot_geometry_msgs::PoseStamped global_pose;
tf3::toMsg(tf3::Transform::getIdentity(), global_pose.pose);
robot_geometry_msgs::PoseStamped goal_pose;
@@ -3218,17 +3209,16 @@ robot::move_base_core::PlannerDataOutput move_base::MoveBase::getGlobalData()
{
robot_nav_msgs::Path path;
tc_->getGlobalPlan(path.poses);
if (!path.poses.empty())
robot_nav_msgs::Path global_path;
global_path.header.stamp = robot::Time::now();
global_path.header.frame_id = planner_costmap_robot_->getGlobalFrameID();
for(auto &p : path.poses)
{
path.header.stamp = path.poses[0].header.stamp;
path.header.frame_id = path.poses[0].header.frame_id;
robot_geometry_msgs::PoseStamped pose = goalToGlobalFrame(p);
pose.header.stamp = robot::Time::now();
global_path.poses.push_back(goalToGlobalFrame(pose));
}
else
{
path.header.stamp = robot::Time::now();
path.header.frame_id = planner_costmap_robot_->getGlobalFrameID();
}
global_data_.plan = robot_nav_2d_utils::pathToPath(path);
global_data_.plan = robot_nav_2d_utils::pathToPath(global_path);
}
ConvertData convert_data(planner_costmap_robot_, planner_costmap_robot_->getGlobalFrameID(), true);
convert_data.updateCostmap(global_data_.costmap, global_data_.costmap_update, global_data_.is_costmap_updated);