Odom.cpp

Go to the documentation of this file.

/*
 * rosserial Planar Odometry Example
 */

#include <ros.h>
#include <ros/time.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>

ros::NodeHandle  nh;

geometry_msgs::TransformStamped t;
tf::TransformBroadcaster broadcaster;

double x = 1.0;
double y = 0.0;
double theta = 1.57;

char base_link[] = "/base_link";
char odom[] = "/odom";

int main(void) {
    nh.initNode();
    broadcaster.init(nh);


    while (1) {
        // drive in a circle
        double dx = 0.2;
        double dtheta = 0.18;
        x += cos(theta)*dx*0.1;
        y += sin(theta)*dx*0.1;
        theta += dtheta*0.1;
        if (theta > 3.14)
            theta=-3.14;

        // tf odom->base_link
        t.header.frame_id = odom;
        t.child_frame_id = base_link;

        t.transform.translation.x = x;
        t.transform.translation.y = y;

        t.transform.rotation = tf::createQuaternionFromYaw(theta);
        t.header.stamp = nh.now();

        broadcaster.sendTransform(t);
        nh.spinOnce();

        wait_ms(10);
    }
}