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

update plan when docking

Browse Source
This commit is contained in:
HiepLM
2026-03-22 17:19:37 +07:00
parent d0ad2d0e21
commit f7fa96ff8b
10 changed files with 134 additions and 86 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_local_planner.h
View File

@@ -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
*

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

@@ -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;

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

@@ -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_);

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

@@ -16,6 +16,70 @@
#endif
#include <robot_xmlrpcpp/XmlRpcValue.h>
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<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Scalar);
}
break;
case robot_xmlrpcpp::XmlRpcValue::TypeInt:
{
int i = v_copy;
node = YAML::Node(i);
const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Scalar);
}
break;
case robot_xmlrpcpp::XmlRpcValue::TypeDouble:
{
double d = v_copy;
node = YAML::Node(d);
const_cast<NodeHandle *>(this)->setParamInternal(key, node, YAML::NodeType::Scalar);
}
break;
case robot_xmlrpcpp::XmlRpcValue::TypeString:
{
std::string s = v_copy;
node = YAML::Node(s);
const_cast<NodeHandle *>(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<NodeHandle *>(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<NodeHandle *>(this)->setParamInternal(key, map_node, YAML::NodeType::Map);
}
break;
default:
// Unknown type, create empty node
const_cast<NodeHandle *>(this)->setParamInternal(key, YAML::Node(), YAML::NodeType::Null);
break;
}
robot_xmlrpcpp::XmlRpcValue v_copy = v;
YAML::Node node = xmlRpcToYaml(v_copy);
const_cast<NodeHandle *>(this)->setParamInternal(key, node, yamlNodeCategory(node));
}
catch (...)
{
// On error, create empty node
const_cast<NodeHandle *>(this)->setParamInternal(key, YAML::Node(), YAML::NodeType::Null);
}
}

2
src/Libraries/xmlrpcpp

Submodule src/Libraries/xmlrpcpp updated: 727233624e...40718158ae

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

@@ -141,6 +141,13 @@ namespace robot_nav_core
*/
virtual void getPlan(std::vector<robot_geometry_msgs::PoseStamped> &path) = 0;
/**
* @brief Gets the current global plan
* @param path The global plan
*/
virtual void getGlobalPlan(std::vector<robot_geometry_msgs::PoseStamped> &path) = 0;
/**
* @brief Constructs the local planner
* @param name The name to give this instance of the local planner

6
src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/local_planner.h
View File

@@ -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
*

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

@@ -167,6 +167,12 @@ namespace robot_nav_core_adapter
*/
virtual void getPlan(std::vector<robot_geometry_msgs::PoseStamped> &path) override;
/**
* @brief Gets the current global plan
* @param path The global plan
*/
virtual void getGlobalPlan(std::vector<robot_geometry_msgs::PoseStamped> &path) override;
/**
* @brief Create a new LocalPlannerAdapter
* @return A shared pointer to the new LocalPlannerAdapter

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

@@ -407,6 +407,17 @@ namespace robot_nav_core_adapter
path = robot_nav_2d_utils::pathToPath(path2d).poses;
}
void LocalPlannerAdapter::getGlobalPlan(std::vector<robot_geometry_msgs::PoseStamped> &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 ||

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

@@ -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);