Ask Your Question
0

tf tree problem with map->odom->base_link [closed]

asked 2016-05-10 05:45:43 -0500

MahsaP gravatar image

updated 2016-05-12 07:44:37 -0500

Hi,

I have a problem with providing the following relationship "map->odom->base_link" in my tf tree. My question is what changes are necessary to fix this problem. In the below sections I will show what I have done.

I have added the following plugins to the .gazebo file.

<!-- GPS plugin -->
<plugin name="robot_gps_sim" filename="libhector_gazebo_ros_gps.so">
  <alwaysOn>true</alwaysOn>
  <updateRate>4.0</updateRate>
  <bodyName>base_link</bodyName>
  <topicName>gps</topicName>
  <velocityTopicName>gps_velocity</velocityTopicName>
  <drift>5.0 5.0 5.0</drift>
  <gaussianNoise>0.1 0.1 0.1</gaussianNoise>
  <velocityDrift>0 0 0</velocityDrift>
  <velocityGaussianNoise>0.1 0.1 0.1</velocityGaussianNoise>
</plugin>

<!-- Groundtruth plugin -->
<plugin name="robot_groundtruth_sim" filename="libgazebo_ros_p3d.so">
  <alwaysOn>true</alwaysOn>
  <updateRate>100.0</updateRate>
  <bodyName>base_link</bodyName>
  <topicName>groundtruth</topicName>
  <gaussianNoise>0.0</gaussianNoise>
  <frameName>map</frameName>
  <xyzOffsets>0 0 0</xyzOffsets>
  <rpyOffsets>0 0 0</rpyOffsets>
</plugin>

<!-- IMU plugin -->
<plugin name="robot_imu_sim" filename="libhector_gazebo_ros_imu.so">
    <alwaysOn>true</alwaysOn>
    <updateRate>100.0</updateRate> 
    <bodyName>base_link</bodyName>
    <frameID>robot_link1</frameID>
    <topicName>/robot/IMU</topicName>
    <xyzOffset>0.0 0.0 0.0</xyzOffset>
    <rpyOffset>0.0 0.0 0.0</rpyOffset>
    <gaussianNoise>0</gaussianNoise>
    <accelDrift>0.5 0.5 0.5</accelDrift>
    <accelGaussianNoise>0.35 0.35 0.3</accelGaussianNoise>
    <rateDrift>0.0 0.0 0.0</rateDrift>
    <rateGaussianNoise>0.00 0.00 0.00</rateGaussianNoise>
    <headingDrift>0.0</headingDrift>
    <headingGaussianNoise>0.00</headingGaussianNoise>
</plugin>

Here is the tf tree: image description

I ran the gmapping node. So because of this node, there is a connection between odom and map links. But there is no connection between odom and base_link.

I also tried to add a node to make the connection between these two links, but when I do rosrun tf view_frames nothing changes.

Edit: I realized that the transform from odom to base_link is computed and broadcast by one of the odometry sources. So, I modified the following node. Now, when I do "rotopic echo odom", the information is getting updated correctly, but there is still no link between odom and base_link. Actually, I tried to do something similar to this link. If I run it , the link between base_link and odom is provided, but I don't know what is wrong with my code that doesn't make the connection between link.

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>

class OdomTf{

public:
    OdomTf();
    void getInfo(const nav_msgs::Odometry::ConstPtr& msg);
    ros::Time current_time;
    void publishOdom();
    nav_msgs::Odometry odom;

private:  
    ros::NodeHandle nh_;
    ros::Subscriber data_sub_;
    ros::Publisher data_pub_;
};

OdomTf::OdomTf(){
    data_sub_ = nh_.subscribe("groundtruth", 1, &OdomTf::getInfo, this);
    data_pub_ = nh_.advertise<nav_msgs::Odometry>("odom", 10);
}
void OdomTf::getInfo(const nav_msgs::Odometry::ConstPtr& msg)
{   
    tf::TransformBroadcaster odom_broadcaster;

    geometry_msgs::TransformStamped odom_trans;

    current_time = ros::Time::now();

    odom_trans.header.stamp = current_time;
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "base_link";
    odom_trans.transform.translation.x = msg->pose.pose.position.x;
    odom_trans.transform.translation.y = msg->pose.pose.position.y;
    odom_trans.transform.translation.z = msg->pose.pose.position.z;

    odom_trans.transform.rotation.x = msg->pose.pose.orientation.x;
    odom_trans.transform.rotation ...
(more)
edit retag flag offensive reopen merge delete

Closed for the following reason the question is answered, right answer was accepted by MahsaP
close date 2016-05-12 09:41:23.957366

Comments

How did you estimated state of robot? The creating link between odom and base_link is job of localization node. It may be robot_pose_ekf or it will be controller's node directly: just set /mobile_base_controller/enable_odom_tf to true.

Orhan gravatar imageOrhan ( 2016-05-10 07:19:42 -0500 )edit

@Orhangazi44 For localizing the robot, I've used gmapping. I am using gazebo, and I want to see the changes of the coordinate frames for each joint in Rviz as my legged robot walks in gazebo. I am using ros_control for gazebo, so I think I can't use the option you said. Do you know what I should do?

MahsaP gravatar imageMahsaP ( 2016-05-10 07:50:35 -0500 )edit

You probably running via controls.launch file. And probably with controls.yaml file. Change enable_odom_tf parameter in controls.yaml. If you are not, try adding these files to you structure. Here is examples in following comments:

Orhan gravatar imageOrhan ( 2016-05-10 08:15:31 -0500 )edit

<node name="controller_spawner" pkg="controller_manager" type="spawner"args="mrp2_joint_publisher mobile_base_controller --shutdown-timeout 1" />

Orhan gravatar imageOrhan ( 2016-05-10 08:18:31 -0500 )edit

@Orhangazi44 Thanks. I am using control.launch with control.yaml file. I think I need to add enable_odom_tf option somewhere. I haven't done it before.

MahsaP gravatar imageMahsaP ( 2016-05-10 08:22:28 -0500 )edit

@Orhangazi44 is enable_odom_tf part of a controller driver for wheeled robots? Can I use it for legged robots?

MahsaP gravatar imageMahsaP ( 2016-05-10 08:26:31 -0500 )edit

I don't know, We are using this parameter to creating transform between base_footprint and odom for wheeled robots. But I think it must be work. Because other transformations are ok with base_link.

Orhan gravatar imageOrhan ( 2016-05-10 09:03:36 -0500 )edit
1

@Orhangazi44@MahsaP a minor correction: typically, the tf odom->base_link comes from the base controller of the robot, not the localization. localization reads this tf. odom->base_link is what the robot thinks it has moved based on motor encoders

mgruhler gravatar imagemgruhler ( 2016-05-11 01:15:44 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
2

answered 2016-05-12 08:20:42 -0500

mgruhler gravatar image

updated 2016-05-12 08:48:10 -0500

It seems you are just "routing" what you receive on the input odometry topic through to the tf broadcast. Do a rostopic echo on the input odometry and check what you have in the pose field. I'm guessing this is all zero (and 1 for the orientation.w).

You'll have to integrate the pose given the velocities of the input odometry topic. Check the lines 30-55 in the link you provided.


EDIT

Hmm, maybe some timing issue. I see that you declare the odom_broadcaster only in the callback. It could be, that you try to broadcast when this is not connected/advertised properly at the rosmaster, and as you do this in each step, this never gets properly sent out...

Can you try declaring it not in the callback but as a class member?

edit flag offensive delete link more

Comments

@mig actually I can see the updates in the pose, and I think updates are reasonable, but the tf is not broadcast for some reason. e.g. pose: position: x: 12.6596827148 orientation: w: 0.997481774674

MahsaP gravatar imageMahsaP ( 2016-05-12 08:28:23 -0500 )edit

@mig I did what you said. Now it's working :)

MahsaP gravatar imageMahsaP ( 2016-05-12 08:59:01 -0500 )edit

great :-) happy to help

mgruhler gravatar imagemgruhler ( 2016-05-12 09:04:32 -0500 )edit

Question Tools

2 followers

Stats

Asked: 2016-05-10 05:45:43 -0500

Seen: 3,462 times

Last updated: May 12 '16