ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

How can I use odometry + AMCL?

asked 2018-12-27 06:35:51 -0500

stevemartin gravatar image

updated 2018-12-28 04:58:34 -0500

The first issue I have is that I hardcode the initial odometry position at the very beginning to be at 0,0. Code is very simple and typical:

int main(int argc, char** argv){
    ros::init(argc, argv, "odometry_publisher");

    ros::NodeHandle n;
    ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 10);
    tf2_ros::TransformBroadcaster odom_broadcaster;

    // API

    double x = 1.0;
    double y = 6.0;
    double th = 0.0;

    double vx = 0.0;
    double vy = 0.0;
    double vth = 0.0;

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

    // Variables
    geometry_msgs::TransformStamped odom_trans;
    tf2::Quaternion q;
    nav_msgs::Odometry odom;

    ros::Rate r(50.0);

    while(n.ok()){

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

        double dt = (current_time-last_time).toSec();
        double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
        double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
        double delta_th = vth * dt;

        x += delta_x;
        y += delta_y;
        th += delta_th;

        // Setting the transform stamped messages:
        q.setRPY(0,0,th);
        odom_trans.header.stamp = current_time;
        odom_trans.header.frame_id = "odom";
        odom_trans.child_frame_id = "base_footprint";

        odom_trans.transform.translation.x = x;
        odom_trans.transform.translation.y = y;
        odom_trans.transform.translation.z = 0.0;
        odom_trans.transform.rotation.x = q.x();
        odom_trans.transform.rotation.y = q.y();
        odom_trans.transform.rotation.z = q.z();
        odom_trans.transform.rotation.w = q.w();

        // Send the transform:
        odom_broadcaster.sendTransform(odom_trans);

        // Send the odometry messages to ROS:
        odom.header.stamp = current_time;
        odom.header.frame_id = "odom";
        // Here, take the positions from the wheelme backend whenever it reaches and send it to ros:
        // Replace the xyz values by path values in for loop
        odom.pose.pose.position.x = x;
        odom.pose.pose.position.y = y;
        odom.pose.pose.position.z = 0.0;
        odom.pose.pose.orientation.x = q.x();
        odom.pose.pose.orientation.x = q.y();
        odom.pose.pose.orientation.x = q.z();
        odom.pose.pose.orientation.x = q.w();


        // This values will be always zero until wheelme will start publishing its velocities:
        odom.child_frame_id = "base_link";
        odom.twist.twist.linear.x = vx;
        odom.twist.twist.linear.y = vy;
        odom.twist.twist.angular.z  = vth;

        // Now publish all that to odom topic:
        odom_pub.publish(odom);
        last_time = current_time;
        r.sleep();

    }

}

After that, I try to initialize the AMCL using my Lidar and I can see that the position of the robot jumps between the harcoded inital odom values and predicted AMCL values.

How can I combine them both? So that the AMCL corrects the position of the robot and odometry frame accepts that change?

EDIT: adding a tf tree

image description

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2018-12-27 08:19:49 -0500

vivek rk gravatar image

I think what you have done is that the map frame is set to /odom and using AMCL to provide the /odom -> /base_link transformation and using the script to provide the same transformation with a different value.

This code publishes the odometry between /base_link and /odom frame. AMCL can be configured to provide the transform between /odom and /map (/world or whatever the map frame is).

In the above mentioned scenario the /base_link will not jump around with respect to the /odom frame anymore.

Could you share your tf tree structure to make sure it isnt the problem

edit flag offensive delete link more

Comments

Thanks for the answer. I will check tomorrow and will let you know

stevemartin gravatar image stevemartin  ( 2018-12-27 10:43:01 -0500 )edit

I have added the frames.pdf file. I have removed the broadcast from map to odom and the robot stopped to jump but now the odometry is still hardcoded with initial values and ignores AMCL position.

stevemartin gravatar image stevemartin  ( 2018-12-28 04:59:31 -0500 )edit

Do you still have the node /broadcaster_frame running? Could you share the launch file parameters. The ones you use when running AMCL ? AMCL should publish the values in the topic /tf Unless it publishes onto /tf topic it wont register in RVIZ

vivek rk gravatar image vivek rk  ( 2019-01-07 01:10:30 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2018-12-27 06:35:51 -0500

Seen: 719 times

Last updated: Dec 28 '18