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
Diff: examples/Odom.cpp
- Revision:
- 3:dff241b66f84
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/examples/Odom.cpp Sat Nov 12 23:53:04 2011 +0000 @@ -0,0 +1,56 @@ +//#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 \ No newline at end of file