Robotics StackExchange | Archived questions

canTransform() methode always returns False

Hi, I want to transform the coordinate frame of a point to another frame, to use it later in generating the commanded velocities. the code is the following:

#include <ros/ros.h>
#include <geometry_msgs/PointStamped.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/Float32.h>
#include <cmath>
#include <cstdlib>
#include <std_msgs/String.h>
#include <nav_msgs/Odometry.h>
#include <std_msgs/Empty.h>
#include <tf/transform_broadcaster.h>
#define M_PI 3.14159265358979323846 /* pi */


float current_x, current_y, current_z;
 float crt_x, crt_y, crt_z;
 int in = 0;
int out = 0;
double z;
double x;
double ti=0;
double t=0;


void poseCallBack(const nav_msgs::Odometry& msg) {

tf::StampedTransform transform;
tf::TransformListener listener;
tf::Quaternion Rot;

crt_x = msg.pose.pose.position.x;
crt_y = msg.pose.pose.position.y;
crt_z = msg.pose.pose.position.z;

  geometry_msgs::PointStamped odom_point;
  odom_point.header.frame_id = "odom";

  //we'll just use the most recent transform available for our simple example
 odom_point.header.stamp = ros::Time();

 //just an arbitrary point in space
 odom_point.point = msg.pose.pose.position;
try
{
ros::Time now = ros::Time::now();
if  (listener.canTransform("odom", "newFrame",ros::Time(0), std::string *error_msg=NULL))

{
listener.lookupTransform("odom","newFrame",  ros::Time(0), transform);
    in = 1;
 ROS_INFO("canTransform: TRUE");
 geometry_msgs::PointStamped newFrame_point;
 listener.transformPoint("newFrame", odom_point, newFrame_point);

 current_x = newFrame_point.point.x;
 current_y = newFrame_point.point.y;
 current_z = newFrame_point.point.z;

  ROS_INFO("odom: (%.2f, %.2f. %.2f) -----> newFrame: (%.2f, %.2f, %.2f) at time %.2f",
       odom_point.point.x, odom_point.point.y, odom_point.point.z,
       newFrame_point.point.x, newFrame_point.point.y, newFrame_point.point.z, newFrame_point.header.stamp.toSec());
   }
   else
   {
    ROS_INFO("canTransform: FALSE");
    }
          }
    catch (tf::TransformException &ex)
      {
     ROS_WARN("%s", ex.what());
      ros::Duration(1);
      }
       }

       int main(int argc, char ** argv) {
      ros::init(argc, argv, "trajectoryGoalTest");
       ros::NodeHandle nh_;
   ros::Publisher landPublisher=nh_.advertise<std_msgs::Empty>("/bebop/land", 10);
   ros::Subscriber odom_sub = nh_.subscribe("/bebop/odom", 10, & poseCallBack);
    ros::Publisher cmd_vel_pub = nh_.advertise<geometry_msgs::Twist>("/bebop/cmd_vel", 10);

     ros::Rate loop_rate(10);
      geometry_msgs::Point goal;
      goal.x = 1;
     goal.z = 1.3;

       while (ros::ok()) {
      if (in> 0) {
       z = goal.z - current_z;
        x = goal.x - current_x;
         ROS_INFO("Current x: %f , x goal:  %f", current_x ,goal.x);
         ROS_INFO("Current Z: %f , Z goal:  %f , Z difference:  %f", current_z , goal.z , z);
       if (z > 0.001 && out == 0) {
      ROS_INFO("in= : %d , z= : %f, out= : %d ", in , z , out);

   }else {
         out = 1;
    ROS_INFO("I reached");}
     if (out == 0) {
    geometry_msgs::Twist cmd;
    cmd.linear.z= 0.1*(goal.z - current_z);
   ROS_INFO("vel Z: %f ",cmd.linear.z);
   cmd_vel_pub.publish(cmd);
   ROS_INFO(" Out : %d, vel X: %f, vel Z: %f", out , cmd.linear.x , cmd.linear.z);
  } else{

   if (x > 0.001 && out == 1) {     
    geometry_msgs::Twist cmd;
    cmd.linear.x = 0.01*(goal.x - current_x);
    cmd.linear.z = -0.01;
  ROS_INFO("Out : %d, vel X: %f, vel Z: %f", out , cmd.linear.x , cmd.linear.z);
   cmd_vel_pub.publish(cmd);
  }
else{  
  ROS_INFO("am landing");
   std_msgs::Empty cmd;
  landPublisher.publish(cmd);    
   ROS_INFO("Out : %d", out );         
    }
        }

 loop_rate.sleep();
  }
   ros::spinOnce();
     }
      return 0;
   }

When I run the code the methode canTransform returns always False and I get this message:

[ INFO] [1567617188.063607542]: canTransform: FALSE

However both frames are existing:

image description

Any suggestion. Thanks

Asked by rayane on 2019-09-04 15:51:45 UTC

Comments

Can you run "lookupTransform" before "canTransform" and share the message in the exception? This may help to identify the problem. Also, check if you can see the transforms you need in RViz.

Asked by VitaliyProoks on 2019-09-04 23:26:30 UTC

@VitaliyProoks I ve put the lookupTransform before the if statement and I got this: [ WARN] [1567670512.361513751]: "odom" passed to lookupTransform argument target_frame does not exist. although I can see both frames.

Asked by rayane on 2019-09-05 03:05:09 UTC

Answers

The problem is caused because you're trying to use the tf::TransformListener listener object immediately after creating it. These objects work by listening to the tf and tf_static topics and storing a buffer of recent values. It then uses this buffer of messages to resolve transform queries.

Because of this you need to allow enough time for the TransformListener object to collect an initial buffer of values before you start using it.

It is recommended that you create a single tf::TransformListener object when your node starts up, then to use that same object for the lifetime of the node.

Hope this helps.

Asked by PeteBlackerThe3rd on 2019-09-05 03:20:43 UTC

Comments

@PeteBlackerThe3rd thanks for your answer You mean by that initialize a tf::TransformListener object as global!!!

Asked by rayane on 2019-09-05 03:26:39 UTC

That's an option, although it's good programming practice to avoid globals. We'd recommend you create it in main after initialising the node. Then pass references to functions that use it.

Asked by PeteBlackerThe3rd on 2019-09-05 03:34:02 UTC