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:
Any suggestion. Thanks
Asked by rayane on 2019-09-04 15:51:45 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
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