Ask Your Question
0

canTransform() methode always returns False

asked 2019-09-04 15:51:45 -0600

rayane gravatar image

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 ... (more)

edit retag flag offensive close merge delete

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.

VitaliyProoks gravatar imageVitaliyProoks ( 2019-09-04 23:26:30 -0600 )edit

@VitaliyProoks I ve put the lookupTransform before the if statement and I got this: although I can see both frames.

rayane gravatar imagerayane ( 2019-09-05 03:05:09 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2019-09-05 03:20:43 -0600

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.

edit flag offensive delete link more

Comments

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

rayane gravatar imagerayane ( 2019-09-05 03:26:39 -0600 )edit

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.

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2019-09-05 03:34:02 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2019-09-04 15:51:45 -0600

Seen: 31 times

Last updated: Sep 05