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

fix bug

Browse Source
This commit is contained in:
HiepLM
2026-03-21 19:04:32 +07:00
parent d38f6b3954
commit c05a3e4439
12 changed files with 182 additions and 76 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp
View File

@@ -789,16 +789,16 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
// (is_stopped || sign(angle_to_path) * sign_x < 0 ) && fabs(angle_to_path) > heading_rotate;
bool result = use_rotate_to_heading_ && fabs(angle_to_path) > heading_rotate;
#ifdef BUILD_WITH_ROS
if (result)
ROS_WARN_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
// #ifdef BUILD_WITH_ROS
// if (result)
// ROS_WARN_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
// else if(fabs(velocity.x) < min_speed_xy_)
// {
// ROS_INFO_THROTTLE(0.1, "velocity.x: %f, velocity.theta: %f, ", velocity.x, velocity.theta);
// ROS_INFO_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
// }
#endif
// #endif
return result;
}

2
src/Algorithms/Packages/global_planners/dock_planner

Submodule src/Algorithms/Packages/global_planners/dock_planner updated: 56d05e4322...d51ecc0986

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

@@ -36,8 +36,6 @@ namespace two_points_planner
if (!initialized_)
{
robot::NodeHandle nh_priv_("~/" + name);
robot::log_info("TwoPointsPlanner: Name is %s", name.c_str());
int lethal_obstacle;
nh_priv_.getParam("lethal_obstacle", lethal_obstacle, 20);
lethal_obstacle_ = (unsigned char)lethal_obstacle;
@@ -121,6 +119,7 @@ namespace two_points_planner
robot::log_error("[%s:%d]\n TwoPointsPlanner: Global planner is not initialized", __FILE__, __LINE__);
return false;
}
robot_nav_2d_msgs::Pose2DStamped start_2d = robot_nav_2d_utils::poseStampedToPose2D(start);
robot_nav_2d_msgs::Pose2DStamped goal_2d = robot_nav_2d_utils::poseStampedToPose2D(goal);
robot::log_info_at(__FILE__, __LINE__, "Making plan from start (%.2f,%.2f,%.2f) to goal (%.2f,%.2f,%.2f)",
@@ -148,21 +147,21 @@ namespace two_points_planner
}
plan.clear();
plan.push_back(start);
// plan.push_back(start);
unsigned int mx_start, my_start;
unsigned int mx_end, my_end;
if (!costmap_robot_->getCostmap()->worldToMap(start.pose.position.x,
start.pose.position.y,
mx_start, my_start)
// unsigned int mx_start, my_start;
// unsigned int mx_end, my_end;
// if (!costmap_robot_->getCostmap()->worldToMap(start.pose.position.x,
// start.pose.position.y,
// mx_start, my_start)
|| !costmap_robot_->getCostmap()->worldToMap(goal.pose.position.x,
goal.pose.position.y,
mx_end, my_end))
{
robot::log_error("[%s:%d]\n TwoPointsPlanner: can not convert world to Map 'start point' or 'goal point'", __FILE__, __LINE__);
return false;
}
// || !costmap_robot_->getCostmap()->worldToMap(goal.pose.position.x,
// goal.pose.position.y,
// mx_end, my_end))
// {
// robot::log_error("[%s:%d]\n TwoPointsPlanner: can not convert world to Map 'start point' or 'goal point'", __FILE__, __LINE__);
// return false;
// }
// unsigned char start_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_start, my_start));
// unsigned char end_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_end, my_end));
// if (start_cost == costMapCostToSBPLCost(robot_costmap_2d::LETHAL_OBSTACLE) || start_cost == costMapCostToSBPLCost(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)

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

@@ -250,11 +250,16 @@ 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);
parent_.setParam(algorithm_nav_name, original_papams_);
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)
@@ -567,17 +572,50 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_
}
pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::DockingPlanner(const std::string &name)
: initialized_(false), is_goal_reached_(false), delayed_(false), detected_timeout_(false),
is_detected_(false), name_(name), docking_planner_name_(""), docking_nav_name_("")
: initialized_(false),
is_detected_(false),
is_goal_reached_(false),
following_(false),
allow_rotate_(false),
xy_goal_tolerance_(0.05),
yaw_goal_tolerance_(0.05),
min_lookahead_dist_(0.4),
max_lookahead_dist_(1.0),
lookahead_time_(1.5),
angle_threshold_(0.4),
name_(name),
docking_planner_name_(),
docking_nav_name_(),
docking_planner_(nullptr),
docking_nav_(nullptr),
linear_(),
angular_(),
nh_(),
nh_priv_(),
tf_(nullptr),
costmap_robot_(nullptr),
traj_generator_(),
docking_planner_loader_(),
docking_nav_loader_(),
detected_timeout_wt_(),
delayed_wt_(),
delayed_(false),
detected_timeout_(false),
robot_base_frame_(),
maker_goal_frame_()
{
}
pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::~DockingPlanner()
{
if (docking_planner_)
detected_timeout_wt_.stop();
delayed_wt_.stop();
if (docking_planner_ != nullptr) {
docking_planner_.reset();
if (docking_nav_)
}
if (docking_nav_ != nullptr) {
docking_nav_.reset();
}
}
void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(robot::NodeHandle &nh, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot,

1
src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp
View File

@@ -107,6 +107,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
{
robot::PluginLoaderHelper loader;
std::string path_file_so = loader.findLibraryPath(algorithm_nav_name);
robot::log_info_at(__FILE__, __LINE__, "path_file_so: %s", path_file_so.c_str());
nav_algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
path_file_so, algorithm_nav_name, boost::dll::load_mode::append_decorations);
nav_algorithm_ = nav_algorithm_loader_();

2
src/Libraries/laser_geometry

Submodule src/Libraries/laser_geometry updated: 50062ef549...7e70a03bc0

3
src/Libraries/robot_cpp/src/node_handle.cpp
View File

@@ -651,7 +651,8 @@ namespace robot
std::string key = it->first.as<std::string>();
const YAML::Node &value = it->second;
std::string full_key = prefix.empty() ? key : prefix + "/" + key;
if(full_key.find("MKTAlgorithmDiffPredictiveTrajectory") == std::string::npos)
continue;
if (value.IsMap())
{
std::cout << "[NodeHandle] " << indent << full_key << ":" << std::endl;

1
src/Libraries/robot_cpp/src/plugin_loader_helper.cpp
View File

@@ -66,6 +66,7 @@ std::string PluginLoaderHelper::findLibraryPath(const std::string& symbol_name)
}
// Try to read from NodeHandle
std::string library_path;
robot::log_info_at(__FILE__, __LINE__, "%s", symbol_name.c_str());
if (nh_.hasParam(param_path)) {
nh_.getParam(param_path, library_path, std::string(""));
if (!library_path.empty()) {

12
src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_local_planner.h
View File

@@ -41,6 +41,7 @@
#include <robot_geometry_msgs/Twist.h>
#include <robot_costmap_2d/costmap_2d_robot.h>
#include <tf3/buffer_core.h>
#include <robot_nav_msgs/Odometry.h>
#include <memory>
namespace robot_nav_core
@@ -148,6 +149,12 @@ namespace robot_nav_core
*/
virtual void initialize(std::string name, tf3::BufferCore *tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) = 0;
/**
* @brief Set the odometry of the robot
* @param odom The odometry of the robot
*/
virtual void setOdom(robot_nav_msgs::Odometry *odom) { odom_ = odom; }
/**
* @brief Virtual destructor for the interface
*/
@@ -155,6 +162,11 @@ namespace robot_nav_core
protected:
BaseLocalPlanner() {}
/**
* @brief The odometry of the robot
*/
robot_nav_msgs::Odometry *odom_;
};
} // namespace robot_nav_core

5
src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/local_planner_adapter.h
View File

@@ -189,11 +189,6 @@ namespace robot_nav_core_adapter
robot_nav_2d_msgs::Pose2DStamped last_goal_;
bool has_active_goal_;
/**
* @brief The odometry of the robot
*/
robot_nav_msgs::Odometry odom_;
// Plugin handling
boost::function<robot_nav_core2::LocalPlanner::Ptr()> planner_loader_;
robot_nav_core2::LocalPlanner::Ptr planner_;

6
src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp
View File

@@ -88,7 +88,7 @@ namespace robot_nav_core_adapter
private_nh_ = robot::NodeHandle("~");
adapter_nh_ = robot::NodeHandle(private_nh_, name);
std::string planner_name;
if (adapter_nh_.hasParam("planner_name"))
{
@@ -291,7 +291,7 @@ namespace robot_nav_core_adapter
robot_geometry_msgs::Twist LocalPlannerAdapter::getActualTwist()
{
return odom_.twist.twist;
return odom_->twist.twist;
}
bool LocalPlannerAdapter::islock()
@@ -356,7 +356,7 @@ namespace robot_nav_core_adapter
if (!getRobotPose(pose2d))
return false;
robot_nav_2d_msgs::Twist2D velocity = robot_nav_2d_utils::twist3Dto2D(odom_.twist.twist);
robot_nav_2d_msgs::Twist2D velocity = robot_nav_2d_utils::twist3Dto2D(odom_->twist.twist);
bool ret = planner_->isGoalReached(pose2d, velocity);
if (ret)
{

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

@@ -324,6 +324,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
throw std::runtime_error("Failed to load local planner " + local_planner);
}
tc_->initialize(local_planner, tf_.get(), controller_costmap_robot_);
tc_->setOdom(&odometry_);
}
catch (const std::exception &ex)
{
@@ -1781,32 +1782,6 @@ void move_base::MoveBase::cancel()
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
cancel_ctr_ = true;
robot::log_info("[MoveBase::cancel] cancel_ctr_ set to true");
if (as_)
{
// Get current goal ID from action server to cancel specific goal
// If we want to cancel all goals, use empty string ""
robot_actionlib_msgs::GoalIDPtr msg = boost::make_shared<robot_actionlib_msgs::GoalID>();
// Use empty string to cancel current goal (processCancel accepts empty string to cancel all)
msg->id = ""; // Empty string cancels current goal
msg->stamp = robot::Time::now();
robot::log_info("[MoveBase::cancel] Sending cancel request to action server");
robot::log_info("[MoveBase::cancel] Cancel message: id='%s', stamp=%ld.%09ld",
msg->id.c_str(), msg->stamp.sec, msg->stamp.nsec);
// Convert to ConstPtr for processCancel
robot_actionlib_msgs::GoalIDConstPtr cancel_msg = msg;
as_->processCancel(cancel_msg);
robot::log_info("[MoveBase::cancel] Cancel request processed by action server");
}
else
{
robot::log_warning("[MoveBase::cancel] Action server (as_) is null - cannot send cancel request");
}
robot::log_info("[MoveBase::cancel] ===== EXIT =====");
}
bool move_base::MoveBase::getRobotPose(robot_geometry_msgs::PoseStamped &pose)
@@ -2381,8 +2356,9 @@ void move_base::MoveBase::executeCb(const robot_move_base_msgs::MoveBaseGoalCons
robot::log_info("Received goal: x=%.2f, y=%.2f", move_base_goal->target_pose.pose.position.x, move_base_goal->target_pose.pose.position.y);
}
}
robot::log_warning("Received goal: x=%.2f, y=%.2f", move_base_goal->target_pose.pose.position.x, move_base_goal->target_pose.pose.position.y);
robot_geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose);
robot::log_warning("Received goalToGlobalFrame: x=%.2f, y=%.2f", goal.pose.position.x, goal.pose.position.y);
publishZeroVelocity();
// we have a goal so start the planner
boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
@@ -2602,7 +2578,6 @@ bool move_base::MoveBase::isQuaternionValid(const robot_geometry_msgs::Quaternio
robot::log_error("Quaternion has nans or infs... discarding as a navigation goal");
return false;
}
tf3::Quaternion tf_q(q.x, q.y, q.z, q.w);
// next, we need to check if the length of the quaternion is close to zero
@@ -2764,6 +2739,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
recovery_index_ = 0;
}
// Cancle executeCycle
if (cancel_ctr_ && tc_)
{
@@ -2772,13 +2748,25 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
tc_->setTwistLinear(linear);
try
{
if (fabs(tc_->getActualTwist().linear.x) <= min_approach_linear_velocity_)
double actual_linear_velocity = sqrt(std::pow(tc_->getActualTwist().linear.x, 2) + std::pow(tc_->getActualTwist().linear.y, 2));
if (actual_linear_velocity <= min_approach_linear_velocity_)
{
robot::log_info("MoveTo is canceled.");
resetState();
cancel_ctr_ = false;
// if(default_config_.base_global_planner != last_config_.base_global_planner)
swapPlanner(default_config_.base_global_planner);
robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, position_planner_name_);
if(nh_planner.hasParam("base_global_planner"))
{
std::string base_global_planner;
nh_planner.getParam("base_global_planner", base_global_planner);
swapPlanner(base_global_planner);
}
else
{
swapPlanner(default_config_.base_global_planner);
}
cancel_ctr_ = false;
// disable the planner thread
boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
runPlanner_ = false;
@@ -2789,7 +2777,21 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
nav_feedback_->navigation_state = robot::move_base_core::State::PREEMPTED;
}
// as_->setPreempted(robot_move_base_msgs::MoveBaseResult(), "Canled.");
cancel_ctr_ = false;
if (as_)
{
robot_actionlib_msgs::GoalIDPtr msg = boost::make_shared<robot_actionlib_msgs::GoalID>();
msg->id = ""; // Empty string cancels current goal
msg->stamp = robot::Time::now();
// Convert to ConstPtr for processCancel
robot_actionlib_msgs::GoalIDConstPtr cancel_msg = msg;
as_->processCancel(cancel_msg);
robot::log_info("[MoveBase::cancel] Cancel request processed by action server");
}
else
{
robot::log_warning("[MoveBase::cancel] Action server (as_) is null - cannot send cancel request");
}
return true;
}
}
@@ -2798,7 +2800,17 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
robot::log_info("MoveTo is canceled.");
resetState();
// if(default_config_.base_global_planner != last_config_.base_global_planner)
swapPlanner(default_config_.base_global_planner);
robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, position_planner_name_);
if(nh_planner.hasParam("base_global_planner"))
{
std::string base_global_planner;
nh_planner.getParam("base_global_planner", base_global_planner);
swapPlanner(base_global_planner);
}
else
{
swapPlanner(default_config_.base_global_planner);
}
// disable the planner thread
boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
@@ -2810,6 +2822,20 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
nav_feedback_->navigation_state = robot::move_base_core::State::PREEMPTED;
}
// as_->setPreempted(robot_move_base_msgs::MoveBaseResult(), "Canled.");
if (as_)
{
robot_actionlib_msgs::GoalIDPtr msg = boost::make_shared<robot_actionlib_msgs::GoalID>();
msg->id = ""; // Empty string cancels current goal
msg->stamp = robot::Time::now();
// Convert to ConstPtr for processCancel
robot_actionlib_msgs::GoalIDConstPtr cancel_msg = msg;
as_->processCancel(cancel_msg);
robot::log_info("[MoveBase::cancel] Cancel request processed by action server");
}
else
{
robot::log_warning("[MoveBase::cancel] Action server (as_) is null - cannot send cancel request");
}
cancel_ctr_ = false;
return true;
}
@@ -2830,7 +2856,6 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
// if we're controlling, we'll attempt to find valid velocity commands
case move_base::CONTROLLING:
// robot::log_debug("In controlling state.");
// check to see if we've reached our goal
try
@@ -3004,7 +3029,8 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
tc_->setTwistLinear(linear);
try
{
if (fabs(tc_->getActualTwist().linear.x) <= min_approach_linear_velocity_)
double actual_linear_velocity = sqrt(std::pow(tc_->getActualTwist().linear.x, 2) + std::pow(tc_->getActualTwist().linear.y, 2));
if (actual_linear_velocity <= min_approach_linear_velocity_)
{
paused_ = true;
}
@@ -3108,15 +3134,48 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
robot_geometry_msgs::PoseStamped move_base::MoveBase::goalToGlobalFrame(const robot_geometry_msgs::PoseStamped &goal_pose_msg)
{
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
if (!tf_)
{
return goal_pose_msg;
}
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;
tf3::toMsg(tf3::Transform::getIdentity(), goal_pose.pose);
goal_pose = goal_pose_msg;
robot::Time current_time = robot::Time::now(); // save time for checking tf delay later
tf3::Time tf3_current_time = data_convert::convertTime(current_time);
tf3::Time tf3_zero_time = data_convert::convertTime(robot::Time());
try
{
tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, tf3::Time());
tf3::doTransform(goal_pose, global_pose, transform);
// use current time if possible (makes sure it's not in the future)
std::string error_msg;
if (tf_->canTransform(global_frame, goal_pose.header.frame_id, tf3_current_time, &error_msg))
{
// Transform is available at current time
tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, tf3_current_time);
tf3::doTransform(goal_pose, global_pose, transform);
}
// use the latest otherwise (tf3::Time() means latest available)
else
{
// Try to get latest transform
tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, tf3_zero_time);
tf3::doTransform(goal_pose, global_pose, transform);
}
}
catch (tf3::LookupException &ex)
{