nucho / Mbed 2 deprecated rosserial_mbed

This program is porting rosserial_arduino for mbed http://www.ros.org/wiki/rosserial_arduino This program supported the revision of 169 of rosserial. This program contains an example.

Dependencies:   rosserial_mbed_lib mbed Servo

examples/Odom.cpp

Committer:
nucho
Date:
2012-02-29
Revision:
4:2cbca0ac2569
Parent:
3:dff241b66f84

File content as of revision 4:2cbca0ac2569:

//#define COMPILE_ODOM_CODE_ROSSERIAL
#ifdef  COMPILE_ODOM_CODE_ROSSERIAL

/*
 * 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);
    }
}
#endif