diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_local_planner.h b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_local_planner.h index 370e223..f28d849 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_local_planner.h +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_local_planner.h @@ -52,6 +52,12 @@ namespace pnkx_local_planner */ void getPlan(robot_nav_2d_msgs::Path2D &path) override; + /** + * @brief robot_nav_core2 getGlobalPlan - Gets the current global plan + * @param path The global plan + */ + void getGlobalPlan(robot_nav_2d_msgs::Path2D &path) override; + /** * @brief robot_nav_core2 computeVelocityCommands - calculates the best command given the current pose and 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 885aa1e..4a8551a 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 @@ -254,7 +254,8 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::reset() parent_.printParams(); std::string algorithm_nav_name; planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA")); - // parent_.setParam(algorithm_nav_name, original_papams_); + + parent_.setParam(algorithm_nav_name, original_papams_); robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name); nh_algorithm.setParam("allow_rotate", false); @@ -326,8 +327,8 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg nh_algorithm.setParam("lookahead_time", dkpl_.front()->lookahead_time_); nh_algorithm.setParam("angle_threshold", dkpl_.front()->angle_threshold_); - planner_nh_.setParam("xy_goal_tolerance", dkpl_.front()->xy_goal_tolerance_); - planner_nh_.setParam("yaw_goal_tolerance", dkpl_.front()->yaw_goal_tolerance_); + parent_.setParam("xy_goal_tolerance", dkpl_.front()->xy_goal_tolerance_); + parent_.setParam("yaw_goal_tolerance", dkpl_.front()->yaw_goal_tolerance_); if (dkpl_.front()->docking_nav_ && !dkpl_.front()->docking_planner_) { @@ -560,7 +561,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_ { if(!dkpl_.empty()) { - if(dkpl_.front()) delete(dkpl_.front()); + // if(dkpl_.front()) delete(dkpl_.front()); dkpl_.erase(dkpl_.begin()); } start_docking_ = false; 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 753985c..aa614ab 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 @@ -235,6 +235,15 @@ void pnkx_local_planner::PNKXLocalPlanner::getPlan(robot_nav_2d_msgs::Path2D &pa } +void pnkx_local_planner::PNKXLocalPlanner::getGlobalPlan(robot_nav_2d_msgs::Path2D &path) +{ + if (global_plan_.poses.empty()) + { + return; + } + path = global_plan_; +} + void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) { this->getParams(planner_nh_); diff --git a/src/Libraries/robot_cpp/src/node_handle.cpp b/src/Libraries/robot_cpp/src/node_handle.cpp index 2b70a79..3345288 100644 --- a/src/Libraries/robot_cpp/src/node_handle.cpp +++ b/src/Libraries/robot_cpp/src/node_handle.cpp @@ -16,6 +16,70 @@ #endif #include +namespace +{ + + static YAML::Node xmlRpcToYaml(robot_xmlrpcpp::XmlRpcValue v) + { + using robot_xmlrpcpp::XmlRpcValue; + switch (v.getType()) + { + case XmlRpcValue::TypeBoolean: + { + bool b = v; + return YAML::Node(b); + } + case XmlRpcValue::TypeInt: + { + int i = v; + return YAML::Node(i); + } + case XmlRpcValue::TypeDouble: + { + double d = v; + return YAML::Node(d); + } + case XmlRpcValue::TypeString: + { + std::string s = v; + return YAML::Node(s); + } + case XmlRpcValue::TypeArray: + { + YAML::Node seq(YAML::NodeType::Sequence); + for (int i = 0; i < v.size(); ++i) + seq.push_back(xmlRpcToYaml(v[i])); + return seq; + } + case XmlRpcValue::TypeStruct: + { + YAML::Node map(YAML::NodeType::Map); + const XmlRpcValue::ValueStruct *members = v.getStructMembers(); + if (members) + { + for (const auto &kv : *members) + map[kv.first] = xmlRpcToYaml(kv.second); + } + return map; + } + default: + return YAML::Node(); + } + } + + static YAML::NodeType::value yamlNodeCategory(const YAML::Node &n) + { + if (n.IsMap()) + return YAML::NodeType::Map; + if (n.IsSequence()) + return YAML::NodeType::Sequence; + if (n.IsScalar()) + return YAML::NodeType::Scalar; + return YAML::NodeType::Null; + } + +} // namespace + namespace robot { @@ -1147,92 +1211,14 @@ namespace robot void NodeHandle::setParam(const std::string &key, const robot_xmlrpcpp::XmlRpcValue &v) const { - // Convert XmlRpcValue to YAML::Node - // Create non-const copy to use conversion operators - robot_xmlrpcpp::XmlRpcValue v_copy = v; - YAML::Node node; try { - switch (v.getType()) - { - case robot_xmlrpcpp::XmlRpcValue::TypeBoolean: - { - bool b = v_copy; - node = YAML::Node(b); - const_cast(this)->setParamInternal(key, node, YAML::NodeType::Scalar); - } - break; - case robot_xmlrpcpp::XmlRpcValue::TypeInt: - { - int i = v_copy; - node = YAML::Node(i); - const_cast(this)->setParamInternal(key, node, YAML::NodeType::Scalar); - } - break; - case robot_xmlrpcpp::XmlRpcValue::TypeDouble: - { - double d = v_copy; - node = YAML::Node(d); - const_cast(this)->setParamInternal(key, node, YAML::NodeType::Scalar); - } - break; - case robot_xmlrpcpp::XmlRpcValue::TypeString: - { - std::string s = v_copy; - node = YAML::Node(s); - const_cast(this)->setParamInternal(key, node, YAML::NodeType::Scalar); - } - break; - case robot_xmlrpcpp::XmlRpcValue::TypeArray: - { - YAML::Node seq(YAML::NodeType::Sequence); - for (int i = 0; i < v_copy.size(); ++i) - { - YAML::Node item; - robot_xmlrpcpp::XmlRpcValue item_v = v_copy[i]; - if (item_v.getType() == robot_xmlrpcpp::XmlRpcValue::TypeBoolean) - { - bool b = item_v; - item = YAML::Node(b); - } - else if (item_v.getType() == robot_xmlrpcpp::XmlRpcValue::TypeInt) - { - int i_val = item_v; - item = YAML::Node(i_val); - } - else if (item_v.getType() == robot_xmlrpcpp::XmlRpcValue::TypeDouble) - { - double d = item_v; - item = YAML::Node(d); - } - else if (item_v.getType() == robot_xmlrpcpp::XmlRpcValue::TypeString) - { - std::string s = item_v; - item = YAML::Node(s); - } - seq.push_back(item); - } - const_cast(this)->setParamInternal(key, seq, YAML::NodeType::Sequence); - } - break; - case robot_xmlrpcpp::XmlRpcValue::TypeStruct: - { - YAML::Node map_node(YAML::NodeType::Map); - // XmlRpcValue::TypeStruct doesn't have begin/end, need to use different approach - // We'll need to iterate through the struct differently - // For now, create empty map - const_cast(this)->setParamInternal(key, map_node, YAML::NodeType::Map); - } - break; - default: - // Unknown type, create empty node - const_cast(this)->setParamInternal(key, YAML::Node(), YAML::NodeType::Null); - break; - } + robot_xmlrpcpp::XmlRpcValue v_copy = v; + YAML::Node node = xmlRpcToYaml(v_copy); + const_cast(this)->setParamInternal(key, node, yamlNodeCategory(node)); } catch (...) { - // On error, create empty node const_cast(this)->setParamInternal(key, YAML::Node(), YAML::NodeType::Null); } } diff --git a/src/Libraries/xmlrpcpp b/src/Libraries/xmlrpcpp index 7272336..4071815 160000 --- a/src/Libraries/xmlrpcpp +++ b/src/Libraries/xmlrpcpp @@ -1 +1 @@ -Subproject commit 727233624ef6cd584e1265382297e6181bcf4d64 +Subproject commit 40718158aec71537bdc06d0d4fcab249f43edb3a diff --git a/src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_local_planner.h b/src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_local_planner.h index 09dfc71..6225cdb 100755 --- a/src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_local_planner.h +++ b/src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_local_planner.h @@ -141,6 +141,13 @@ namespace robot_nav_core */ virtual void getPlan(std::vector &path) = 0; + + /** + * @brief Gets the current global plan + * @param path The global plan + */ + virtual void getGlobalPlan(std::vector &path) = 0; + /** * @brief Constructs the local planner * @param name The name to give this instance of the local planner diff --git a/src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/local_planner.h b/src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/local_planner.h index 95527ae..55802a8 100755 --- a/src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/local_planner.h +++ b/src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/local_planner.h @@ -99,6 +99,12 @@ namespace robot_nav_core2 */ virtual void getPlan(robot_nav_2d_msgs::Path2D &path) = 0; + /** + * @brief Gets the current global plan + * @param path The global plan + */ + virtual void getGlobalPlan(robot_nav_2d_msgs::Path2D &path) = 0; + /** * @brief Compute the best command given the current pose, velocity and goal * diff --git a/src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/local_planner_adapter.h b/src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/local_planner_adapter.h index 8bd4e12..80bc1f8 100755 --- a/src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/local_planner_adapter.h +++ b/src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/local_planner_adapter.h @@ -167,6 +167,12 @@ namespace robot_nav_core_adapter */ virtual void getPlan(std::vector &path) override; + /** + * @brief Gets the current global plan + * @param path The global plan + */ + virtual void getGlobalPlan(std::vector &path) override; + /** * @brief Create a new LocalPlannerAdapter * @return A shared pointer to the new LocalPlannerAdapter diff --git a/src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp b/src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp index e3b82e9..7bbc5de 100755 --- a/src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp +++ b/src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp @@ -407,6 +407,17 @@ namespace robot_nav_core_adapter path = robot_nav_2d_utils::pathToPath(path2d).poses; } + void LocalPlannerAdapter::getGlobalPlan(std::vector &path) + { + if (!planner_) + { + return; + } + robot_nav_2d_msgs::Path2D path2d; + planner_->getGlobalPlan(path2d); + path = robot_nav_2d_utils::pathToPath(path2d).poses; + } + bool LocalPlannerAdapter::hasGoalChanged(const robot_nav_2d_msgs::Pose2DStamped &new_goal) { if (last_goal_.header.frame_id != new_goal.header.frame_id || diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index 4330fd6..19b58c9 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -3214,6 +3214,22 @@ robot::move_base_core::NavFeedback *move_base::MoveBase::getFeedback() robot::move_base_core::PlannerDataOutput move_base::MoveBase::getGlobalData() { + if (tc_) + { + robot_nav_msgs::Path path; + tc_->getGlobalPlan(path.poses); + if (!path.poses.empty()) + { + path.header.stamp = path.poses[0].header.stamp; + path.header.frame_id = path.poses[0].header.frame_id; + } + else + { + path.header.stamp = robot::Time::now(); + path.header.frame_id = planner_costmap_robot_->getGlobalFrameID(); + } + global_data_.plan = robot_nav_2d_utils::pathToPath(path); + } ConvertData convert_data(planner_costmap_robot_, planner_costmap_robot_->getGlobalFrameID(), true); convert_data.updateCostmap(global_data_.costmap, global_data_.costmap_update, global_data_.is_costmap_updated); convert_data.updateFootprint(global_data_.footprint);