From 4617ce85b6906e44f9d36d689c3763ad1a8ac279 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Wed, 4 Mar 2026 09:43:39 +0000 Subject: [PATCH] update --- config/{mprim => }/maker_sources.yaml | 0 examples/NavigationExample/Program.cs | 14 ++- examples/NavigationExample/libnav_c_api.so | Bin 300144 -> 300144 bytes .../pnkx_local_planner/CMakeLists.txt | 1 + .../pnkx_docking_local_planner.h | 9 +- .../src/pnkx_docking_local_planner.cpp | 108 ++++++++++-------- .../src/pnkx_local_planner.cpp | 4 +- .../robot_cpp/include/robot/node_handle.h | 8 ++ src/Libraries/robot_cpp/include/robot/robot.h | 12 +- src/Libraries/robot_cpp/src/node_handle.cpp | 81 +++++++++++++ src/Libraries/robot_time | 2 +- .../Packages/move_base/src/move_base.cpp | 4 +- 12 files changed, 177 insertions(+), 66 deletions(-) rename config/{mprim => }/maker_sources.yaml (100%) diff --git a/config/mprim/maker_sources.yaml b/config/maker_sources.yaml similarity index 100% rename from config/mprim/maker_sources.yaml rename to config/maker_sources.yaml diff --git a/examples/NavigationExample/Program.cs b/examples/NavigationExample/Program.cs index ae04c07..e088a7a 100644 --- a/examples/NavigationExample/Program.cs +++ b/examples/NavigationExample/Program.cs @@ -404,17 +404,19 @@ namespace NavigationExample goal.pose.orientation.z = 0.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_angular(navHandle, 0.0, 0.0, 0.2); // NavigationAPI.navigation_move_straight_to(navHandle, 1.0); - // while (true) - // { - // System.Threading.Thread.Sleep(100); + while (true) + { + System.Threading.Thread.Sleep(100); // NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback(); // if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback)) // { @@ -474,7 +476,7 @@ namespace NavigationExample // Console.WriteLine("Local Costmap is not updated"); // } // } - // } + } // Cleanup (destroy nav first, then tf3 buffer) NavigationAPI.navigation_destroy(navHandle); diff --git a/examples/NavigationExample/libnav_c_api.so b/examples/NavigationExample/libnav_c_api.so index fc78ee20b66d82a3a7862d280f54c7e69c8bd217..80acdbdaec9cc1c60e74ed8db3f0d36f4191f760 100755 GIT binary patch delta 71 zcmeycLg>Q^p$#{fMC4QX%J;vlD!8Y5V_Jo-=wrU#uMN#_nA+bkF#<8u_BTw-?Tnnp bnhJ)7dM0`XlO364+GnsZZ=b=!a-$yrUK|^@ delta 71 zcmeycLg>Q^p$#{fMAQxoipi(PIVH>qU^*McWszvXsL}j}sr?NTBM>uff5XJw&d6z` asbFZRXQF2?*^x=6eFh8j_8BZJH~ImWGZ(A? diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/CMakeLists.txt b/src/Algorithms/Packages/local_planners/pnkx_local_planner/CMakeLists.txt index ae810da..9255f4c 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/CMakeLists.txt +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/CMakeLists.txt @@ -101,6 +101,7 @@ add_library(${PROJECT_NAME} SHARED src/pnkx_local_planner.cpp src/pnkx_go_straight_local_planner.cpp src/pnkx_rotate_local_planner.cpp + src/pnkx_docking_local_planner.cpp ) if(BUILDING_WITH_CATKIN) diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_docking_local_planner.h b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_docking_local_planner.h index e9b0f03..36b3f23 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_docking_local_planner.h +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_docking_local_planner.h @@ -1,8 +1,9 @@ #ifndef _PNKX_DOCKING_LOCAL_PLANNER_H_INCLUDED_ #define _PNKX_DOCKING_LOCAL_PLANNER_H_INCLUDED_ -#include +#include #include +#include namespace pnkx_local_planner { @@ -137,7 +138,7 @@ namespace pnkx_local_planner std::string docking_planner_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_; robot_geometry_msgs::Vector3 linear_; @@ -158,7 +159,7 @@ namespace pnkx_local_planner TFListenerPtr tf_; robot_costmap_2d::Costmap2DROBOT *costmap_robot_; score_algorithm::TrajectoryGenerator::Ptr traj_generator_; - std::function docking_planner_loader_; + std::function docking_planner_loader_; std::function docking_nav_loader_; robot::WallTimer detected_timeout_wt_, delayed_wt_; @@ -169,7 +170,7 @@ namespace pnkx_local_planner bool start_docking_; double original_xy_goal_tolerance_, original_yaw_goal_tolerance_; - XmlRpc::XmlRpcValue original_papams_; + robot_xmlrpcpp::XmlRpcValue original_papams_; std::vector dkpl_; bool dockingHanlde(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity); diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp index d030d04..07ef9b6 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp @@ -56,19 +56,18 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle & if (!costmap_robot_->isCurrent()) throw robot_nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow."); - nh_ = robot::NodeHandle("~"); parent_ = parent; 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_); 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()); 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( path_file_so, traj_generator_name, boost::dll::load_mode::append_decorations); 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()); 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); 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); } - std::string algorithm_nav_name; - planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA")); + std::string algorithm_nav_name; + 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()); 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"; - algorithm_loader_ = boost::dll::import_alias( + robot::PluginLoaderHelper loader; + std::string path_file_so = loader.findLibraryPath(algorithm_nav_name); + nav_algorithm_loader_ = boost::dll::import_alias( path_file_so, algorithm_nav_name, boost::dll::load_mode::append_decorations); - nav_algorithm_ = algorithm_loader_(); + nav_algorithm_ = nav_algorithm_loader_(); if (!nav_algorithm_) { robot::log_error_at(__FILE__, __LINE__, "Failed to load algorithm %s", algorithm_nav_name.c_str()); 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) { @@ -110,19 +110,20 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle & } 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 { - 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"; - algorithm_loader_ = boost::dll::import_alias( + robot::PluginLoaderHelper loader; + std::string path_file_so = loader.findLibraryPath(algorithm_rotate_name); + rotate_algorithm_loader_ = boost::dll::import_alias( path_file_so, algorithm_rotate_name, boost::dll::load_mode::append_decorations); - rotate_algorithm_ = algorithm_loader_(); + rotate_algorithm_ = rotate_algorithm_loader_(); if (!rotate_algorithm_) { robot::log_error_at(__FILE__, __LINE__, "Failed to create rotate algorithm \"%s\": returned null pointer", algorithm_rotate_name.c_str()); 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()); } catch (const std::exception &ex) @@ -133,10 +134,12 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle & std::string goal_checker_name; 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 { - 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"; - boost::dll::import_alias( + robot::PluginLoaderHelper loader; + std::string path_file_so = loader.findLibraryPath(goal_checker_name); + goal_checker_loader_ = boost::dll::import_alias( path_file_so, goal_checker_name, boost::dll::load_mode::append_decorations); goal_checker_ = goal_checker_loader_(); 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()); 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()); } 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()); exit(1); } + this->initializeOthers(); this->getMaker(); 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() { std::string maker_name, maker_sources; - nh_.param("maker_name", maker_name, std::string("")); - nh_.param("maker_sources", maker_sources, std::string("")); + parent_.param("maker_name", maker_name, 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::string source; @@ -171,24 +177,23 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::getMaker() { if (maker_name == source) { - robot::NodeHandle source_node(nh_, source); + robot::NodeHandle source_node(parent_, source); if (source_node.hasParam("plugins")) { - XmlRpc::XmlRpcValue plugins; + robot_xmlrpcpp::XmlRpcValue 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) { - if (plugins[i].getType() == XmlRpc::XmlRpcValue::TypeStruct) + if (plugins[i].getType() == robot_xmlrpcpp::XmlRpcValue::TypeStruct) { std::stringstream name; name << source << "/" << static_cast(plugins[i]["name"]); std::string docking_planner_name = static_cast(plugins[i]["docking_planner"]); std::string docking_nav_name = static_cast(plugins[i]["docking_nav"]); - // std::shared_ptr dkpl = std::make_shared(name.str()); DockingPlanner* dkpl = new DockingPlanner(name.str()); dkpl->docking_planner_name_ = docking_planner_name; @@ -215,7 +220,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initializeOthers() { std::string algorithm_nav_name; 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()); original_xy_goal_tolerance_ = xy_goal_tolerance_; original_yaw_goal_tolerance_ = yaw_goal_tolerance_; @@ -247,8 +252,8 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::reset() std::string algorithm_nav_name; 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); - nh_.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); } @@ -279,7 +284,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg 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"); } 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; 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()); 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; 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("min_lookahead_dist", dkpl_.front()->min_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("angle_threshold", dkpl_.front()->angle_threshold_); - nh_.setParam("xy_goal_tolerance", dkpl_.front()->xy_goal_tolerance_); - nh_.setParam("yaw_goal_tolerance", dkpl_.front()->yaw_goal_tolerance_); + planner_nh_.setParam("xy_goal_tolerance", dkpl_.front()->xy_goal_tolerance_); + planner_nh_.setParam("yaw_goal_tolerance", dkpl_.front()->yaw_goal_tolerance_); 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; } } - 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"); 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 { - 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"); 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; 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::Path2D plan = transformed_plan_; + robot_nav_2d_msgs::Path2D plan = transformed_global_plan_; if (start_docking_) { 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_); std::string algorithm_nav_name; 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); - nh_.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); } return ret_nav_ && ret_angle_ && dock_ok; @@ -479,7 +484,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_ { 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; } else @@ -589,7 +594,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(rob 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"; - docking_planner_loader_ = boost::dll::import_alias( + docking_planner_loader_ = boost::dll::import_alias( path_file_so, docking_planner_name_, boost::dll::load_mode::append_decorations); docking_planner_ = docking_planner_loader_(); 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()); 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_); 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("angle_threshold", angle_threshold_, 0.4); - detected_timeout_wt_ = - nh_priv_.createWallTimer(::robot::WallDuration(time_out + delay_time), &pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::detectedTimeOutCb, this); + robot::log_info("delay: %f", delay_time); + 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(); robot::log_warning_at(__FILE__, __LINE__, "%s %f time_out start %f", name_.c_str(), delay_time, robot::Time::now().toSec()); - delayed_wt_ = - nh_priv_.createWallTimer(::robot::WallDuration(delay_time), &pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::delayCb, this); + delayed_wt_ = ::robot::WallTimer( + ::robot::WallDuration(delay_time), &pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::delayCb, this, false, false); delayed_wt_.start(); robot::log_warning_at(__FILE__, __LINE__, "%s %f delay start %f", name_.c_str(), delay_time, robot::Time::now().toSec()); diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp index 04358f5..5989001 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp @@ -120,7 +120,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent, 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()); - // exit(1); + exit(1); } std::string algorithm_rotate_name; @@ -143,7 +143,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent, 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()); - // exit(1); + exit(1); } std::string goal_checker_name; diff --git a/src/Libraries/robot_cpp/include/robot/node_handle.h b/src/Libraries/robot_cpp/include/robot/node_handle.h index af3155b..9931e3f 100644 --- a/src/Libraries/robot_cpp/include/robot/node_handle.h +++ b/src/Libraries/robot_cpp/include/robot/node_handle.h @@ -283,6 +283,14 @@ namespace robot */ 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). * diff --git a/src/Libraries/robot_cpp/include/robot/robot.h b/src/Libraries/robot_cpp/include/robot/robot.h index ae57aa0..bf829ff 100644 --- a/src/Libraries/robot_cpp/include/robot/robot.h +++ b/src/Libraries/robot_cpp/include/robot/robot.h @@ -2,11 +2,17 @@ #define ROBOT_ROBOT_H #include -#include -#include -#include #include #include #include + +#include +#include +#include +#include +#include +#include +#include +#include #endif \ No newline at end of file diff --git a/src/Libraries/robot_cpp/src/node_handle.cpp b/src/Libraries/robot_cpp/src/node_handle.cpp index 31657aa..7d38393 100644 --- a/src/Libraries/robot_cpp/src/node_handle.cpp +++ b/src/Libraries/robot_cpp/src/node_handle.cpp @@ -1727,6 +1727,87 @@ namespace robot 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 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(); + out = robot_xmlrpcpp::XmlRpcValue(i); + return true; + } + catch (...) {} + try + { + double d = y.as(); + 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(y.size())); + for (size_t i = 0; i < y.size(); ++i) + { + robot_xmlrpcpp::XmlRpcValue item; + if (yamlToXmlRpc(y[i], item)) + out[static_cast(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(); + 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 T NodeHandle::param(const std::string ¶m_name) const { diff --git a/src/Libraries/robot_time b/src/Libraries/robot_time index 750dc94..8201dcc 160000 --- a/src/Libraries/robot_time +++ b/src/Libraries/robot_time @@ -1 +1 @@ -Subproject commit 750dc94c61be406937ad49e0f215a4fdce538fc5 +Subproject commit 8201dcc6bdfea1eca785c866737228fb73436320 diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index 57aed24..1b4f501 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -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++) { // intensities_str << laser_scan.intensities[i] << " "; // } - // robot::log_info("intensities: %s", intensities_str.str().c_str()); - // robot::log_info("--------------------------------"); + // robot::log_error("intensities: %s", intensities_str.str().c_str()); updateLocalCostmap(laser_scan, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name); updateGlobalCostmap(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) else if (state_ == move_base::PLANNING) { - robot::log_info("No Plan..."); 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