From 400a458fb0fdca487bd8850e814a24b3dfc3f12a Mon Sep 17 00:00:00 2001
From: Alopez <alopez@iri.upc.edu>
Date: Fri, 16 Jul 2021 14:14:39 +0200
Subject: [PATCH] Fixed some bugs on goal distance calculation
---
include/tiago_nav_module/tiago_nav_module.h | 13 +++++++++-
src/tiago_nav_module.cpp | 27 ++++++++++++++-------
2 files changed, 30 insertions(+), 10 deletions(-)
diff --git a/include/tiago_nav_module/tiago_nav_module.h b/include/tiago_nav_module/tiago_nav_module.h
index 570080d..8f213e2 100644
--- a/include/tiago_nav_module/tiago_nav_module.h
+++ b/include/tiago_nav_module/tiago_nav_module.h
@@ -9,6 +9,7 @@
#include <iri_ros_tools/watchdog.h>
#include <iri_ros_tools/timeout.h>
#include <iostream>
+#include <limits>
// ROS headers
#include <nav_msgs/Odometry.h>
@@ -217,7 +218,17 @@ class CTiagoNavModule : public CModule<tiago_nav_module::TiagoNavModuleConfig>
*/
void path_callback(const nav_msgs::Path::ConstPtr& msg);
- nav_msgs::Path *path_msg;
+ /**
+ * \brief
+ *
+ */
+ nav_msgs::Path path_msg;
+
+ /**
+ * \brief
+ *
+ */
+ bool path_available;
/**
* \brief
*
diff --git a/src/tiago_nav_module.cpp b/src/tiago_nav_module.cpp
index dd337b9..5680d0d 100644
--- a/src/tiago_nav_module.cpp
+++ b/src/tiago_nav_module.cpp
@@ -30,6 +30,7 @@ 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
if(!this->module_nh.getParam("odom_watchdog_time_s", this->config.odom_watchdog_time_s))
@@ -409,8 +410,8 @@ void CTiagoNavModule::odom_callback(const nav_msgs::Odometry::ConstPtr& msg)
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->path_msg = *msg;
+ this->path_available = true;
this->unlock();
}
@@ -534,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;
@@ -579,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;
@@ -633,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;
@@ -923,18 +927,23 @@ geometry_msgs::Pose CTiagoNavModule::getCurrentPose(void) {
double CTiagoNavModule::getGoalDistance(void)
{
this->lock();
- if (this->state == TIAGO_NAV_MODULE_IDLE)
+ if (!this->path_available)
{
- ROS_WARN("CTiagoNavModule::getGoalDistance -> Goal distance not calculed. There is no current goal.");
+ ROS_WARN("CTiagoNavModule::getGoalDistance -> Goal distance not calculed. No path received.");
this->unlock();
- return -1.0;
+ return std::numeric_limits<double>::quiet_NaN();
}
double dist = 0.0;
-
- for (unsigned int i=0; i<this->path_msg->poses.size()-1; i++)
+ 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 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;
}
--
GitLab