tf lookuptransform
Hello,
I'm trying to move my robot to some position. So I made a frame in some random postition named /posicao_objectivo and I made a simple broadcaster to set this position in reference to the frame /odom that gets set by Gazebo.
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
int main(int argc, char** argv){
ros::init(argc, argv, "tf_broadcaster_posicaoRobot");
ros::NodeHandle node;
tf::TransformBroadcaster br;
tf::Transform transform;
ros::Rate rate(1.0);
while(node.ok()){
transform.setOrigin( tf::Vector3(5, 0.0, 0.0) );
transform.setRotation(tf::Quaternion(0, 0, 0, 1) );
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(),"/odom", "/posicao_objectivo"));
rate.sleep();
}
return 0;
};
After this I made a listener that uses the function lookuptransform to calculate the transform from the robot /base_link to my set position /posicao_objectivo created by my broadcaster.
#include <ros/ros.h>
#include <tf transform_listener.h=""> #include <geometry_msgs twist.h="">
int main(int argc, char** argv){ ros::init(argc, argv, "my_tf_listener_mexeRobot");
ros::NodeHandle node;
ros::Publisher robot_vel = node.advertise<geometry_msgs::Twist>("p3dx/cmd_vel", 10);
tf::TransformListener listener;
ros::Rate rate(1.0); while (node.ok()){
tf::StampedTransform transform;
try{
listener.waitForTransform("/base_link", "/posicao_objectivo",
ros::Time(0), ros::Duration(3.0));
listener.lookupTransform("/base_link", "/posicao_objectivo",
ros::Time(0), transform);
}
catch (tf::TransformException &ex) {
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
geometry_msgs::Twist vel_msg;
vel_msg.angular.z = 0.2 * atan2(transform.getOrigin().y(),
transform.getOrigin().x());
vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
pow(transform.getOrigin().y(), 2));
robot_vel.publish(vel_msg);
rate.sleep();
}
return 0;
};
The problem is that when I run the listener I get this error:
[ERROR] [1425495472.170621190, 132.992000000]: "posicao_objectivo" passed to lookupTransform argument source_frame does not exist.
I tried using the command: rosrun tf tf_echo odom posicao_objectivo
And it says:
Exception thrown:"posicao_objectivo" passed to lookupTransform argument source_frame does not exist.
The current list of frames is:
Frame base_link exists with parent odom.
Frame front_sonar exists with parent base_link.
Frame p3dx_left_hubcap exists with parent p3dx_left_wheel.
Frame p3dx_right_hubcap exists with parent p3dx_right_wheel.
Frame top_plate exists with parent base_link.
Frame camera_link exists with parent Arucos.
Frame Arucos exists with parent top_plate.
Frame center_hubcap exists with parent center_wheel.
Frame lms100 exists with parent base_link.
What is confusing me Is that went I start the broadcast of my frame /posicao_objectivo the frame appears in RVIZ tree and it appears in rosrun rqt_tf_tree rqt_tf_tree. So the frame does exist, don't understand how it does not work with rosrun tf tf_echo odom posicao_objectivo.
I did what you said, I think the problem is that the publish rate of the transform posicao_objectivo is too low. It is 1hz and the base_link has a rate of 100hz. I think this is the problem. I'm still testing. Thx you.
Ok, did you manage to get it working.. also the lookupTransform take some time to pic up the transformation from /tf topic. I had the same problem before. I also changed ROS_ERROR to ROS_WARNING.
I set the rate to 1000hz and I stop having problems, I was also giving was too much velocity and angle to my robot. Now it moves to a point. Thx you so much for the help.