Added global planner make_plan service client

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>

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