From b0ba1521db8d6d9a0a52758e23997998da95526d Mon Sep 17 00:00:00 2001
From: Alopez <alopez@iri.upc.edu>
Date: Tue, 17 Jan 2023 17:39:48 +0100
Subject: [PATCH] Added global planner make_plan service client
---
include/iri_nav_module/gp_module.h | 76 +++++++++++++++++++++++++++++-
src/iri_nav_module/dyn_params.py | 13 +++++
2 files changed, 88 insertions(+), 1 deletion(-)
diff --git a/include/iri_nav_module/gp_module.h b/include/iri_nav_module/gp_module.h
index 51370bb..f2bdf78 100644
--- a/include/iri_nav_module/gp_module.h
+++ b/include/iri_nav_module/gp_module.h
@@ -4,6 +4,10 @@
// IRI ROS headers
#include <iri_ros_tools/module_service.h>
#include <iri_nav_module/nav_planner_module.h>
+#include <iri_ros_tools/module_dyn_reconf.h>
+
+// ROS headers
+#include <nav_msgs/GetPlan.h>
/**
* \brief
@@ -12,6 +16,9 @@
template <class ModuleCfg>
class CGPModule : public CNavPlannerModule<ModuleCfg>
{
+ private:
+ CModuleService<nav_msgs::GetPlan,ModuleCfg> make_plan_service; ///< Make plan service client object.
+
public:
/**
* \brief Constructor
@@ -19,6 +26,31 @@ class CGPModule : public CNavPlannerModule<ModuleCfg>
*/
CGPModule(const std::string &name,const std::string &name_space=std::string(""));
+ /**
+ * \brief Function to call make plan service from Global planner
+ *
+ * \param start_x Start point X coordenate.
+ *
+ * \param start_y Start point Y coordenate.
+ *
+ * \param start_yaw Start point yaw coordenate.
+ *
+ * \param end_x End point X coordenate.
+ *
+ * \param end_y End point Y coordenate.
+ *
+ * \param end_yaw End point yaw coordenate.
+ *
+ * \param frame_id Points' frame id.
+ *
+ * \param tolerance Planner tolerance.
+ *
+ * \param plan Output path.
+ *
+ * \return The status of the operation.
+ */
+ bool gp_make_plan(double start_x,double start_y,double start_yaw,double end_x,double end_y,double end_yaw,std::string &frame_id,double tolerance,nav_msgs::Path &plan);
+
/**
* \brief Function to set the parameter
*
@@ -160,11 +192,52 @@ class CGPModule : public CNavPlannerModule<ModuleCfg>
};
template<class ModuleCfg>
- CGPModule<ModuleCfg>::CGPModule(const std::string &name,const std::string &name_space) : CNavPlannerModule<ModuleCfg>(name,name_space)
+ CGPModule<ModuleCfg>::CGPModule(const std::string &name,const std::string &name_space) : CNavPlannerModule<ModuleCfg>(name,name_space),
+ make_plan_service("make_plan",name_space+"/"+name)
{
}
+ template <class ModuleCfg>
+ bool CGPModule<ModuleCfg>::gp_make_plan(double start_x,double start_y,double start_yaw,double end_x,double end_y,double end_yaw,std::string &frame_id,double tolerance,nav_msgs::Path &plan)
+ {
+ nav_msgs::GetPlan get_plan_req;
+ tf2::Quaternion quat;
+
+ // start position
+ get_plan_req.request.start.header.frame_id=frame_id;
+ get_plan_req.request.start.header.stamp=ros::Time::now();
+ get_plan_req.request.start.pose.position.x=start_x;
+ get_plan_req.request.start.pose.position.y=start_y;
+ get_plan_req.request.start.pose.position.z=0.0;
+ quat.setRPY(0.0,0.0,start_yaw);
+ get_plan_req.request.start.pose.orientation=tf2::toMsg(quat);
+ // end position
+ get_plan_req.request.goal.header.frame_id=frame_id;
+ get_plan_req.request.goal.header.stamp=ros::Time::now();
+ get_plan_req.request.goal.pose.position.x=end_x;
+ get_plan_req.request.goal.pose.position.y=end_y;
+ get_plan_req.request.goal.pose.position.z=0.0;
+ quat.setRPY(0.0,0.0,end_yaw);
+ get_plan_req.request.goal.pose.orientation=tf2::toMsg(quat);
+ get_plan_req.request.tolerance=tolerance;
+ switch(this->make_plan_service.call(get_plan_req))
+ {
+ case ACT_SRV_SUCCESS: ROS_INFO("CNavModule: New plan generated successfuilly");
+ plan=get_plan_req.response.plan;
+ return true;
+ break;
+ case ACT_SRV_PENDING: ROS_WARN("CNavModule: Still waiting for new plan");
+ return false;
+ break;
+ case ACT_SRV_FAIL: ROS_ERROR("CNavModule: Impossible to get new plan");
+ return false;
+ break;
+ default: return false;
+ break;
+ }
+ }
+
/* parameter functions */
template <class ModuleCfg>
dyn_reconf_status_t CGPModule<ModuleCfg>::set_lethal_cost(int &value)
@@ -267,6 +340,7 @@ class CGPModule : public CNavPlannerModule<ModuleCfg>
template<class ModuleCfg>
void CGPModule<ModuleCfg>::dynamic_reconfigure(ModuleCfg &config, const std::string &name)
{
+ this->make_plan_service.dynamic_reconfigure(config,name+"_gp_make_plan");
}
template<class ModuleCfg>
diff --git a/src/iri_nav_module/dyn_params.py b/src/iri_nav_module/dyn_params.py
index ded8131..9e11851 100644
--- a/src/iri_nav_module/dyn_params.py
+++ b/src/iri_nav_module/dyn_params.py
@@ -48,3 +48,16 @@ def add_opendrive_gp_module_params(gen,name):
add_module_service_params(new_group,prefix+"_get_nodes")
return new_group
+
+def add_gp_module_params(gen,name):
+
+ if len(name) == 0:
+ prefix = "gp"
+ else:
+ prefix = name
+
+ new_group = gen.add_group(prefix)
+
+ add_module_service_params(new_group,prefix+"_make_plan")
+
+ return new_group
--
GitLab