Added function to get the path and the current speed of the robot

include/iri_nav_module/nav_module.h

@@ -83,6 +83,11 @@ class CNavModule : public CModule<ModuleCfg>
       *
       */
     void odom_callback(const nav_msgs::Odometry::ConstPtr& msg);
+    /**
+      * \brief
+      *
+      */
+    geometry_msgs::Twist twist_msg;
 
     /**
       * \brief
@@ -279,6 +284,18 @@ class CNavModule : public CModule<ModuleCfg>
       *         enabled (true) or not (false).
       */
     bool costamp_is_auto_clear_enabled(void);
+    /**
+     * \brief Function to get the current speed of the robot.
+     *
+     * \return The robot speed.
+     */
+    geometry_msgs::Twist get_current_speed(void);
+    /**
+     * \brief Function to get the current path.
+     *
+     * \return The current path.
+     */
+    nav_msgs::Path get_current_path(void);
     /**
      * \brief Function to get the current pose of the robot.
      *
@@ -468,6 +485,8 @@ class CNavModule : public CModule<ModuleCfg>
     // save the current position of the robot
     this->current_pose.pose=msg->pose.pose;
     this->current_pose.header=msg->header;
+
+    this->twist_msg = msg->twist.twist;
     this->unlock();
   }
 
@@ -617,6 +636,18 @@ class CNavModule : public CModule<ModuleCfg>
     return this->enable_auto_clear;
   }
 
+  template <class ModuleCfg>
+  geometry_msgs::Twist CNavModule<ModuleCfg>::get_current_speed(void)
+  {
+    return this->twist_msg;
+  }
+
+  template <class ModuleCfg>
+  nav_msgs::Path CNavModule<ModuleCfg>::get_current_path(void)
+  {
+    return this->path_msg;
+  }
+
   template <class ModuleCfg>
   geometry_msgs::PoseStamped CNavModule<ModuleCfg>::get_current_pose(void)
   {

include/iri_nav_module/nav_module_bt.h

@@ -106,15 +106,14 @@ class CNavModuleBT
       *
       */
     BT::NodeStatus costmaps_disable_auto_clear(void);
-
     /**
-      * \brief Synchronized getCurrentPose add_sbpl nav function.
+      * \brief Synchronized getCurrentSpeed add_sbpl nav function.
       *
-      * This function calls getCurrentPose of nav_module.
+      * This function calls getCurrentSpeed of nav_module.
       *
       * It has the following output ports:
       *
-      *   pose (geometry_msgs::Pose): current pose.
+      *   twist (geometry_msgs::Twist): current speed.
       *
       * \param self Self node with the required ports:
       *
@@ -123,6 +122,41 @@ class CNavModuleBT
       * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
       *
       */
+      BT::NodeStatus get_current_speed(BT::TreeNode& self);
+      /**
+        * \brief Synchronized getCurrentPath add_sbpl nav function.
+        *
+        * This function calls getCurrentPath of nav_module.
+        *
+        * It has the following output ports:
+        *
+        *   path (nav_msgs::Path): current path.
+        *
+        * \param self Self node with the required ports:
+        *
+        * \return a BT:NodeStatus indicating whether the request has been
+        * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+        * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
+        *
+        */
+      BT::NodeStatus get_current_path(BT::TreeNode& self);
+      /**
+        * \brief Synchronized getCurrentPose add_sbpl nav function.
+        *
+        * This function calls getCurrentPose of nav_module.
+        *
+        * It has the following output ports:
+        *
+        *   pose (geometry_msgs::Pose): current pose.
+        *
+        * \param self Self node with the required ports:
+        *
+        * \return a BT:NodeStatus indicating whether the request has been
+        * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+        * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
+        *
+        */
+
     BT::NodeStatus get_current_pose(BT::TreeNode& self);
 
     /**
@@ -255,6 +289,8 @@ class CNavModuleBT
     //Definition of ports
     BT::PortsList set_goal_frame_ports = {BT::InputPort<std::string>("frame_id")};
     BT::PortsList auto_clear_ports = {BT::InputPort<double>("rate_hz")};
+    BT::PortsList current_speed_ports = {BT::OutputPort<geometry_msgs::Twist>("speed")};
+    BT::PortsList current_path_ports = {BT::OutputPort<nav_msgs::Path>("path")};
     BT::PortsList current_pose_ports = {BT::OutputPort<geometry_msgs::PoseStamped>("pose")};
     BT::PortsList goal_distance_ports = {BT::OutputPort<double>("distance")};
     BT::PortsList path_length_ports = {BT::OutputPort<double>("length")};
@@ -282,6 +318,8 @@ class CNavModuleBT
     factory.registerSimpleAction(name+"_costmaps_clear", std::bind(&CNavModuleBT::costmaps_clear, this));
     factory.registerSimpleAction(name+"_costmaps_enable_auto_clear", std::bind(&CNavModuleBT::costmaps_enable_auto_clear, this, std::placeholders::_1), auto_clear_ports);
     factory.registerSimpleAction(name+"_costmaps_disable_auto_clear", std::bind(&CNavModuleBT::costmaps_disable_auto_clear, this));
+    factory.registerSimpleAction(name+"_get_current_pose", std::bind(&CNavModuleBT::get_current_speed, this, std::placeholders::_1), current_speed_ports);
+    factory.registerSimpleAction(name+"_get_current_pose", std::bind(&CNavModuleBT::get_current_path, this, std::placeholders::_1), current_path_ports);
     factory.registerSimpleAction(name+"_get_current_pose", std::bind(&CNavModuleBT::get_current_pose, this, std::placeholders::_1), current_pose_ports);
     factory.registerSimpleAction(name+"_get_goal_distance", std::bind(&CNavModuleBT::get_goal_distance, this, std::placeholders::_1), goal_distance_ports);
     factory.registerSimpleAction(name+"_get_path_length", std::bind(&CNavModuleBT::get_path_length, this, std::placeholders::_1), path_length_ports);
@@ -358,7 +396,26 @@ class CNavModuleBT
     this->nav_module.costmaps_disable_auto_clear();
     return BT::NodeStatus::SUCCESS;
   }
+  template <class ModuleCfg>
+  BT::NodeStatus CNavModuleBT<ModuleCfg>::get_current_speed(BT::TreeNode& self)
+  {
+    ROS_DEBUG("CNavModuleBT::get_current_speed-> get_current_speed");
 
+    geometry_msgs::Twist speed_out = this->nav_module.get_current_speed();
+
+    self.setOutput("speed", speed_out);
+    return BT::NodeStatus::SUCCESS;
+  }
+  template <class ModuleCfg>
+  BT::NodeStatus CNavModuleBT<ModuleCfg>::get_current_path(BT::TreeNode& self)
+  {
+    ROS_DEBUG("CNavModuleBT::get_current_path-> get_current_path");
+
+    nav_msgs::Path path_out = this->nav_module.get_current_path();
+
+    self.setOutput("path", path_out);
+    return BT::NodeStatus::SUCCESS;
+  }
   template <class ModuleCfg>
   BT::NodeStatus CNavModuleBT<ModuleCfg>::get_current_pose(BT::TreeNode& self)
   {