From 3436a5fa10511730db0b8a06f70da6b6cf395a6c Mon Sep 17 00:00:00 2001
From: Alopez <alopez@iri.upc.edu>
Date: Mon, 20 Feb 2023 16:56:31 +0100
Subject: [PATCH] Added time argument to transform_pose_2D
---
include/iri_nav_module/nav_module.h | 15 +++++++--------
1 file changed, 7 insertions(+), 8 deletions(-)
diff --git a/include/iri_nav_module/nav_module.h b/include/iri_nav_module/nav_module.h
index 44144fe..1502a8f 100644
--- a/include/iri_nav_module/nav_module.h
+++ b/include/iri_nav_module/nav_module.h
@@ -252,7 +252,7 @@ class CNavModule : public CModule<ModuleCfg>
* \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);
+ bool transform_pose_2D(const std::string& parent_frame, const std::string& child_frame, ros::Time time, double &x,double &y,double &yaw);
// common functions
/**
@@ -559,22 +559,21 @@ class CNavModule : public CModule<ModuleCfg>
}
template <class ModuleCfg>
- bool CNavModule<ModuleCfg>::transform_pose_2D(const std::string& parent_frame, const std::string& child_frame, double &x,double &y,double &yaw)
+ bool CNavModule<ModuleCfg>::transform_pose_2D(const std::string& parent_frame, const std::string& child_frame, ros::Time time, double &x,double &y,double &yaw)
{
geometry_msgs::TransformStamped transform;
geometry_msgs::PoseStamped pose,current_pose;
- ros::Time current_time=ros::Time::now();
tf2::Quaternion quat;
double roll, pitch;
bool tf2_exists;
try{
- tf2_exists=this->tf2_buffer.canTransform(parent_frame,child_frame,current_time,ros::Duration(this->tf_timeout_time_s));
+ tf2_exists=this->tf2_buffer.canTransform(parent_frame,child_frame,time,ros::Duration(this->tf_timeout_time_s));
if(tf2_exists)
{
- transform = this->tf2_buffer.lookupTransform(parent_frame,child_frame,current_time);
+ transform = this->tf2_buffer.lookupTransform(parent_frame,child_frame,time);
current_pose.header.frame_id=parent_frame;
- current_pose.header.stamp=current_time;
+ current_pose.header.stamp=time;
current_pose.pose.position.x=x;
current_pose.pose.position.y=y;
current_pose.pose.position.z=0.0;
@@ -590,7 +589,7 @@ class CNavModule : public CModule<ModuleCfg>
}
else
{
- ROS_WARN_STREAM("CNavModule: No transform found from " << child_frame << " to " << parent_frame);
+ ROS_WARN_STREAM("CNavModule: No transform found from " << child_frame << " to " << parent_frame << " at " << time);
return false;
}
}catch(tf2::TransformException &ex){
@@ -604,7 +603,7 @@ class CNavModule : public CModule<ModuleCfg>
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))
+ if (transform_pose_2D(this->global_frame_id,this->goal_frame_id,ros::Time::now(),x,y,yaw))
return true;
return false;
}
--
GitLab