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

update

Browse Source
This commit is contained in:
HiepLM
2026-03-04 09:43:39 +00:00
parent a1cc2fccb1
commit 4617ce85b6
12 changed files with 177 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

0
config/mprim/maker_sources.yaml → config/maker_sources.yaml
View File

14
examples/NavigationExample/Program.cs
View File

@@ -404,17 +404,19 @@ namespace NavigationExample
goal.pose.orientation.z = 0.0; goal.pose.orientation.z = 0.0;
goal.pose.orientation.w = 1.0; goal.pose.orientation.w = 1.0;
NavigationAPI.navigation_dock_to_order(navHandle, order, "docking_point", goal);
// NavigationAPI.navigation_move_to_order(navHandle, order, goal);` Console.WriteLine("Docking to docking_point");
NavigationAPI.navigation_dock_to_order(navHandle, order, "charger", goal);
// NavigationAPI.navigation_move_to_order(navHandle, order, goal);
NavigationAPI.navigation_set_twist_linear(navHandle, 0.1, 0.0, 0.0); NavigationAPI.navigation_set_twist_linear(navHandle, 0.1, 0.0, 0.0);
NavigationAPI.navigation_set_twist_angular(navHandle, 0.0, 0.0, 0.2); NavigationAPI.navigation_set_twist_angular(navHandle, 0.0, 0.0, 0.2);
// NavigationAPI.navigation_move_straight_to(navHandle, 1.0); // NavigationAPI.navigation_move_straight_to(navHandle, 1.0);
// while (true) while (true)
// { {
// System.Threading.Thread.Sleep(100); System.Threading.Thread.Sleep(100);
// NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback(); // NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback();
// if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback)) // if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback))
// { // {
@@ -474,7 +476,7 @@ namespace NavigationExample
// Console.WriteLine("Local Costmap is not updated"); // Console.WriteLine("Local Costmap is not updated");
// } // }
// } // }
// } }
// Cleanup (destroy nav first, then tf3 buffer) // Cleanup (destroy nav first, then tf3 buffer)
NavigationAPI.navigation_destroy(navHandle); NavigationAPI.navigation_destroy(navHandle);

BIN
examples/NavigationExample/libnav_c_api.so
View File

Binary file not shown.

1
src/Algorithms/Packages/local_planners/pnkx_local_planner/CMakeLists.txt
View File

@@ -101,6 +101,7 @@ add_library(${PROJECT_NAME} SHARED
src/pnkx_local_planner.cpp src/pnkx_local_planner.cpp
src/pnkx_go_straight_local_planner.cpp src/pnkx_go_straight_local_planner.cpp
src/pnkx_rotate_local_planner.cpp src/pnkx_rotate_local_planner.cpp
src/pnkx_docking_local_planner.cpp
) )
if(BUILDING_WITH_CATKIN) if(BUILDING_WITH_CATKIN)

9
src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_docking_local_planner.h
View File

@@ -1,8 +1,9 @@
#ifndef _PNKX_DOCKING_LOCAL_PLANNER_H_INCLUDED_ #ifndef _PNKX_DOCKING_LOCAL_PLANNER_H_INCLUDED_
#define _PNKX_DOCKING_LOCAL_PLANNER_H_INCLUDED_ #define _PNKX_DOCKING_LOCAL_PLANNER_H_INCLUDED_
#include <robot_nav_core2/base_global_planner.h> #include <robot_nav_core/base_global_planner.h>
#include <pnkx_local_planner/pnkx_local_planner.h> #include <pnkx_local_planner/pnkx_local_planner.h>
#include <robot_xmlrpcpp/XmlRpcValue.h>
namespace pnkx_local_planner namespace pnkx_local_planner
{ {
@@ -137,7 +138,7 @@ namespace pnkx_local_planner
std::string docking_planner_name_; std::string docking_planner_name_;
std::string docking_nav_name_; std::string docking_nav_name_;
robot_nav_core2::BaseGlobalPlanner::Ptr docking_planner_; robot_nav_core::BaseGlobalPlanner::Ptr docking_planner_;
score_algorithm::ScoreAlgorithm::Ptr docking_nav_; score_algorithm::ScoreAlgorithm::Ptr docking_nav_;
robot_geometry_msgs::Vector3 linear_; robot_geometry_msgs::Vector3 linear_;
@@ -158,7 +159,7 @@ namespace pnkx_local_planner
TFListenerPtr tf_; TFListenerPtr tf_;
robot_costmap_2d::Costmap2DROBOT *costmap_robot_; robot_costmap_2d::Costmap2DROBOT *costmap_robot_;
score_algorithm::TrajectoryGenerator::Ptr traj_generator_; score_algorithm::TrajectoryGenerator::Ptr traj_generator_;
std::function<robot_nav_core2::BaseGlobalPlanner::Ptr()> docking_planner_loader_; std::function<robot_nav_core::BaseGlobalPlanner::Ptr()> docking_planner_loader_;
std::function<score_algorithm::ScoreAlgorithm::Ptr()> docking_nav_loader_; std::function<score_algorithm::ScoreAlgorithm::Ptr()> docking_nav_loader_;
robot::WallTimer detected_timeout_wt_, delayed_wt_; robot::WallTimer detected_timeout_wt_, delayed_wt_;
@@ -169,7 +170,7 @@ namespace pnkx_local_planner
bool start_docking_; bool start_docking_;
double original_xy_goal_tolerance_, original_yaw_goal_tolerance_; double original_xy_goal_tolerance_, original_yaw_goal_tolerance_;
XmlRpc::XmlRpcValue original_papams_; robot_xmlrpcpp::XmlRpcValue original_papams_;
std::vector<DockingPlanner*> dkpl_; std::vector<DockingPlanner*> dkpl_;
bool dockingHanlde(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity); bool dockingHanlde(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity);

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

@@ -56,19 +56,18 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
if (!costmap_robot_->isCurrent()) if (!costmap_robot_->isCurrent())
throw robot_nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow."); throw robot_nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");
nh_ = robot::NodeHandle("~");
parent_ = parent; parent_ = parent;
planner_nh_ = robot::NodeHandle(parent_, name); planner_nh_ = robot::NodeHandle(parent_, name);
robot::log_info_at(__FILE__, __LINE__, "Planner namespace: %s", planner_nh_.getNamespace().c_str()); 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->getParams(planner_nh_);
std::string traj_generator_name; std::string traj_generator_name;
planner_nh_.param("trajectory_generator_name", traj_generator_name, std::string("nav_plugins::StandardTrajectoryGenerator")); planner_nh_.param("trajectory_generator_name", traj_generator_name, std::string("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 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()>( traj_gen_loader_ = boost::dll::import_alias<score_algorithm::TrajectoryGenerator::Ptr()>(
path_file_so, traj_generator_name, boost::dll::load_mode::append_decorations); path_file_so, traj_generator_name, boost::dll::load_mode::append_decorations);
traj_generator_ = traj_gen_loader_(); traj_generator_ = traj_gen_loader_();
@@ -77,7 +76,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
robot::log_error_at(__FILE__, __LINE__, "Failed to create trajectory generator \"%s\": returned null pointer", traj_generator_name.c_str()); robot::log_error_at(__FILE__, __LINE__, "Failed to create trajectory generator \"%s\": returned null pointer", traj_generator_name.c_str());
exit(1); exit(1);
} }
robot::NodeHandle nh_traj_gen = robot::NodeHandle(nh_, traj_generator_name); robot::NodeHandle nh_traj_gen = robot::NodeHandle(parent_, traj_generator_name);
traj_generator_->initialize(nh_traj_gen); traj_generator_->initialize(nh_traj_gen);
robot::log_info_at(__FILE__, __LINE__, "Successfully initialized trajectory generator \"%s\"", traj_generator_name.c_str()); robot::log_info_at(__FILE__, __LINE__, "Successfully initialized trajectory generator \"%s\"", traj_generator_name.c_str());
} }
@@ -87,21 +86,22 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
exit(1); exit(1);
} }
std::string algorithm_nav_name; std::string algorithm_nav_name;
planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA")); planner_nh_.getParam("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
robot::log_info_at(__FILE__, __LINE__, "Using Algorithm \"%s\"", algorithm_nav_name.c_str()); robot::log_info_at(__FILE__, __LINE__, "Using Algorithm \"%s\"", algorithm_nav_name.c_str());
try 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;
algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>( 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); path_file_so, algorithm_nav_name, boost::dll::load_mode::append_decorations);
nav_algorithm_ = algorithm_loader_(); nav_algorithm_ = nav_algorithm_loader_();
if (!nav_algorithm_) if (!nav_algorithm_)
{ {
robot::log_error_at(__FILE__, __LINE__, "Failed to load algorithm %s", algorithm_nav_name.c_str()); robot::log_error_at(__FILE__, __LINE__, "Failed to load algorithm %s", algorithm_nav_name.c_str());
exit(1); exit(1);
} }
nav_algorithm_->initialize(nh_, algorithm_nav_name, tf, costmap_robot_, traj_generator_); nav_algorithm_->initialize(parent_, algorithm_nav_name, tf, costmap_robot_, traj_generator_);
} }
catch (const std::exception &ex) catch (const std::exception &ex)
{ {
@@ -110,19 +110,20 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
} }
std::string algorithm_rotate_name; std::string algorithm_rotate_name;
planner_nh_.param("algorithm_rotate_name", algorithm_rotate_name, std::string("pnkx_local_planner::RotateToGoalDiff")); planner_nh_.param("algorithm_rotate_name", algorithm_rotate_name, std::string("MKTAlgorithmDiffRotateToGoal"));
try 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;
algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>( 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); path_file_so, algorithm_rotate_name, boost::dll::load_mode::append_decorations);
rotate_algorithm_ = algorithm_loader_(); rotate_algorithm_ = rotate_algorithm_loader_();
if (!rotate_algorithm_) if (!rotate_algorithm_)
{ {
robot::log_error_at(__FILE__, __LINE__, "Failed to create rotate algorithm \"%s\": returned null pointer", algorithm_rotate_name.c_str()); robot::log_error_at(__FILE__, __LINE__, "Failed to create rotate algorithm \"%s\": returned null pointer", algorithm_rotate_name.c_str());
exit(1); exit(1);
} }
rotate_algorithm_->initialize(nh_, algorithm_rotate_name, tf, costmap_robot_, traj_generator_); rotate_algorithm_->initialize(parent_, algorithm_rotate_name, tf, costmap_robot_, traj_generator_);
robot::log_info_at(__FILE__, __LINE__, "Successfully initialized rotate algorithm \"%s\"", algorithm_rotate_name.c_str()); robot::log_info_at(__FILE__, __LINE__, "Successfully initialized rotate algorithm \"%s\"", algorithm_rotate_name.c_str());
} }
catch (const std::exception &ex) catch (const std::exception &ex)
@@ -133,10 +134,12 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
std::string goal_checker_name; std::string goal_checker_name;
planner_nh_.param("goal_checker_name", goal_checker_name, std::string("dwb_plugins::SimpleGoalChecker")); planner_nh_.param("goal_checker_name", goal_checker_name, std::string("dwb_plugins::SimpleGoalChecker"));
robot::log_info_at(__FILE__, __LINE__, "goal_checker_name: %s", goal_checker_name.c_str());
try 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;
boost::dll::import_alias<score_algorithm::GoalChecker::Ptr()>( 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); path_file_so, goal_checker_name, boost::dll::load_mode::append_decorations);
goal_checker_ = goal_checker_loader_(); goal_checker_ = goal_checker_loader_();
if (!goal_checker_) if (!goal_checker_)
@@ -144,7 +147,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
robot::log_error_at(__FILE__, __LINE__, "Failed to create goal checker \"%s\": returned null pointer", goal_checker_name.c_str()); robot::log_error_at(__FILE__, __LINE__, "Failed to create goal checker \"%s\": returned null pointer", goal_checker_name.c_str());
exit(1); exit(1);
} }
goal_checker_->initialize(nh_); goal_checker_->initialize(parent_);
robot::log_info_at(__FILE__, __LINE__, "Successfully initialized goal checker \"%s\"", goal_checker_name.c_str()); robot::log_info_at(__FILE__, __LINE__, "Successfully initialized goal checker \"%s\"", goal_checker_name.c_str());
} }
catch (const std::exception &ex) catch (const std::exception &ex)
@@ -152,6 +155,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
robot::log_error_at(__FILE__, __LINE__, "Unexpected exception while creating goal checker \"%s\": %s", goal_checker_name.c_str(), ex.what()); robot::log_error_at(__FILE__, __LINE__, "Unexpected exception while creating goal checker \"%s\": %s", goal_checker_name.c_str(), ex.what());
exit(1); exit(1);
} }
this->initializeOthers(); this->initializeOthers();
this->getMaker(); this->getMaker();
robot::log_info_at(__FILE__, __LINE__, "%s is sucessed", name.c_str()); robot::log_info_at(__FILE__, __LINE__, "%s is sucessed", name.c_str());
@@ -162,8 +166,10 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
void pnkx_local_planner::PNKXDockingLocalPlanner::getMaker() void pnkx_local_planner::PNKXDockingLocalPlanner::getMaker()
{ {
std::string maker_name, maker_sources; std::string maker_name, maker_sources;
nh_.param("maker_name", maker_name, std::string("")); parent_.param("maker_name", maker_name, std::string(""));
nh_.param("maker_sources", maker_sources, std::string("")); parent_.param("maker_sources", maker_sources, std::string(""));
robot::log_info_at(__FILE__, __LINE__, "maker_name: %s", maker_name.c_str());
robot::log_info_at(__FILE__, __LINE__, "maker_sources: %s", maker_sources.c_str());
std::stringstream ss(maker_sources); std::stringstream ss(maker_sources);
std::string source; std::string source;
@@ -171,24 +177,23 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::getMaker()
{ {
if (maker_name == source) if (maker_name == source)
{ {
robot::NodeHandle source_node(nh_, source); robot::NodeHandle source_node(parent_, source);
if (source_node.hasParam("plugins")) if (source_node.hasParam("plugins"))
{ {
XmlRpc::XmlRpcValue plugins; robot_xmlrpcpp::XmlRpcValue plugins;
source_node.getParam("plugins", plugins); source_node.getParam("plugins", plugins);
if (plugins.getType() == XmlRpc::XmlRpcValue::TypeArray) if (plugins.getType() == robot_xmlrpcpp::XmlRpcValue::TypeArray)
{ {
for (int i = 0; i < plugins.size(); ++i) for (int i = 0; i < plugins.size(); ++i)
{ {
if (plugins[i].getType() == XmlRpc::XmlRpcValue::TypeStruct) if (plugins[i].getType() == robot_xmlrpcpp::XmlRpcValue::TypeStruct)
{ {
std::stringstream name; std::stringstream name;
name << source << "/" << static_cast<std::string>(plugins[i]["name"]); name << source << "/" << static_cast<std::string>(plugins[i]["name"]);
std::string docking_planner_name = static_cast<std::string>(plugins[i]["docking_planner"]); std::string docking_planner_name = static_cast<std::string>(plugins[i]["docking_planner"]);
std::string docking_nav_name = static_cast<std::string>(plugins[i]["docking_nav"]); std::string docking_nav_name = static_cast<std::string>(plugins[i]["docking_nav"]);
// std::shared_ptr<DockingPlanner> dkpl = std::make_shared<DockingPlanner>(name.str()); // std::shared_ptr<DockingPlanner> dkpl = std::make_shared<DockingPlanner>(name.str());
DockingPlanner* dkpl = new DockingPlanner(name.str()); DockingPlanner* dkpl = new DockingPlanner(name.str());
dkpl->docking_planner_name_ = docking_planner_name; dkpl->docking_planner_name_ = docking_planner_name;
@@ -215,7 +220,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initializeOthers()
{ {
std::string algorithm_nav_name; std::string algorithm_nav_name;
planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA")); planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
if (!nh_.getParam(algorithm_nav_name, original_papams_)) if (!parent_.getParam(algorithm_nav_name, original_papams_))
robot::log_warning_at(__FILE__, __LINE__, "No found in %s in yaml-file, please check configuration", algorithm_nav_name.c_str()); robot::log_warning_at(__FILE__, __LINE__, "No found in %s in yaml-file, please check configuration", algorithm_nav_name.c_str());
original_xy_goal_tolerance_ = xy_goal_tolerance_; original_xy_goal_tolerance_ = xy_goal_tolerance_;
original_yaw_goal_tolerance_ = yaw_goal_tolerance_; original_yaw_goal_tolerance_ = yaw_goal_tolerance_;
@@ -247,8 +252,8 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::reset()
std::string algorithm_nav_name; std::string algorithm_nav_name;
planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA")); planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
robot::NodeHandle nh_algorithm = robot::NodeHandle(nh_, algorithm_nav_name); robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name);
nh_.setParam(algorithm_nav_name, original_papams_); parent_.setParam(algorithm_nav_name, original_papams_);
nh_algorithm.setParam("allow_rotate", false); nh_algorithm.setParam("allow_rotate", false);
} }
@@ -279,7 +284,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
try try
{ {
if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_)) if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_global_plan_))
robot::log_warning_at(__FILE__, __LINE__, "Transform global plan is failed"); robot::log_warning_at(__FILE__, __LINE__, "Transform global plan is failed");
} }
catch(const robot_nav_core2::LocalPlannerException& e) catch(const robot_nav_core2::LocalPlannerException& e)
@@ -290,7 +295,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
double x_direction, y_direction, theta_direction; double x_direction, y_direction, theta_direction;
if (!ret_nav_) if (!ret_nav_)
{ {
if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction)) if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
{ {
robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str()); robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str());
throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare"); throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
@@ -309,15 +314,15 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
std::string algorithm_nav_name; std::string algorithm_nav_name;
planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA")); planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
robot::NodeHandle nh_algorithm = robot::NodeHandle(nh_, algorithm_nav_name); robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name);
nh_algorithm.setParam("allow_rotate", dkpl_.front()->allow_rotate_); nh_algorithm.setParam("allow_rotate", dkpl_.front()->allow_rotate_);
nh_algorithm.setParam("min_lookahead_dist", dkpl_.front()->min_lookahead_dist_); nh_algorithm.setParam("min_lookahead_dist", dkpl_.front()->min_lookahead_dist_);
nh_algorithm.setParam("max_lookahead_dist", dkpl_.front()->max_lookahead_dist_); nh_algorithm.setParam("max_lookahead_dist", dkpl_.front()->max_lookahead_dist_);
nh_algorithm.setParam("lookahead_time", dkpl_.front()->lookahead_time_); nh_algorithm.setParam("lookahead_time", dkpl_.front()->lookahead_time_);
nh_algorithm.setParam("angle_threshold", dkpl_.front()->angle_threshold_); nh_algorithm.setParam("angle_threshold", dkpl_.front()->angle_threshold_);
nh_.setParam("xy_goal_tolerance", dkpl_.front()->xy_goal_tolerance_); planner_nh_.setParam("xy_goal_tolerance", dkpl_.front()->xy_goal_tolerance_);
nh_.setParam("yaw_goal_tolerance", dkpl_.front()->yaw_goal_tolerance_); planner_nh_.setParam("yaw_goal_tolerance", dkpl_.front()->yaw_goal_tolerance_);
if (dkpl_.front()->docking_nav_ && !dkpl_.front()->docking_planner_) if (dkpl_.front()->docking_nav_ && !dkpl_.front()->docking_planner_)
{ {
@@ -330,7 +335,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
local_goal_pose = follow_pose; local_goal_pose = follow_pose;
} }
} }
if (!dkpl_.front()->docking_nav_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction)) if (!dkpl_.front()->docking_nav_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
{ {
throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare"); throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str()); robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str());
@@ -340,7 +345,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
} }
else else
{ {
if (!rotate_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction)) if (!rotate_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
{ {
throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare"); throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", rotate_algorithm_->getName().c_str()); robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", rotate_algorithm_->getName().c_str());
@@ -418,7 +423,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::isGoalReached(const robot_nav_
// goal_pose_.header.stamp = pose.header.stamp; // goal_pose_.header.stamp = pose.header.stamp;
robot_nav_2d_msgs::Pose2DStamped local_pose = this->transformPoseToLocal(pose); robot_nav_2d_msgs::Pose2DStamped local_pose = this->transformPoseToLocal(pose);
robot_nav_2d_msgs::Pose2DStamped local_goal = this->transformPoseToLocal(goal_pose_); robot_nav_2d_msgs::Pose2DStamped local_goal = this->transformPoseToLocal(goal_pose_);
robot_nav_2d_msgs::Path2D plan = transformed_plan_; robot_nav_2d_msgs::Path2D plan = transformed_global_plan_;
if (start_docking_) if (start_docking_)
{ {
local_goal = goal_pose_; local_goal = goal_pose_;
@@ -462,8 +467,8 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::isGoalReached(const robot_nav_
// nh_.setParam("yaw_goal_tolerance", original_yaw_goal_tolerance_); // nh_.setParam("yaw_goal_tolerance", original_yaw_goal_tolerance_);
std::string algorithm_nav_name; std::string algorithm_nav_name;
planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA")); planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
robot::NodeHandle nh_algorithm = robot::NodeHandle(nh_, algorithm_nav_name); robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name);
nh_.setParam(algorithm_nav_name, original_papams_); parent_.setParam(algorithm_nav_name, original_papams_);
nh_algorithm.setParam("allow_rotate", false); nh_algorithm.setParam("allow_rotate", false);
} }
return ret_nav_ && ret_angle_ && dock_ok; return ret_nav_ && ret_angle_ && dock_ok;
@@ -479,7 +484,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_
{ {
if (!dkpl_.front()->initialized_) if (!dkpl_.front()->initialized_)
{ {
dkpl_.front()->initialize(nh_, tf_, costmap_robot_, traj_generator_); dkpl_.front()->initialize(parent_, tf_, costmap_robot_, traj_generator_);
dkpl_.front()->initialized_ = true; dkpl_.front()->initialized_ = true;
} }
else else
@@ -589,7 +594,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(rob
try 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"; 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";
docking_planner_loader_ = boost::dll::import_alias<robot_nav_core2::BaseGlobalPlanner::Ptr()>( docking_planner_loader_ = boost::dll::import_alias<robot_nav_core::BaseGlobalPlanner::Ptr()>(
path_file_so, docking_planner_name_, boost::dll::load_mode::append_decorations); path_file_so, docking_planner_name_, boost::dll::load_mode::append_decorations);
docking_planner_ = docking_planner_loader_(); docking_planner_ = docking_planner_loader_();
if (!docking_planner_) if (!docking_planner_)
@@ -620,7 +625,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(rob
robot::log_error_at(__FILE__, __LINE__, "Failed to create docking nav \"%s\": returned null pointer", docking_nav_name_.c_str()); robot::log_error_at(__FILE__, __LINE__, "Failed to create docking nav \"%s\": returned null pointer", docking_nav_name_.c_str());
exit(1); exit(1);
} }
robot::NodeHandle nh_docking_nav = robot::NodeHandle(nh_, docking_nav_name_); robot::NodeHandle nh_docking_nav = robot::NodeHandle(nh_priv_, docking_nav_name_);
docking_nav_->initialize(nh_docking_nav, docking_nav_name_, tf_, costmap_robot_, traj_generator_); docking_nav_->initialize(nh_docking_nav, docking_nav_name_, tf_, costmap_robot_, traj_generator_);
robot::log_info_at(__FILE__, __LINE__, "Created docking nav %s", docking_nav_name_.c_str()); robot::log_info_at(__FILE__, __LINE__, "Created docking nav %s", docking_nav_name_.c_str());
} }
@@ -657,13 +662,22 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(rob
nh_priv_.param("lookahead_time", lookahead_time_, 1.5); nh_priv_.param("lookahead_time", lookahead_time_, 1.5);
nh_priv_.param("angle_threshold", angle_threshold_, 0.4); nh_priv_.param("angle_threshold", angle_threshold_, 0.4);
detected_timeout_wt_ = robot::log_info("delay: %f", delay_time);
nh_priv_.createWallTimer(::robot::WallDuration(time_out + delay_time), &pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::detectedTimeOutCb, this); robot::log_info("timeout: %f", time_out);
robot::log_info("xy_goal_tolerance: %f", xy_goal_tolerance_);
robot::log_info("yaw_goal_tolerance: %f", yaw_goal_tolerance_);
robot::log_info("min_lookahead_dist: %f", min_lookahead_dist_);
robot::log_info("max_lookahead_dist: %f", max_lookahead_dist_);
robot::log_info("lookahead_time: %f", lookahead_time_);
robot::log_info("angle_threshold: %f", angle_threshold_);
detected_timeout_wt_ = ::robot::WallTimer(
::robot::WallDuration(time_out + delay_time), &pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::detectedTimeOutCb, this, false, false);
detected_timeout_wt_.start(); detected_timeout_wt_.start();
robot::log_warning_at(__FILE__, __LINE__, "%s %f time_out start %f", name_.c_str(), delay_time, robot::Time::now().toSec()); robot::log_warning_at(__FILE__, __LINE__, "%s %f time_out start %f", name_.c_str(), delay_time, robot::Time::now().toSec());
delayed_wt_ = delayed_wt_ = ::robot::WallTimer(
nh_priv_.createWallTimer(::robot::WallDuration(delay_time), &pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::delayCb, this); ::robot::WallDuration(delay_time), &pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::delayCb, this, false, false);
delayed_wt_.start(); delayed_wt_.start();
robot::log_warning_at(__FILE__, __LINE__, "%s %f delay start %f", name_.c_str(), delay_time, robot::Time::now().toSec()); robot::log_warning_at(__FILE__, __LINE__, "%s %f delay start %f", name_.c_str(), delay_time, robot::Time::now().toSec());

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

@@ -120,7 +120,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
catch (const std::exception &ex) catch (const std::exception &ex)
{ {
robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s algorithm , are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_nav_name.c_str(), ex.what()); robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s algorithm , are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_nav_name.c_str(), ex.what());
// exit(1); exit(1);
} }
std::string algorithm_rotate_name; std::string algorithm_rotate_name;
@@ -143,7 +143,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
catch (const std::exception &ex) catch (const std::exception &ex)
{ {
robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s rotate algorithm, are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_rotate_name.c_str(), ex.what()); robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s rotate algorithm, are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_rotate_name.c_str(), ex.what());
// exit(1); exit(1);
} }
std::string goal_checker_name; std::string goal_checker_name;

8
src/Libraries/robot_cpp/include/robot/node_handle.h
View File

@@ -283,6 +283,14 @@ namespace robot
*/ */
bool getParam (const std::string &key, YAML::Node &v, YAML::Node default_value = YAML::Node()) const; bool getParam (const std::string &key, YAML::Node &v, YAML::Node default_value = YAML::Node()) const;
/**
* @brief Get a parameter as robot_xmlrpcpp::XmlRpcValue (converted from YAML).
* @param key The parameter key (supports nested keys with '/' separator).
* @param v Storage for the retrieved value. Left unchanged if key not found.
* @return true if the parameter was retrieved and converted successfully, false otherwise.
*/
bool getParam (const std::string &key, robot_xmlrpcpp::XmlRpcValue &v) const;
/** /**
* @brief Template method to get a parameter value (without default). * @brief Template method to get a parameter value (without default).
* *

12
src/Libraries/robot_cpp/include/robot/robot.h
View File

@@ -2,11 +2,17 @@
#define ROBOT_ROBOT_H #define ROBOT_ROBOT_H
#include <robot/init.h> #include <robot/init.h>
#include <robot/time.h>
#include <robot/timer.h>
#include <robot/rate.h>
#include <robot/console.h> #include <robot/console.h>
#include <robot/node_handle.h> #include <robot/node_handle.h>
#include <robot/plugin_loader_helper.h> #include <robot/plugin_loader_helper.h>
#include <robot/time.h>
#include <robot/timer.h>
#include <robot/duration.h>
#include <robot/wall_timer.h>
#include <robot/rate.h>
#include <robot/exception.h>
#include <robot/macros.h>
#include <robot/platform.h>
#endif #endif

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

@@ -1727,6 +1727,87 @@ namespace robot
return true; return true;
} }
namespace
{
bool yamlToXmlRpc(const YAML::Node &y, robot_xmlrpcpp::XmlRpcValue &out)
{
if (!y.IsDefined())
return false;
if (y.IsScalar())
{
try
{
std::string s = y.as<std::string>();
std::string lower = s;
std::transform(lower.begin(), lower.end(), lower.begin(), ::tolower);
if (lower == "true" || lower == "1" || lower == "yes" || lower == "on")
{
out = robot_xmlrpcpp::XmlRpcValue(true);
return true;
}
if (lower == "false" || lower == "0" || lower == "no" || lower == "off")
{
out = robot_xmlrpcpp::XmlRpcValue(false);
return true;
}
try
{
int i = y.as<int>();
out = robot_xmlrpcpp::XmlRpcValue(i);
return true;
}
catch (...) {}
try
{
double d = y.as<double>();
out = robot_xmlrpcpp::XmlRpcValue(d);
return true;
}
catch (...) {}
out = robot_xmlrpcpp::XmlRpcValue(s);
return true;
}
catch (...)
{
return false;
}
}
if (y.IsSequence())
{
out = robot_xmlrpcpp::XmlRpcValue();
out.setSize(static_cast<int>(y.size()));
for (size_t i = 0; i < y.size(); ++i)
{
robot_xmlrpcpp::XmlRpcValue item;
if (yamlToXmlRpc(y[i], item))
out[static_cast<int>(i)] = item;
}
return true;
}
if (y.IsMap())
{
out = robot_xmlrpcpp::XmlRpcValue();
for (YAML::const_iterator it = y.begin(); it != y.end(); ++it)
{
std::string k = it->first.as<std::string>();
robot_xmlrpcpp::XmlRpcValue item;
if (yamlToXmlRpc(it->second, item))
out[k] = item;
}
return true;
}
return false;
}
}
bool NodeHandle::getParam(const std::string &key, robot_xmlrpcpp::XmlRpcValue &v) const
{
YAML::Node value = getNestedValue(key);
if (!value.IsDefined())
return false;
return yamlToXmlRpc(value, v);
}
template <typename T> template <typename T>
T NodeHandle::param(const std::string &param_name) const T NodeHandle::param(const std::string &param_name) const
{ {

2
src/Libraries/robot_time

Submodule src/Libraries/robot_time updated: 750dc94c61...8201dcc6bd

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

@@ -527,8 +527,7 @@ void move_base::MoveBase::addLaserScan(const std::string &laser_scan_name, robot
// for (size_t i = 0; i < laser_scan.intensities.size(); i++) { // for (size_t i = 0; i < laser_scan.intensities.size(); i++) {
// intensities_str << laser_scan.intensities[i] << " "; // intensities_str << laser_scan.intensities[i] << " ";
// } // }
// robot::log_info("intensities: %s", intensities_str.str().c_str()); // robot::log_error("intensities: %s", intensities_str.str().c_str());
// robot::log_info("--------------------------------");
updateLocalCostmap<robot_sensor_msgs::LaserScan>(laser_scan, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name); updateLocalCostmap<robot_sensor_msgs::LaserScan>(laser_scan, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name);
updateGlobalCostmap<robot_sensor_msgs::LaserScan>(laser_scan, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name); updateGlobalCostmap<robot_sensor_msgs::LaserScan>(laser_scan, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name);
@@ -2198,7 +2197,6 @@ void move_base::MoveBase::planThread()
// if we didn't get a plan and we are in the planning state (the robot isn't moving) // if we didn't get a plan and we are in the planning state (the robot isn't moving)
else if (state_ == move_base::PLANNING) else if (state_ == move_base::PLANNING)
{ {
robot::log_info("No Plan...");
robot::Time attempt_end = last_valid_plan_ + robot::Duration(planner_patience_); robot::Time attempt_end = last_valid_plan_ + robot::Duration(planner_patience_);
// check if we've tried to make a plan for over our time limit or our maximum number of retries // check if we've tried to make a plan for over our time limit or our maximum number of retries