Added getGoalDistance

cfg/TiagoNavClient.cfg

@@ -64,6 +64,7 @@ move_base.add("start_pose_nav",    bool_t,    0,                               "
 move_base.add("start_position_nav",bool_t,    0,                               "Start navigating towards a position",     False)
 move_base.add("start_orientation_nav",bool_t, 0,                               "Start navigating towards a orientation",  False)
 move_base.add("stop_nav",          bool_t,    0,                               "Stop the current navigation",             False)
+move_base.add("print_goal_distance",bool_t,   0,                               "Print distance to goal",                  False)
 
 poi.add("start_poi",               bool_t,    0,                               "Start the POI navigation",                False)
 poi.add("stop_poi",                bool_t,    0,                               "Stop the POI navigation",                 False)

include/tiago_nav_module/tiago_nav_bt_module.h

@@ -558,6 +558,24 @@ class CTiagoNavModuleBT
       */
     BT::NodeStatus get_tiago_nav_current_pose(BT::TreeNode& self);
 
+    /**
+      * \brief Synchronized getGoalDistance TIAGo nav function.
+      *
+      * This function calls getGoalDistance of tiago_nav_module.
+      *
+      * It has the following output ports:
+      *
+      *   distance (double): current distance to goal.
+      *
+      * \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_tiago_nav_goal_distance(BT::TreeNode& self);
+
     /**
       * \brief Synchronized stop TIAGo nav function.
       *

include/tiago_nav_module/tiago_nav_module.h

@@ -12,6 +12,7 @@
 
 // ROS headers
 #include <nav_msgs/Odometry.h>
+#include <nav_msgs/Path.h>
 #include <tf/transform_listener.h>
 #include <move_base_msgs/MoveBaseAction.h>
 #include <std_srvs/Empty.h>
@@ -204,6 +205,19 @@ class CTiagoNavModule : public CModule<tiago_nav_module::TiagoNavModuleConfig>
       *
       */
     void odom_callback(const nav_msgs::Odometry::ConstPtr& msg);
+
+    /**
+      * \brief
+      *
+      */
+    ros::Subscriber path_subscriber;
+    /**
+      * \brief
+      *
+      */
+    void path_callback(const nav_msgs::Path::ConstPtr& msg);
+
+    nav_msgs::Path *path_msg;
     /**
       * \brief
       *
@@ -653,6 +667,12 @@ class CTiagoNavModule : public CModule<tiago_nav_module::TiagoNavModuleConfig>
     bool set_parameter(const std::string &name_space,const std::string &name,double value);
 
     geometry_msgs::Pose getCurrentPose(void);
+    /**
+     * \brief Function to get the euclidian distance to the goal.
+     * 
+     * \return The Euclidian distance to the goal. -1.0 when not calculated.
+     */
+    double getGoalDistance(void);
 
     /**
       * \brief Destructor

launch/ivo_nav_bt_client.launch

@@ -8,6 +8,7 @@
   <arg name="move_poi_action" default="/poi_navigation_server/go_to_poi"/>
   <arg name="move_waypoint_action" default="/pal_waypoint/navigate"/>
   <arg name="odom_topic" default="/mobile_base_controller/odom"/>
+  <arg name="path_topic" default="/move_base/GlobalPlanner/plan"/>
   <arg name="clear_costmaps_service" default="/move_base/clear_costmaps"/>
   <arg name="change_map_service" default="/pal_map_manager/change_map"/>
   <arg name="set_op_mode_service" default="/pal_navigation_sm"/>
@@ -26,6 +27,7 @@
     <arg name="move_poi_action"        value="$(arg move_poi_action)"/>
     <arg name="move_waypoint_action"   value="$(arg move_waypoint_action)"/>
     <arg name="odom_topic"             value="$(arg odom_topic)"/>
+    <arg name="path_topic"             value="$(arg path_topic)"/>
     <arg name="clear_costmaps_service" value="$(arg clear_costmaps_service)"/>
     <arg name="change_map_service"     value="$(arg change_map_service)"/>
     <arg name="set_op_mode_service"    value="$(arg set_op_mode_service)"/>

launch/ivo_nav_bt_client_sim.launch

@@ -8,6 +8,7 @@
   <arg name="move_poi_action" default="/poi_navigation_server/go_to_poi"/>
   <arg name="move_waypoint_action" default="/pal_waypoint/navigate"/>
   <arg name="odom_topic" default="/mobile_base_controller/odom"/>
+  <arg name="path_topic" default="/move_base/GlobalPlanner/plan"/>
   <arg name="clear_costmaps_service" default="/move_base/clear_costmaps"/>
   <arg name="change_map_service" default="/pal_map_manager/change_map"/>
   <arg name="set_op_mode_service" default="/pal_navigation_sm"/>
@@ -31,6 +32,7 @@
     <arg name="move_poi_action"        value="$(arg move_poi_action)"/>
     <arg name="move_waypoint_action"   value="$(arg move_waypoint_action)"/>
     <arg name="odom_topic"             value="$(arg odom_topic)"/>
+    <arg name="path_topic"             value="$(arg path_topic)"/>
     <arg name="clear_costmaps_service" value="$(arg clear_costmaps_service)"/>
     <arg name="change_map_service"     value="$(arg change_map_service)"/>
     <arg name="set_op_mode_service"    value="$(arg set_op_mode_service)"/>

launch/ivo_nav_client.launch

@@ -8,6 +8,7 @@
   <arg name="move_poi_action" default="/poi_navigation_server/go_to_poi"/>
   <arg name="move_waypoint_action" default="/pal_waypoint/navigate"/>
   <arg name="odom_topic" default="/mobile_base_controller/odom"/>
+  <arg name="path_topic" default="/move_base/GlobalPlanner/plan"/>
   <arg name="clear_costmaps_service" default="/move_base/clear_costmaps"/>
   <arg name="change_map_service" default="/pal_map_manager/change_map"/>
   <arg name="set_op_mode_service" default="/pal_navigation_sm"/>
@@ -22,6 +23,7 @@
     <arg name="move_poi_action"        value="$(arg move_poi_action)"/>
     <arg name="move_waypoint_action"   value="$(arg move_waypoint_action)"/>
     <arg name="odom_topic"             value="$(arg odom_topic)"/>
+    <arg name="path_topic"             value="$(arg path_topic)"/>
     <arg name="clear_costmaps_service" value="$(arg clear_costmaps_service)"/>
     <arg name="change_map_service"     value="$(arg change_map_service)"/>
     <arg name="set_op_mode_service"    value="$(arg set_op_mode_service)"/>

launch/ivo_nav_client_sim.launch

@@ -8,6 +8,7 @@
   <arg name="move_poi_action" default="/poi_navigation_server/go_to_poi"/>
   <arg name="move_waypoint_action" default="/pal_waypoint/navigate"/>
   <arg name="odom_topic" default="/mobile_base_controller/odom"/>
+  <arg name="path_topic" default="/move_base/GlobalPlanner/plan"/>
   <arg name="clear_costmaps_service" default="/move_base/clear_costmaps"/>
   <arg name="change_map_service" default="/pal_map_manager/change_map"/>
   <arg name="set_op_mode_service" default="/pal_navigation_sm"/>
@@ -27,6 +28,7 @@
     <arg name="move_poi_action"        value="$(arg move_poi_action)"/>
     <arg name="move_waypoint_action"   value="$(arg move_waypoint_action)"/>
     <arg name="odom_topic"             value="$(arg odom_topic)"/>
+    <arg name="path_topic"             value="$(arg path_topic)"/>
     <arg name="clear_costmaps_service" value="$(arg clear_costmaps_service)"/>
     <arg name="change_map_service"     value="$(arg change_map_service)"/>
     <arg name="set_op_mode_service"    value="$(arg set_op_mode_service)"/>

launch/tiago_nav_bt_client.launch

@@ -8,6 +8,7 @@
   <arg name="move_poi_action" default="/poi_navigation_server/go_to_poi"/>
   <arg name="move_waypoint_action" default="/pal_waypoint/navigate"/>
   <arg name="odom_topic" default="/mobile_base_controller/odom"/>
+  <arg name="path_topic" default="/move_base/GlobalPlanner/plan"/>
   <arg name="clear_costmaps_service" default="/move_base/clear_costmaps"/>
   <arg name="change_map_service" default="/pal_map_manager/change_map"/>
   <arg name="set_op_mode_service" default="/pal_navigation_sm"/>
@@ -31,6 +32,8 @@
              to="$(arg move_waypoint_action)"/>
     <remap from="~/tiago_nav_module/odom"
              to="$(arg odom_topic)"/>
+    <remap from="~/tiago_nav_module/path"
+             to="$(arg path_topic)"/>
     <remap from="~/tiago_nav_module/clear_costmaps"
              to="$(arg clear_costmaps_service)"/>
     <remap from="~/tiago_nav_module/change_map"

launch/tiago_nav_bt_client_sim.launch

@@ -8,6 +8,7 @@
   <arg name="move_poi_action" default="/poi_navigation_server/go_to_poi"/>
   <arg name="move_waypoint_action" default="/pal_waypoint/navigate"/>
   <arg name="odom_topic" default="/mobile_base_controller/odom"/>
+  <arg name="path_topic" default="/move_base/GlobalPlanner/plan"/>
   <arg name="clear_costmaps_service" default="/move_base/clear_costmaps"/>
   <arg name="change_map_service" default="/pal_map_manager/change_map"/>
   <arg name="set_op_mode_service" default="/pal_navigation_sm"/>
@@ -31,6 +32,7 @@
     <arg name="move_poi_action"        value="$(arg move_poi_action)"/>
     <arg name="move_waypoint_action"   value="$(arg move_waypoint_action)"/>
     <arg name="odom_topic"             value="$(arg odom_topic)"/>
+    <arg name="path_topic"             value="$(arg path_topic)"/>
     <arg name="clear_costmaps_service" value="$(arg clear_costmaps_service)"/>
     <arg name="change_map_service"     value="$(arg change_map_service)"/>
     <arg name="set_op_mode_service"    value="$(arg set_op_mode_service)"/>

launch/tiago_nav_client.launch

@@ -8,6 +8,7 @@
   <arg name="move_poi_action" default="/poi_navigation_server/go_to_poi"/>
   <arg name="move_waypoint_action" default="/pal_waypoint/navigate"/>
   <arg name="odom_topic" default="/mobile_base_controller/odom"/>
+  <arg name="path_topic" default="/move_base/GlobalPlanner/plan"/>
   <arg name="clear_costmaps_service" default="/move_base/clear_costmaps"/>
   <arg name="change_map_service" default="/pal_map_manager/change_map"/>
   <arg name="set_op_mode_service" default="/pal_navigation_sm"/>
@@ -27,6 +28,8 @@
              to="$(arg move_waypoint_action)"/>
     <remap from="~/nav_module/odom"
              to="$(arg odom_topic)"/>
+    <remap from="~/nav_module/path"
+             to="$(arg path_topic)"/>
     <remap from="~/nav_module/clear_costmaps"
              to="$(arg clear_costmaps_service)"/>
     <remap from="~/nav_module/change_map"

launch/tiago_nav_client_sim.launch

@@ -8,6 +8,7 @@
   <arg name="move_poi_action" default="/poi_navigation_server/go_to_poi"/>
   <arg name="move_waypoint_action" default="/pal_waypoint/navigate"/>
   <arg name="odom_topic" default="/mobile_base_controller/odom"/>
+  <arg name="path_topic" default="/move_base/GlobalPlanner/plan"/>
   <arg name="clear_costmaps_service" default="/move_base/clear_costmaps"/>
   <arg name="change_map_service" default="/pal_map_manager/change_map"/>
   <arg name="set_op_mode_service" default="/pal_navigation_sm"/>
@@ -27,6 +28,7 @@
     <arg name="move_poi_action"        value="$(arg move_poi_action)"/>
     <arg name="move_waypoint_action"   value="$(arg move_waypoint_action)"/>
     <arg name="odom_topic"             value="$(arg odom_topic)"/>
+    <arg name="path_topic"             value="$(arg path_topic)"/>
     <arg name="clear_costmaps_service" value="$(arg clear_costmaps_service)"/>
     <arg name="change_map_service"     value="$(arg change_map_service)"/>
     <arg name="set_op_mode_service"    value="$(arg set_op_mode_service)"/>

src/tiago_nav_bt_module.cpp

@@ -33,6 +33,7 @@ void CTiagoNavModuleBT::init(IriBehaviorTreeFactory &factory)
   BT::PortsList set_string_parameter_ports = {BT::InputPort<std::string>("name_space"), BT::InputPort<std::string>("name"), BT::InputPort<std::string>("value")};
   BT::PortsList set_double_parameter_ports = {BT::InputPort<std::string>("name_space"), BT::InputPort<std::string>("name"), BT::InputPort<double>("value")};
   BT::PortsList current_pose_ports = {BT::OutputPort<geometry_msgs::Pose>("pose")};
+  BT::PortsList goal_distance_ports = {BT::OutputPort<double>("distance")};
 
 
   //Registration of conditions
@@ -78,6 +79,7 @@ void CTiagoNavModuleBT::init(IriBehaviorTreeFactory &factory)
   factory.registerSimpleAction("tiago_nav_set_string_parameter",  std::bind(&CTiagoNavModuleBT::tiago_nav_set_string_parameter, this, std::placeholders::_1), set_string_parameter_ports);
   factory.registerSimpleAction("tiago_nav_set_double_parameter",  std::bind(&CTiagoNavModuleBT::tiago_nav_set_double_parameter, this, std::placeholders::_1), set_double_parameter_ports);
   factory.registerSimpleAction("get_tiago_nav_current_pose",  std::bind(&CTiagoNavModuleBT::get_tiago_nav_current_pose, this, std::placeholders::_1), current_pose_ports);
+  factory.registerSimpleAction("get_tiago_nav_goal_distance",  std::bind(&CTiagoNavModuleBT::get_tiago_nav_goal_distance, this, std::placeholders::_1), goal_distance_ports);
 
   #ifdef HAVE_WAYPOINTS
   BT::PortsList sync_go_to_waypoints_ports = {BT::InputPort<std::string>("group"), BT::InputPort<unsigned int>("first_index"), BT::InputPort<unsigned int>("num"), BT::InputPort<bool>("continue_on_error")};
@@ -682,6 +684,15 @@ BT::NodeStatus CTiagoNavModuleBT::get_tiago_nav_current_pose(BT::TreeNode& self)
   return BT::NodeStatus::SUCCESS;
 }
 
+BT::NodeStatus CTiagoNavModuleBT::get_tiago_nav_goal_distance(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoNavModuleBT::get_tiago_nav_goal_distance-> get_tiago_nav_goal_distance");
+
+  double dist = this->tiago_nav_module.getGoalDistance();
+  self.setOutput("distance", dist);
+  return BT::NodeStatus::SUCCESS;
+}
+
 BT::NodeStatus CTiagoNavModuleBT::sync_cancel_tiago_nav_action(void)
 {
   ROS_DEBUG("CTiagoNavModuleBT::sync_cancel_tiago_nav_action-> sync_cancel_tiago_nav_action");

src/tiago_nav_client_alg_node.cpp

@@ -111,6 +111,12 @@ void TiagoNavClientAlgNode::node_config_update(Config &config, uint32_t level)
     this->nav.stop();
     config.stop_wp=false;
   }
+  if (config.print_goal_distance)
+  {
+    double dist = this->nav.getGoalDistance();
+    ROS_INFO_STREAM("Goal distance = " << dist);
+    config.print_goal_distance = false;
+  }
   this->alg_.unlock();
 }
 

src/tiago_nav_module.cpp

@@ -43,6 +43,9 @@ CTiagoNavModule::CTiagoNavModule(const std::string &name,const std::string &name
   // this->odom_watchdog.reset(ros::Duration(this->config.odom_watchdog_time_s));
   this->odom_subscriber = this->module_nh.subscribe("odom",1,&CTiagoNavModule::odom_callback,this);
 
+  // initialize path subscriber
+  this->path_subscriber = this->module_nh.subscribe("path", 1, &CTiagoNavModule::path_callback, this);
+
   // tf listener
   this->transform_position=false;
   this->transform_orientation=false;
@@ -403,6 +406,14 @@ void CTiagoNavModule::odom_callback(const nav_msgs::Odometry::ConstPtr& msg)
   this->unlock();
 }
 
+void CTiagoNavModule::path_callback(const nav_msgs::Path::ConstPtr& msg){
+  ROS_DEBUG("CTiagoNavModule: path callback");
+  this->lock();
+  this->path_msg = new nav_msgs::Path();
+  *(this->path_msg) = *msg;
+  this->unlock();
+}
+
 bool CTiagoNavModule::transform_current_pose(void)
 {
   geometry_msgs::PoseStamped pose;
@@ -909,6 +920,28 @@ geometry_msgs::Pose CTiagoNavModule::getCurrentPose(void) {
   return this->current_pose.pose;
 }
 
+double CTiagoNavModule::getGoalDistance(void)
+{
+  this->lock();
+  if (this->state == TIAGO_NAV_MODULE_IDLE)
+  {
+    ROS_WARN("CTiagoNavModule::getGoalDistance -> Goal distance not calculed. There is no current goal.");
+    this->unlock();
+    return -1.0;
+  }
+  double dist = 0.0;
+
+  for (unsigned int i=0; i<this->path_msg->poses.size()-1; i++)
+  {
+    double dx = this->path_msg->poses[i].pose.position.x - this->path_msg->poses[i+1].pose.position.x;
+    double dy = this->path_msg->poses[i].pose.position.y - this->path_msg->poses[i+1].pose.position.y;
+    double d = std::sqrt(std::pow(dx,2) + std::pow(dy,2));
+    dist += d;
+  }
+  this->unlock();
+  return dist;
+}
+
 CTiagoNavModule::~CTiagoNavModule()
 {
   if(!this->move_base_action.is_finished())

src/xml/bt_test.xml

@@ -143,6 +143,9 @@
         <Action ID="get_tiago_nav_current_pose">
             <output_port name="pose"> current pose</output_port>
         </Action>
+        <Action ID="get_tiago_goal_distance">
+            <output_port name="distance"> current goal distance</output_port>
+        </Action>
         <Condition ID="is_tiago_nav_finished"/>
         <Condition ID="is_tiago_nav_module_running"/>
         <Condition ID="is_tiago_nav_module_success"/>