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