From e2f33076393adad6f099717f44b073041b9522e3 Mon Sep 17 00:00:00 2001
From: Alopez <alopez@iri.upc.edu>
Date: Fri, 3 Feb 2023 11:39:17 +0100
Subject: [PATCH] Added transform pose2D function. Changed get_current_pose to
return a success bool
---
include/iri_nav_module/nav_module.h | 87 +++++++++++++++++---------
include/iri_nav_module/nav_module_bt.h | 10 +--
2 files changed, 64 insertions(+), 33 deletions(-)
diff --git a/include/iri_nav_module/nav_module.h b/include/iri_nav_module/nav_module.h
index 6aba9cf..44144fe 100644
--- a/include/iri_nav_module/nav_module.h
+++ b/include/iri_nav_module/nav_module.h
@@ -68,6 +68,22 @@ class CNavModule : public CModule<ModuleCfg>
*
*/
CModuleService<nav_msgs::GetPlan,ModuleCfg> make_plan_service;
+ // tf listener
+ /**
+ * \brief
+ *
+ */
+ tf2_ros::Buffer tf2_buffer;
+ /**
+ * \brief
+ *
+ */
+ tf2_ros::TransformListener tf2_listener;
+ /**
+ * \brief
+ *
+ */
+ double tf_timeout_time_s;
// dynamic reconfigure
/**
* \brief
@@ -157,22 +173,6 @@ class CNavModule : public CModule<ModuleCfg>
*/
CNavCostmapModule<ModuleCfg> global_costmap;
protected:
- // tf listener
- /**
- * \brief
- *
- */
- tf2_ros::Buffer tf2_buffer;
- /**
- * \brief
- *
- */
- tf2_ros::TransformListener tf2_listener;
- /**
- * \brief
- *
- */
- double tf_timeout_time_s;
/**
* \brief
*
@@ -235,6 +235,25 @@ class CNavModule : public CModule<ModuleCfg>
*
*/
bool 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 transform a 2D pose
+ *
+ * \param parent_frame Parent frame id.
+ *
+ * \param child_frame Child frame id.
+ *
+ * \param x X coordinate to be transform.
+ *
+ * \param y Y coordinate to be transform.
+ *
+ * \param yaw Orientation to be transform.
+ *
+ * \return False if the transform can't be done.
+ *
+ */
+ bool transform_pose_2D(const std::string& parent_frame, const std::string& child_frame, double &x,double &y,double &yaw);
+
// common functions
/**
* \brief Set the robot reference frame
@@ -328,10 +347,12 @@ class CNavModule : public CModule<ModuleCfg>
nav_msgs::Path get_current_path(void);
/**
* \brief Function to get the current pose of the robot.
+ *
+ * \param _pose The robot pose with respect to the map frame.
*
- * \return The robot pose with respect to the map frame.
+ * \return False if something goes wrong.
*/
- geometry_msgs::PoseStamped get_current_pose(void);
+ bool get_current_pose(geometry_msgs::PoseStamped& _pose);
/**
* \brief Function to get the euclidian distance to the goal.
*
@@ -538,7 +559,7 @@ class CNavModule : public CModule<ModuleCfg>
}
template <class ModuleCfg>
- bool CNavModule<ModuleCfg>::transform_goal(double &x,double &y,double &yaw)
+ bool CNavModule<ModuleCfg>::transform_pose_2D(const std::string& parent_frame, const std::string& child_frame, double &x,double &y,double &yaw)
{
geometry_msgs::TransformStamped transform;
geometry_msgs::PoseStamped pose,current_pose;
@@ -548,11 +569,11 @@ class CNavModule : public CModule<ModuleCfg>
bool tf2_exists;
try{
- tf2_exists=this->tf2_buffer.canTransform(this->global_frame_id,this->goal_frame_id,current_time,ros::Duration(this->tf_timeout_time_s));
+ tf2_exists=this->tf2_buffer.canTransform(parent_frame,child_frame,current_time,ros::Duration(this->tf_timeout_time_s));
if(tf2_exists)
{
- transform = this->tf2_buffer.lookupTransform(this->global_frame_id,this->goal_frame_id,current_time);
- current_pose.header.frame_id=this->global_frame_id;
+ transform = this->tf2_buffer.lookupTransform(parent_frame,child_frame,current_time);
+ current_pose.header.frame_id=parent_frame;
current_pose.header.stamp=current_time;
current_pose.pose.position.x=x;
current_pose.pose.position.y=y;
@@ -569,7 +590,7 @@ class CNavModule : public CModule<ModuleCfg>
}
else
{
- ROS_WARN_STREAM("CNavModule: No transform found from " << this->goal_frame_id << " to " << this->global_frame_id);
+ ROS_WARN_STREAM("CNavModule: No transform found from " << child_frame << " to " << parent_frame);
return false;
}
}catch(tf2::TransformException &ex){
@@ -580,6 +601,14 @@ class CNavModule : public CModule<ModuleCfg>
return false;
}
+ template <class ModuleCfg>
+ bool CNavModule<ModuleCfg>::transform_goal(double &x,double &y,double &yaw)
+ {
+ if (transform_pose_2D(this->global_frame_id,this->goal_frame_id,x,y,yaw))
+ return true;
+ return false;
+ }
+
template <class ModuleCfg>
void CNavModule<ModuleCfg>::clear_costmaps_call(const ros::TimerEvent& event)
{
@@ -701,10 +730,10 @@ class CNavModule : public CModule<ModuleCfg>
}
template <class ModuleCfg>
- geometry_msgs::PoseStamped CNavModule<ModuleCfg>::get_current_pose(void)
+ bool CNavModule<ModuleCfg>::get_current_pose(geometry_msgs::PoseStamped& _pose)
{
geometry_msgs::TransformStamped transform;
- geometry_msgs::PoseStamped pose,current_pose;
+ geometry_msgs::PoseStamped current_pose;
ros::Time current_time=ros::Time::now();
bool tf2_exists;
@@ -722,19 +751,19 @@ class CNavModule : public CModule<ModuleCfg>
current_pose.pose.orientation.y=0.0;
current_pose.pose.orientation.z=0.0;
current_pose.pose.orientation.w=1.0;
- tf2::doTransform(current_pose,pose,transform);
+ tf2::doTransform(current_pose,_pose,transform);
}
else
{
ROS_WARN_STREAM("CNavModule: No transform found from " << this->robot_frame_id << " to " << this->global_frame_id);
- return pose;
+ return false;
}
}catch(tf2::TransformException &ex){
ROS_ERROR_STREAM("CNavModule: TF2 Exception: " << ex.what());
- return pose;
+ return false;
}
- return pose;
+ return true;
}
template <class ModuleCfg>
diff --git a/include/iri_nav_module/nav_module_bt.h b/include/iri_nav_module/nav_module_bt.h
index 589b840..a17ae64 100644
--- a/include/iri_nav_module/nav_module_bt.h
+++ b/include/iri_nav_module/nav_module_bt.h
@@ -463,11 +463,13 @@ class CNavModuleBT
BT::NodeStatus CNavModuleBT<ModuleCfg>::get_current_pose(BT::TreeNode& self)
{
ROS_DEBUG("CNavModuleBT::get_current_pose-> get_current_pose");
-
- geometry_msgs::PoseStamped pose_out = this->nav_module.get_current_pose();
-
+ geometry_msgs::PoseStamped pose_out;
+ bool success = this->nav_module.get_current_pose(pose_out);
self.setOutput("pose", pose_out);
- return BT::NodeStatus::SUCCESS;
+
+ if (success)
+ return BT::NodeStatus::SUCCESS;
+ return BT::NodeStatus::FAILURE;
}
template <class ModuleCfg>
--
GitLab