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

update 5/2

Browse Source
This commit is contained in:
HiepLM
2026-02-05 14:13:40 +07:00
parent cab5655769
commit a43b714f01
13 changed files with 409 additions and 156 deletions
Download Patch File Download Diff File Expand all files Collapse all files

93
src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp
View File

@@ -108,16 +108,101 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
{
twist = traj_->nextTwist();
}
drive_cmd.x = sqrt(twist.x * twist.x);
robot_nav_2d_msgs::Path2D transformed_plan = transform_plan_;
double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
drive_cmd.x = std::min(sqrt(twist.x * twist.x), fabs(v_max));
robot_nav_2d_msgs::Path2D transformed_plan = this->transform_plan_;
if (transformed_plan.poses.empty())
{
robot::log_warning("Transformed plan is empty. Cannot determine a local plan.");
robot::log_warning("[%s:%d]\n Transformed plan is empty. Cannot determine a localglobal_plan.", __FILE__, __LINE__);
stopWithAccLimits(pose, velocity, drive_cmd, dt);
result.velocity = drive_cmd;
prevous_drive_cmd_ = drive_cmd;
return result;
}
double lookahead_dist = getLookAheadDistance(velocity);
auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
robot_geometry_msgs::PoseStamped carrot_pose_stamped = robot_nav_2d_utils::pose2DToPoseStamped(carrot_pose);
// === Final Heading Alignment Check ===
double xy_error = 0.0, heading_error = 0.0;
if (shouldAlignToFinalHeading(transformed_plan, carrot_pose, velocity, xy_error, heading_error, sign_x))
{
// Use Arc Motion controller for final heading alignment
alignToFinalHeading(xy_error, heading_error, velocity, sign_x, dt, drive_cmd);
#ifdef BUILD_WITH_ROS
ROS_INFO("xy_err=%.3f, heading_err=%.3f deg, v=%.3f, w_current=%.3f, w_target=%.3f",
xy_error, heading_error * 180.0 / M_PI, drive_cmd.x, velocity.theta, drive_cmd.theta);
#endif
}
else
{
if(fabs(carrot_pose.pose.y) > 0.2)
{
lookahead_dist = sqrt(carrot_pose.pose.y *carrot_pose.pose.y + lookahead_dist * lookahead_dist);
}
robot_nav_2d_msgs::Twist2D drive_target;
transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target);
carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
// Normal Pure Pursuit
this->computePurePursuit(
carrot_pose,
drive_target,
velocity,
transformed_plan,
min_approach_linear_velocity_,
sign_x,
dt,
drive_cmd);
}
applyDistanceSpeedScaling(compute_plan_, velocity, drive_cmd, sign_x, dt);
if (this->nav_stop_)
{
if (!stopped(velocity, rot_stopped_velocity_, trans_stopped_velocity_))
{
if (!stopWithAccLimits(pose, velocity, drive_cmd, dt))
return result;
}
else
drive_cmd = {};
result.velocity = drive_cmd;
return result;
}
Eigen::VectorXd y(2);
y << drive_cmd.x, drive_cmd.theta;
// Cập nhật lại A nếu dt thay đổi
for (int i = 0; i < n_; ++i)
for (int j = 0; j < n_; ++j)
A(i, j) = (i == j ? 1.0 : 0.0);
for (int i = 0; i < n_; i += 3)
{
A(i, i + 1) = dt;
A(i, i + 2) = 0.5 * dt * dt;
A(i + 1, i + 2) = dt;
}
kf_->update(y, dt, A);
double v_min = min_approach_linear_velocity_;
drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_max), fabs(v_max));
drive_cmd.x = fabs(drive_cmd.x) >= v_min ? drive_cmd.x : std::copysign(v_min, sign_x);
drive_cmd.theta = std::clamp(kf_->state()[3], -max_vel_theta_, max_vel_theta_);
result.poses.clear();
result.poses.reserve(transformed_plan.poses.size());
for (const auto &pose_stamped : transformed_plan.poses)
{
result.poses.push_back(pose_stamped.pose);
if(fabs(pose_stamped.pose.x - carrot_pose.pose.x) < 1e-6 &&
fabs(pose_stamped.pose.y - carrot_pose.pose.y) < 1e-6 &&
fabs(pose_stamped.pose.theta - carrot_pose.pose.theta) < 1e-6)
break;
}
result.velocity = drive_cmd;
prevous_drive_cmd_ = drive_cmd;
return result;
}

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

@@ -627,7 +627,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
drive_cmd.x = velocity.x + dv;
drive_cmd.theta = velocity.theta + dw;
robot::log_info("v_target: %f, w_target: %f, dv: %f, dw: %f, drive_cmd.x: %f, drive_cmd.theta: %f", v_target, w_target, dv, dw, drive_cmd.x, drive_cmd.theta);
// robot::log_info("v_target: %f, w_target: %f, dv: %f, dw: %f, drive_cmd.x: %f, drive_cmd.theta: %f", v_target, w_target, dv, dw, drive_cmd.x, drive_cmd.theta);
}
void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling(

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

@@ -49,6 +49,8 @@ namespace two_points_planner
initialized_ = true;
return true;
}
robot::log_info("[%s:%d]\n TwoPointsPlanner: Already initialized", __FILE__, __LINE__);
return true;
}
unsigned char TwoPointsPlanner::computeCircumscribedCost()
@@ -104,7 +106,7 @@ namespace two_points_planner
{
if (!initialized_)
{
std::cerr << "TwoPointsPlanner: Global planner is not initialized" << std::endl;
robot::log_error("[%s:%d]\n TwoPointsPlanner: Global planner is not initialized", __FILE__, __LINE__);
return false;
}
@@ -112,14 +114,14 @@ namespace two_points_planner
if (current_env_width_ != costmap_robot_->getCostmap()->getSizeInCellsX() ||
current_env_height_ != costmap_robot_->getCostmap()->getSizeInCellsY())
{
printf("TwoPointsPlanner: Costmap dimensions have changed from (%u x %u) to (%u x %u), reinitializing two_points_planner.\n",
robot::log_warning("[%s:%d]\n TwoPointsPlanner: Costmap dimensions have changed from (%u x %u) to (%u x %u), reinitializing two_points_planner.", __FILE__, __LINE__,
current_env_width_, current_env_height_,
costmap_robot_->getCostmap()->getSizeInCellsX(), costmap_robot_->getCostmap()->getSizeInCellsY());
do_init = true;
}
else if (footprint_ != costmap_robot_->getRobotFootprint())
{
printf("TwoPointsPlanner: Robot footprint has changed, reinitializing two_points_planner.\n");
robot::log_warning("[%s:%d]\n TwoPointsPlanner: Robot footprint has changed, reinitializing two_points_planner.", __FILE__, __LINE__);
do_init = true;
}
@@ -131,7 +133,7 @@ namespace two_points_planner
plan.clear();
plan.push_back(start);
printf("TwoPointsPlanner getting start point (%.2f,%.2f) goal point (%.2f,%.2f)\n",
robot::log_info("[%s:%d]\n TwoPointsPlanner getting start point (%.2f,%.2f) goal point (%.2f,%.2f)", __FILE__, __LINE__,
start.pose.position.x, start.pose.position.y, goal.pose.position.x, goal.pose.position.y);
unsigned int mx_start, my_start;
@@ -144,7 +146,7 @@ namespace two_points_planner
goal.pose.position.y,
mx_end, my_end))
{
std::cerr << "TwoPointsPlanner: can not convert world to Map 'start point' or 'goal point'" << std::endl;
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));
@@ -152,7 +154,7 @@ namespace two_points_planner
if (start_cost == costMapCostToSBPLCost(robot_costmap_2d::LETHAL_OBSTACLE) || start_cost == costMapCostToSBPLCost(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
|| end_cost == costMapCostToSBPLCost(robot_costmap_2d::LETHAL_OBSTACLE) || end_cost == costMapCostToSBPLCost(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE))
{
std::cerr << "[two_points_planner] base_pootprint in infated obstacle " << std::endl;
robot::log_error("[%s:%d]\n TwoPointsPlanner: base_pootprint in infated obstacle", __FILE__, __LINE__);
return false;
}
robot::Time plan_time = robot::Time::now();
@@ -171,7 +173,7 @@ namespace two_points_planner
}
else
{
std::cerr << "[two_points_planner] can not calculating theta from 'start point' or 'goal point'" << std::endl;
robot::log_error("[%s:%d]\n TwoPointsPlanner: can not calculating theta from 'start point' or 'goal point'", __FILE__, __LINE__);
return false;
}

15
src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp
View File

@@ -51,17 +51,18 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
parent_ = parent;
planner_nh_ = robot::NodeHandle(parent_, name);
robot::log_info_at(__FILE__, __LINE__, "Planner namespace: %s, parent namespace: %s", planner_nh_.getNamespace().c_str(), parent_.getNamespace().c_str());
this->getParams(planner_nh_);
// This is needed when using the CostmapAdapter to ensure that the costmap's info matches the rolling window
std::string traj_generator_name;
planner_nh_.param("trajectory_generator_name", traj_generator_name, std::string("nav_plugins::StandardTrajectoryGenerator"));
robot::log_info_at(__FILE__, __LINE__, "Using Trajectory Generator \"%s\"", traj_generator_name.c_str());
robot::log_info_at(__FILE__, __LINE__, "Using Trajectory Generator \"%s\"", traj_generator_name.c_str());
try
{
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
robot::PluginLoaderHelper loader;
std::string path_file_so = loader.findLibraryPath(traj_generator_name);
traj_gen_loader_ = boost::dll::import_alias<score_algorithm::TrajectoryGenerator::Ptr()>(
path_file_so, traj_generator_name, boost::dll::load_mode::append_decorations);
traj_generator_ = traj_gen_loader_();
@@ -84,7 +85,8 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
robot::log_info_at(__FILE__, __LINE__, "Using Algorithm \"%s\"", algorithm_nav_name.c_str());
try
{
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
robot::PluginLoaderHelper loader;
std::string path_file_so = loader.findLibraryPath(algorithm_nav_name);
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_();
@@ -106,7 +108,8 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
planner_nh_.param("goal_checker_name", goal_checker_name, std::string("score_algorithm::GoalChecker"));
try
{
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
robot::PluginLoaderHelper loader;
std::string path_file_so = loader.findLibraryPath(goal_checker_name);
goal_checker_loader_ = boost::dll::import_alias<score_algorithm::GoalChecker::Ptr()>(
path_file_so, goal_checker_name, boost::dll::load_mode::append_decorations);
goal_checker_ = goal_checker_loader_();

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

@@ -58,6 +58,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
parent_ = parent;
planner_nh_ = robot::NodeHandle(parent_, name);
robot::log_info_at(__FILE__, __LINE__, "Planner namespace: %s, parent namespace: %s", planner_nh_.getNamespace().c_str(), parent_.getNamespace().c_str());
this->getParams(planner_nh_);
std::string traj_generator_name;

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

@@ -55,7 +55,8 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
robot::log_info_at(__FILE__, __LINE__, "Using Trajectory Generator \"%s\"", traj_generator_name.c_str());
try
{
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
robot::PluginLoaderHelper loader;
std::string path_file_so = loader.findLibraryPath(traj_generator_name);
traj_gen_loader_ = boost::dll::import_alias<score_algorithm::TrajectoryGenerator::Ptr()>(
path_file_so, traj_generator_name, boost::dll::load_mode::append_decorations);
traj_generator_ = traj_gen_loader_();
@@ -77,7 +78,8 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
planner_nh_.param("algorithm_rotate_name", algorithm_rotate_name, std::string("nav_algorithm::Rotate"));
try
{
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
robot::PluginLoaderHelper loader;
std::string path_file_so = loader.findLibraryPath(algorithm_rotate_name);
rotate_algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
path_file_so, algorithm_rotate_name, boost::dll::load_mode::append_decorations);
rotate_algorithm_ = rotate_algorithm_loader_();
@@ -99,7 +101,8 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
planner_nh_.param("goal_checker_name", goal_checker_name, std::string("nav_algorithm::GoalChecker"));
try
{
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
robot::PluginLoaderHelper loader;
std::string path_file_so = loader.findLibraryPath(goal_checker_name);
goal_checker_loader_ = boost::dll::import_alias<score_algorithm::GoalChecker::Ptr()>(
path_file_so, goal_checker_name, boost::dll::load_mode::append_decorations);
goal_checker_ = goal_checker_loader_();

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

@@ -197,7 +197,8 @@ namespace robot_nav_core_adapter
// Plugin handling
boost::function<robot_nav_core2::LocalPlanner::Ptr()> planner_loader_;
robot_nav_core2::LocalPlanner::Ptr planner_;
bool initialized_;
// Pointer Copies
TFListenerPtr tf_;
@@ -211,7 +212,6 @@ namespace robot_nav_core_adapter
robot_nav_core_adapter::NavCoreAdapterConfig last_config_;
robot_nav_core_adapter::NavCoreAdapterConfig default_config_;
bool setup_;
};
} // namespace robot_nav_core_adapter

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

@@ -75,87 +75,104 @@ namespace robot_nav_core_adapter
*/
void LocalPlannerAdapter::initialize(std::string name, tf3::BufferCore *tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot)
{
tf_ = createSharedPointerWithNoDelete<tf3::BufferCore>(tf);
costmap_robot_ = costmap_robot;
costmap_adapter_ = std::make_shared<CostmapAdapter>();
costmap_adapter_->initialize(costmap_robot);
private_nh_ = robot::NodeHandle("~");
adapter_nh_ = robot::NodeHandle(private_nh_, name);
std::string planner_name;
if (adapter_nh_.hasParam("planner_name"))
if (!initialized_)
{
adapter_nh_.getParam("planner_name", planner_name, std::string("dwb_local_planner::DWBLocalPlanner"));
tf_ = createSharedPointerWithNoDelete<tf3::BufferCore>(tf);
costmap_robot_ = costmap_robot;
costmap_adapter_ = std::make_shared<CostmapAdapter>();
costmap_adapter_->initialize(costmap_robot);
private_nh_ = robot::NodeHandle("~");
adapter_nh_ = robot::NodeHandle(private_nh_, name);
std::string planner_name;
if (adapter_nh_.hasParam("planner_name"))
{
adapter_nh_.getParam("planner_name", planner_name, std::string("dwb_local_planner::DWBLocalPlanner"));
}
else
{
planner_name = robot_nav_2d_utils::searchAndGetParam(adapter_nh_, "position_planner_name", std::string("dwb_local_planner::DWBLocalPlanner"));
adapter_nh_.setParam("planner_name", planner_name);
}
try
{
robot::PluginLoaderHelper loader;
std::string path_file_so = loader.findLibraryPath(planner_name);
planner_loader_ = boost::dll::import_alias<robot_nav_core2::LocalPlanner::Ptr()>(
path_file_so, planner_name, boost::dll::load_mode::append_decorations);
planner_ = planner_loader_();
if (!planner_)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to load planner %s: returned null pointer", planner_name.c_str());
exit(EXIT_FAILURE);
}
last_config_.planner_name = planner_name;
robot::log_info("\nLocalPlannerAdapter initialize: %s", planner_name.c_str());
planner_->initialize(private_nh_, planner_name, tf_, costmap_robot_);
robot::log_info("[%s:%d]\n Successfully initialized planner \"%s\"", __FILE__, __LINE__, planner_name.c_str());
}
catch (const boost::system::system_error &ex)
{
robot::log_error_at(__FILE__, __LINE__, "System error while loading planner \"%s\": %s", planner_name.c_str(), ex.what());
exit(EXIT_FAILURE);
}
catch (const std::exception &ex)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to load planner \"%s\", are you sure it is properly registered and that the containing library is built? Exception: %s", planner_name.c_str(), ex.what());
exit(EXIT_FAILURE);
}
initialized_ = true;
robot::log_info("LocalPlannerAdapter initialized successfully");
}
else
{
planner_name = robot_nav_2d_utils::searchAndGetParam(adapter_nh_, "position_planner_name", std::string("dwb_local_planner::DWBLocalPlanner"));
adapter_nh_.setParam("planner_name", planner_name);
}
try
{
robot::PluginLoaderHelper loader;
std::string path_file_so = loader.findLibraryPath(planner_name);
planner_loader_ = boost::dll::import_alias<robot_nav_core2::LocalPlanner::Ptr()>(
path_file_so, planner_name, boost::dll::load_mode::append_decorations);
planner_ = planner_loader_();
if (!planner_)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to load planner %s: returned null pointer", planner_name.c_str());
exit(EXIT_FAILURE);
}
robot::log_info("\nLocalPlannerAdapter initialize: %s", planner_name.c_str());
planner_->initialize(private_nh_, planner_name, tf_, costmap_robot_);
robot::log_info("[%s:%d]\n Successfully initialized planner \"%s\"", __FILE__, __LINE__, planner_name.c_str());
}
catch (const boost::system::system_error &ex)
{
robot::log_error_at(__FILE__, __LINE__, "System error while loading planner \"%s\": %s", planner_name.c_str(), ex.what());
exit(EXIT_FAILURE);
}
catch (const std::exception &ex)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to load planner \"%s\", are you sure it is properly registered and that the containing library is built? Exception: %s", planner_name.c_str(), ex.what());
exit(EXIT_FAILURE);
robot::log_info("LocalPlannerAdapter already initialized");
}
}
bool LocalPlannerAdapter::swapPlanner(std::string planner_name)
{
boost::recursive_mutex::scoped_lock l(configuration_mutex_);
if (!setup_)
if (!initialized_)
{
return false;
}
if (planner_name != last_config_.planner_name)
{
robot_nav_core2::LocalPlanner::Ptr new_planner;
robot_nav_core2::LocalPlanner::Ptr old_planner = planner_;
auto old_loader = planner_loader_;
try
{
// Tạo planner mới
std::string path_file_so;
planner_loader_ = boost::dll::import_alias<robot_nav_core2::LocalPlanner::Ptr()>(
boost::unique_lock<boost::recursive_mutex> lock(configuration_mutex_);
robot::PluginLoaderHelper loader;
std::string path_file_so = loader.findLibraryPath(planner_name);
auto new_loader = boost::dll::import_alias<robot_nav_core2::LocalPlanner::Ptr()>(
path_file_so, planner_name, boost::dll::load_mode::append_decorations);
new_planner = planner_loader_();
auto new_planner = new_loader();
if (!new_planner)
{
std::cerr << "Failed to load planner " << planner_name << std::endl;
exit(EXIT_FAILURE);
robot::log_error_at(__FILE__, __LINE__, "Failed to load planner %s", planner_name.c_str());
throw std::runtime_error("Failed to load planner " + planner_name);
}
new_planner->initialize(adapter_nh_, planner_name, tf_, costmap_robot_);
if (planner_)
planner_.reset();
new_planner->initialize(private_nh_, planner_name, tf_, costmap_robot_);
planner_ = new_planner;
planner_loader_ = new_loader;
last_config_.planner_name = planner_name;
robot::log_info("Loaded new planner: %s", planner_name.c_str());
// Release old planner while old loader is still alive.
old_planner.reset();
old_loader = boost::function<robot_nav_core2::LocalPlanner::Ptr()>();
lock.unlock();
}
catch (const std::exception &ex)
{
robot::log_error("Failed to load planner %s: %s", planner_name.c_str(), ex.what());
robot::log_error_at(__FILE__, __LINE__, "Failed to load planner %s: %s", planner_name.c_str(), ex.what());
planner_ = old_planner;
planner_loader_ = old_loader;
return false;
}
}

6
src/Navigations/Packages/move_base/CMakeLists.txt
View File

@@ -4,7 +4,7 @@ project(move_base VERSION 1.0.0 LANGUAGES CXX)
if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
set(BUILDING_WITH_CATKIN TRUE)
message(STATUS "Building move_base with Catkin")
else()
set(BUILDING_WITH_CATKIN FALSE)
message(STATUS "Building move_base with Standalone CMake")
@@ -90,6 +90,10 @@ add_library(${PROJECT_NAME} SHARED
if(BUILDING_WITH_CATKIN)
add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_compile_definitions(${PROJECT_NAME}
PUBLIC BUILD_WITH_ROS
)
target_include_directories(${PROJECT_NAME}
PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>

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

@@ -4,6 +4,9 @@
*
* Author: Kevin Lee
*********************************************************************/
#ifdef BUILD_WITH_ROS
#include <ros/ros.h>
#endif
#include <move_base/move_base.h>
#include <move_base/convert_data.h>
#include <cmath>
@@ -231,6 +234,8 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
robot::log_error("[%s:%d]\n ERROR: planner_loader_() returned nullptr\n", __FILE__, __LINE__);
throw std::runtime_error("Failed to load global planner " + global_planner);
}
last_config_.base_global_planner = global_planner;
default_config_.base_global_planner = global_planner;
if (planner_->initialize(global_planner, planner_costmap_robot_))
robot::log_info("[%s:%d]\n Global planner initialized successfully", __FILE__, __LINE__);
else
@@ -333,7 +338,54 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
void move_base::MoveBase::swapPlanner(std::string base_global_planner)
{
if (base_global_planner != last_config_.base_global_planner)
{
robot_nav_core::BaseGlobalPlanner::Ptr old_planner = planner_;
auto old_loader = planner_loader_;
// initialize the global planner
try
{
// wait for the current planner to finish planning
boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
robot::PluginLoaderHelper loader;
std::string path_file_so = loader.findLibraryPath(base_global_planner);
auto new_loader = boost::dll::import_alias<robot_nav_core::BaseGlobalPlanner::Ptr()>(
path_file_so, base_global_planner, boost::dll::load_mode::append_decorations);
auto new_planner = new_loader();
if (!new_planner)
{
robot::log_error("[%s:%d]\n ERROR: planner_loader_() returned nullptr", __FILE__, __LINE__);
throw std::runtime_error("Failed to load global planner " + base_global_planner);
}
if (planner_plan_)
planner_plan_->clear();
if (latest_plan_)
latest_plan_->clear();
if (controller_plan_)
controller_plan_->clear();
resetState();
new_planner->initialize(base_global_planner, planner_costmap_robot_);
planner_ = new_planner;
planner_loader_ = new_loader;
last_config_.base_global_planner = base_global_planner;
// Release old planner while old loader is still alive.
old_planner.reset();
old_loader = boost::function<robot_nav_core::BaseGlobalPlanner::Ptr()>();
lock.unlock();
}
catch (const std::exception &ex)
{
robot::log_error("[%s:%d]\n Failed to create the %s planner, are you sure it is properly registered and that the \
containing library is built? Exception: %s",__FILE__, __LINE__, base_global_planner.c_str(), ex.what());
planner_ = old_planner;
planner_loader_ = old_loader;
}
}
}
void move_base::MoveBase::setRobotFootprint(const std::vector<robot_geometry_msgs::Point> &fprt)
@@ -788,8 +840,9 @@ bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg,
}
catch (const std::exception &e)
{
robot::log_error("[MoveBase] Exception in swapPlanner: %s", e.what());
throw std::exception(e);
robot::log_error("[MoveBase::moveTo] Exception in swapPlanner: %s", e.what());
lock.unlock();
return false;
}
if (nav_feedback_)
@@ -897,8 +950,9 @@ bool move_base::MoveBase::dockTo(const std::string &maker, const robot_geometry_
}
catch (const std::exception &e)
{
std::cerr << e.what() << "\n";
throw std::exception(e);
robot::log_error("[MoveBase::dockTo] Exception in swapPlanner: %s", e.what());
lock.unlock();
return false;
}
if (nav_feedback_)
@@ -923,6 +977,7 @@ bool move_base::MoveBase::dockTo(const robot_protocol_msgs::Order &msg,
bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped &goal, double xy_goal_tolerance)
{
robot::log_info("[MoveBase::moveStraightTo] Entry");
if (setup_)
{
std::string global_planner = "TwoPointsPlanner";
@@ -938,12 +993,10 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped
}
return false;
}
if (fabs(xy_goal_tolerance) > 0.001)
this->setXyGoalTolerance(fabs(xy_goal_tolerance));
else
this->setXyGoalTolerance(fabs(original_xy_goal_tolerance_));
if (!tc_)
{
throw std::runtime_error("Null 'tc_' pointer encountered");
@@ -954,7 +1007,17 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped
}
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
tc_->swapPlanner(go_straight_planner_name_);
try
{
tc_->swapPlanner(go_straight_planner_name_);
}
catch (const std::exception &e)
{
robot::log_error("[MoveBase::moveStraightTo] Exception in swapPlanner: %s", e.what());
lock.unlock();
return false;
}
if (nav_feedback_)
{
@@ -965,6 +1028,48 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped
if (cancel_ctr_)
cancel_ctr_ = false;
// Check if action server exists
if (!as_)
{
robot::log_error("[MoveBase::moveTo] as_ pointer is null!");
lock.unlock();
return false;
}
try
{
robot_move_base_msgs::MoveBaseActionGoalPtr action_goal = boost::make_shared<robot_move_base_msgs::MoveBaseActionGoal>();
action_goal->header.stamp = robot::Time::now();
action_goal->goal.target_pose = goal;
// Generate unique goal ID using timestamp
robot::Time now = robot::Time::now();
action_goal->goal_id.stamp = now;
std::ostringstream goal_id_stream;
goal_id_stream << "move_base_goal_" << now.sec << "_" << now.nsec;
action_goal->goal_id.id = goal_id_stream.str();
robot::log_info("[MoveBase::moveStraightTo] Generated goal ID: %s", action_goal->goal_id.id.c_str());
robot::log_info("[MoveBase::moveStraightTo] Goal stamp: %ld.%09ld",
action_goal->goal_id.stamp.sec, action_goal->goal_id.stamp.nsec);
// Clear Order message since this is a non-Order moveTo call
{
boost::unique_lock<boost::recursive_mutex> planner_lock(planner_mutex_);
planner_order_.reset();
}
robot::log_info("[MoveBase::moveStraightTo] Processing goal through action server...");
as_->processGoal(action_goal);
robot::log_info("[MoveBase::moveStraightTo] Goal processed successfully by action server");
}
catch (const std::exception &e)
{
lock.unlock();
robot::log_error("[MoveBase::moveStraightTo] Exception during processGoal: %s", e.what());
return false;
}
lock.unlock();
return true;
}
@@ -987,7 +1092,7 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal,
return false;
}
this->setXyGoalTolerance(0.25);
this->setXyGoalTolerance(std::numeric_limits<double>::infinity());
if (fabs(yaw_goal_tolerance) > 0.001)
this->setYawGoalTolerance(fabs(yaw_goal_tolerance));
@@ -1003,7 +1108,17 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal,
throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
}
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
tc_->swapPlanner(rotate_planner_name_);
try
{
tc_->swapPlanner(rotate_planner_name_);
}
catch (const std::exception &e)
{
robot::log_error("[MoveBase::rotateTo] Exception in swapPlanner: %s", e.what());
lock.unlock();
return false;
}
if (nav_feedback_)
{
@@ -1013,17 +1128,49 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal,
}
if (cancel_ctr_)
cancel_ctr_ = false;
robot_geometry_msgs::PoseStamped pose;
if (!this->getRobotPose(pose))
// Check if action server exists
if (!as_)
{
if (nav_feedback_)
{
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
nav_feedback_->feed_back_str = std::string("Coudn't get robot pose");
nav_feedback_->goal_checked = false;
}
robot::log_error("[MoveBase::moveTo] as_ pointer is null!");
lock.unlock();
return false;
}
try
{
robot_move_base_msgs::MoveBaseActionGoalPtr action_goal = boost::make_shared<robot_move_base_msgs::MoveBaseActionGoal>();
action_goal->header.stamp = robot::Time::now();
action_goal->goal.target_pose = goal;
// Generate unique goal ID using timestamp
robot::Time now = robot::Time::now();
action_goal->goal_id.stamp = now;
std::ostringstream goal_id_stream;
goal_id_stream << "move_base_goal_" << now.sec << "_" << now.nsec;
action_goal->goal_id.id = goal_id_stream.str();
robot::log_info("[MoveBase::rotateTo] Generated goal ID: %s", action_goal->goal_id.id.c_str());
robot::log_info("[MoveBase::rotateTo] Goal stamp: %ld.%09ld",
action_goal->goal_id.stamp.sec, action_goal->goal_id.stamp.nsec);
// Clear Order message since this is a non-Order moveTo call
{
boost::unique_lock<boost::recursive_mutex> planner_lock(planner_mutex_);
planner_order_.reset();
}
robot::log_info("[MoveBase::rotateTo] Processing goal through action server...");
as_->processGoal(action_goal);
robot::log_info("[MoveBase::rotateTo] Goal processed successfully by action server");
}
catch (const std::exception &e)
{
lock.unlock();
robot::log_error("[MoveBase::moveStraightTo] Exception during processGoal: %s", e.what());
return false;
}
lock.unlock();
return true;
}