0
$\begingroup$

Rosanswers logo

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.y = msg->pose.pose.orientation.y;
    odom_trans.transform.rotation.z = msg->pose.pose.orientation.z;
    odom_trans.transform.rotation.w = msg->pose.pose.orientation.w;

    odom_broadcaster.sendTransform(odom_trans);

    //next, we'll publish the odometry message over ROS
    odom.header.stamp = current_time;
    odom.header.frame_id = "odom";
    odom.child_frame_id = "base_link";

    //set the position
    odom.pose.pose.position.x = msg->pose.pose.position.x;
    odom.pose.pose.position.y = msg->pose.pose.position.y;
    odom.pose.pose.position.z = msg->pose.pose.position.z;
    odom.pose.pose.orientation.x = msg->pose.pose.orientation.x;
    odom.pose.pose.orientation.y = msg->pose.pose.orientation.y;
    odom.pose.pose.orientation.z = msg->pose.pose.orientation.z;
    odom.pose.pose.orientation.w = msg->pose.pose.orientation.w;

    odom.twist.twist.linear.x = msg->twist.twist.linear.x;
    odom.twist.twist.linear.y = msg->twist.twist.linear.y;
    odom.twist.twist.angular.z = msg->twist.twist.linear.z;
}

void OdomTf::publishOdom()
{
    data_pub_.publish(odom);
}

int main(int argc, char** argv){
    ros::init(argc, argv, "robot_tf_publisher");
    OdomTf odomtf;
    ros::Rate loop_rate(100);
    while(ros::ok())
    {
        odomtf.publishOdom();
        ROS_INFO("The Pose is updated");
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}

Does anyone know what changes are necessary to solve this problem?

Thank you.


Originally posted by MahsaP on ROS Answers with karma: 79 on 2016-05-10

Post score: 0


Original comments

Comment by Orhan on 2016-05-10:
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.

Comment by MahsaP on 2016-05-10:
@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?

Comment by Orhan on 2016-05-10:
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:

Comment by Orhan on 2016-05-10:
<node name="controller_spawner" pkg="controller_manager" type="spawner" args="mrp2_joint_publisher mobile_base_controller --shutdown-timeout 1" />

Comment by MahsaP on 2016-05-10:
@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.

Comment by MahsaP on 2016-05-10:
@Orhangazi44 is enable_odom_tf part of a controller driver for wheeled robots? Can I use it for legged robots?

Comment by Orhan on 2016-05-10:
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.

Comment by mgruhler on 2016-05-11:
@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

Comment by Orhan on 2016-05-11:
I'm partly new to ROS and continuing to learn. Thanks a lot for correction Sir!

Comment by MahsaP on 2016-05-11:
@mig Thanks for your explanation. You know I just added the above plugins to .gazebo file. When I echo the topics the data from each is updated. But, I don't know how to obtain the link between for example odom and base_link. The robot walks correctly in gazebo, but in rviz base_link is fixed.

Comment by MahsaP on 2016-05-11:
I have found this explanation from here: The transform from odom to base_link is computed and broadcast by one of the odometry sources. So, the odom topic is published from the plugin, but the connection between links have the problem.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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?


Originally posted by mgruhler with karma: 12390 on 2016-05-12

This answer was ACCEPTED on the original site

Post score: 2


Original comments

Comment by MahsaP on 2016-05-12:
@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

Comment by MahsaP on 2016-05-12:
@mig I did what you said. Now it's working :)

Comment by mgruhler on 2016-05-12:
great :-) happy to help

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.