Added time argument to transform_pose_2D

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;
   }