diff --git a/cfg/TiagoNavClient.cfg b/cfg/TiagoNavClient.cfg
index cc49d7b81a0f6a7a23e39fe1f7447f5dc4ee0a50..2636b87254a9fab9879e795e95fa16dd05ee91c0 100755
--- a/cfg/TiagoNavClient.cfg
+++ b/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)
diff --git a/include/tiago_nav_module/tiago_nav_bt_module.h b/include/tiago_nav_module/tiago_nav_bt_module.h
index 376343aec6823b833599d3fadd0b2fe842e1961b..6eca5824cf2b14b6d05c92623741419760a59452 100644
--- a/include/tiago_nav_module/tiago_nav_bt_module.h
+++ b/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.
*
diff --git a/include/tiago_nav_module/tiago_nav_module.h b/include/tiago_nav_module/tiago_nav_module.h
index 214a7e181aec81c6a92dfa95cdb9817ea34c64fe..8f213e28300a61a47784775f34c612db5a4d80c1 100644
--- a/include/tiago_nav_module/tiago_nav_module.h
+++ b/include/tiago_nav_module/tiago_nav_module.h
@@ -9,9 +9,11 @@
#include <iri_ros_tools/watchdog.h>
#include <iri_ros_tools/timeout.h>
#include <iostream>
+#include <limits>
// 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 +206,29 @@ 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);
+
+ /**
+ * \brief
+ *
+ */
+ nav_msgs::Path path_msg;
+
+ /**
+ * \brief
+ *
+ */
+ bool path_available;
/**
* \brief
*
@@ -653,6 +678,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
diff --git a/launch/ivo_nav_bt_client.launch b/launch/ivo_nav_bt_client.launch
index 8234d51896feed6e931d71a0027c4f1b79cee6a7..480be466d531081e42475e78cfb17fe5c12ffda2 100644
--- a/launch/ivo_nav_bt_client.launch
+++ b/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)"/>
diff --git a/launch/ivo_nav_bt_client_sim.launch b/launch/ivo_nav_bt_client_sim.launch
index 6dfa75e0e0dcdd4ba125c5efa238507ac0e3fb97..ba18392e8ae0439ace8cb0b5969e49f4dfef020a 100644
--- a/launch/ivo_nav_bt_client_sim.launch
+++ b/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)"/>
diff --git a/launch/ivo_nav_client.launch b/launch/ivo_nav_client.launch
index 32acbb0273355eb6ad0185e3ef79528b07994f0e..c06285f5fe0107a605b04241be056ba5944ddcf5 100644
--- a/launch/ivo_nav_client.launch
+++ b/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)"/>
diff --git a/launch/ivo_nav_client_sim.launch b/launch/ivo_nav_client_sim.launch
index 69bb18e8456fff165e098820942212bcdb210afb..40da268f0f318c66112d6b0c071d82968576dcc4 100644
--- a/launch/ivo_nav_client_sim.launch
+++ b/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)"/>
diff --git a/launch/tiago_nav_bt_client.launch b/launch/tiago_nav_bt_client.launch
index 60c0d7c7f2ad18ea8e8d5f870ddd1946fa2a15f3..711ddb66dc969121af95bfceeb0422354865c88e 100644
--- a/launch/tiago_nav_bt_client.launch
+++ b/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"
diff --git a/launch/tiago_nav_bt_client_sim.launch b/launch/tiago_nav_bt_client_sim.launch
index 7ab7b5600ab0a20a623ed717e7929dc45f2b3f44..dae4375d1938780ff85def764a9be11eef8a2430 100644
--- a/launch/tiago_nav_bt_client_sim.launch
+++ b/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)"/>
diff --git a/launch/tiago_nav_client.launch b/launch/tiago_nav_client.launch
index 0402ef114d8173e4b507adb9df5cfaa9268a8755..df19caa174bd4eaa3d2635f345608ab5713f945a 100644
--- a/launch/tiago_nav_client.launch
+++ b/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"
diff --git a/launch/tiago_nav_client_sim.launch b/launch/tiago_nav_client_sim.launch
index 69bb18e8456fff165e098820942212bcdb210afb..40da268f0f318c66112d6b0c071d82968576dcc4 100644
--- a/launch/tiago_nav_client_sim.launch
+++ b/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)"/>
diff --git a/src/tiago_nav_bt_module.cpp b/src/tiago_nav_bt_module.cpp
index 5e51e472cbdc392b312def1f3a5d30214d0c1d1b..2f1cc4c96ddb670e827a8dbaf087c78e34e91595 100644
--- a/src/tiago_nav_bt_module.cpp
+++ b/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")};
@@ -685,6 +687,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");
diff --git a/src/tiago_nav_client_alg_node.cpp b/src/tiago_nav_client_alg_node.cpp
index 722503043ded76de2b465c8761410b303573add4..13887e1e6b29dc3775b50845584ec43d90ce0925 100644
--- a/src/tiago_nav_client_alg_node.cpp
+++ b/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();
}
diff --git a/src/tiago_nav_module.cpp b/src/tiago_nav_module.cpp
index 510c4a0b0fd8d5cf7e2906a2a717cbac11e35781..5680d0d44ab17973ce017389ea2ddf236a25bed1 100644
--- a/src/tiago_nav_module.cpp
+++ b/src/tiago_nav_module.cpp
@@ -30,17 +30,36 @@ CTiagoNavModule::CTiagoNavModule(const std::string &name,const std::string &name
this->new_waypoint=false;
#endif
this->cancel_pending=false;
+ this->path_available = false;
// initialize odometry subscriber
- this->odom_watchdog.reset(ros::Duration(this->config.odom_watchdog_time_s));
+ if(!this->module_nh.getParam("odom_watchdog_time_s", this->config.odom_watchdog_time_s))
+ {
+ ROS_WARN("CTiagoNavModule::CTiagoNavModule: param 'odom_watchdog_time_s' not found. Configuring wathcdog with 1.0 sec");
+ this->odom_watchdog.reset(ros::Duration(1.0));
+ }
+ else
+ this->odom_watchdog.reset(ros::Duration(this->config.odom_watchdog_time_s));
+
+ // 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;
// costmaps
- this->clear_costmaps_timer=this->module_nh.createTimer(ros::Duration(1.0/this->config.clear_costmap_auto_clear_rate_hz),&CTiagoNavModule::clear_costmaps_call,this);
+ if(!this->module_nh.getParam("clear_costmap_auto_clear_rate_hz", this->config.clear_costmap_auto_clear_rate_hz))
+ {
+ ROS_WARN("CTiagoNavModule::CTiagoNavModule: param 'clear_costmap_auto_clear_rate_hz' not found. Configuring wathcdog with 0.1 Hz");
+ this->clear_costmaps_timer=this->module_nh.createTimer(ros::Duration(1.0/0.1),&CTiagoNavModule::clear_costmaps_call,this);
+ }
+ else
+ this->clear_costmaps_timer=this->module_nh.createTimer(ros::Duration(1.0/this->config.clear_costmap_auto_clear_rate_hz),&CTiagoNavModule::clear_costmaps_call,this);
+ // this->clear_costmaps_timer=this->module_nh.createTimer(ros::Duration(1.0/this->config.clear_costmap_auto_clear_rate_hz),&CTiagoNavModule::clear_costmaps_call,this);
this->clear_costmaps_timer.stop();
this->enable_auto_clear=false;
@@ -388,6 +407,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 = *msg;
+ this->path_available = true;
+ this->unlock();
+}
+
bool CTiagoNavModule::transform_current_pose(void)
{
geometry_msgs::PoseStamped pose;
@@ -508,6 +535,7 @@ bool CTiagoNavModule::go_to_orientation(double yaw,double heading_tol)
this->move_base_goal.target_pose.header.frame_id=this->goal_frame_id;
this->move_base_goal.target_pose.pose.orientation=tf::createQuaternionMsgFromYaw(yaw);
this->new_goal=true;
+ this->path_available = false;
this->unlock();
return true;
@@ -553,6 +581,7 @@ bool CTiagoNavModule::go_to_position(double x,double y,double x_y_pos_tol)
this->move_base_goal.target_pose.pose.position.y=y;
this->move_base_goal.target_pose.pose.position.z=0.0;
this->new_goal=true;
+ this->path_available = false;
this->unlock();
return true;
@@ -607,6 +636,7 @@ bool CTiagoNavModule::go_to_pose(double x,double y,double yaw,double heading_tol
this->move_base_goal.target_pose.pose.position.z=0.0;
this->move_base_goal.target_pose.pose.orientation=tf::createQuaternionMsgFromYaw(yaw);
this->new_goal=true;
+ this->path_available = false;
this->unlock();
return true;
@@ -894,6 +924,33 @@ geometry_msgs::Pose CTiagoNavModule::getCurrentPose(void) {
return this->current_pose.pose;
}
+double CTiagoNavModule::getGoalDistance(void)
+{
+ this->lock();
+ if (!this->path_available)
+ {
+ ROS_WARN("CTiagoNavModule::getGoalDistance -> Goal distance not calculed. No path received.");
+ this->unlock();
+ return std::numeric_limits<double>::quiet_NaN();
+ }
+ double dist = 0.0;
+ if (this->path_msg.poses.size() < 2)
+ {
+ ROS_WARN("CTiagoNavModule::getGoalDistance -> Goal distance not calculed. Not enough points.");
+ this->unlock();
+ return std::numeric_limits<double>::quiet_NaN();
+ }
+ 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())
diff --git a/src/xml/bt_test.xml b/src/xml/bt_test.xml
index 7b5632e97faaeb24b989b9e9a2a5ae17620189a0..63ecdbd3051fd08b21c94de0d841fbc5be36e53c 100644
--- a/src/xml/bt_test.xml
+++ b/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"/>