nucho
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@4:2cbca0ac2569, 2012-02-29 (annotated)
- Committer:
- nucho
- Date:
- Wed Feb 29 23:02:12 2012 +0000
- Revision:
- 4:2cbca0ac2569
- Parent:
- 3:dff241b66f84
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| nucho | 3:dff241b66f84 | 1 | //#define COMPILE_ODOM_CODE_ROSSERIAL |
| nucho | 3:dff241b66f84 | 2 | #ifdef COMPILE_ODOM_CODE_ROSSERIAL |
| nucho | 3:dff241b66f84 | 3 | |
| nucho | 3:dff241b66f84 | 4 | /* |
| nucho | 3:dff241b66f84 | 5 | * rosserial Planar Odometry Example |
| nucho | 3:dff241b66f84 | 6 | */ |
| nucho | 3:dff241b66f84 | 7 | |
| nucho | 3:dff241b66f84 | 8 | #include <ros.h> |
| nucho | 3:dff241b66f84 | 9 | #include <ros/time.h> |
| nucho | 3:dff241b66f84 | 10 | #include <tf/tf.h> |
| nucho | 3:dff241b66f84 | 11 | #include <tf/transform_broadcaster.h> |
| nucho | 3:dff241b66f84 | 12 | |
| nucho | 3:dff241b66f84 | 13 | ros::NodeHandle nh; |
| nucho | 3:dff241b66f84 | 14 | |
| nucho | 3:dff241b66f84 | 15 | geometry_msgs::TransformStamped t; |
| nucho | 3:dff241b66f84 | 16 | tf::TransformBroadcaster broadcaster; |
| nucho | 3:dff241b66f84 | 17 | |
| nucho | 3:dff241b66f84 | 18 | double x = 1.0; |
| nucho | 3:dff241b66f84 | 19 | double y = 0.0; |
| nucho | 3:dff241b66f84 | 20 | double theta = 1.57; |
| nucho | 3:dff241b66f84 | 21 | |
| nucho | 3:dff241b66f84 | 22 | char base_link[] = "/base_link"; |
| nucho | 3:dff241b66f84 | 23 | char odom[] = "/odom"; |
| nucho | 3:dff241b66f84 | 24 | |
| nucho | 3:dff241b66f84 | 25 | int main(void) { |
| nucho | 3:dff241b66f84 | 26 | nh.initNode(); |
| nucho | 3:dff241b66f84 | 27 | broadcaster.init(nh); |
| nucho | 3:dff241b66f84 | 28 | |
| nucho | 3:dff241b66f84 | 29 | |
| nucho | 3:dff241b66f84 | 30 | while (1) { |
| nucho | 3:dff241b66f84 | 31 | // drive in a circle |
| nucho | 3:dff241b66f84 | 32 | double dx = 0.2; |
| nucho | 3:dff241b66f84 | 33 | double dtheta = 0.18; |
| nucho | 3:dff241b66f84 | 34 | x += cos(theta)*dx*0.1; |
| nucho | 3:dff241b66f84 | 35 | y += sin(theta)*dx*0.1; |
| nucho | 3:dff241b66f84 | 36 | theta += dtheta*0.1; |
| nucho | 3:dff241b66f84 | 37 | if (theta > 3.14) |
| nucho | 3:dff241b66f84 | 38 | theta=-3.14; |
| nucho | 3:dff241b66f84 | 39 | |
| nucho | 3:dff241b66f84 | 40 | // tf odom->base_link |
| nucho | 3:dff241b66f84 | 41 | t.header.frame_id = odom; |
| nucho | 3:dff241b66f84 | 42 | t.child_frame_id = base_link; |
| nucho | 3:dff241b66f84 | 43 | |
| nucho | 3:dff241b66f84 | 44 | t.transform.translation.x = x; |
| nucho | 3:dff241b66f84 | 45 | t.transform.translation.y = y; |
| nucho | 3:dff241b66f84 | 46 | |
| nucho | 3:dff241b66f84 | 47 | t.transform.rotation = tf::createQuaternionFromYaw(theta); |
| nucho | 3:dff241b66f84 | 48 | t.header.stamp = nh.now(); |
| nucho | 3:dff241b66f84 | 49 | |
| nucho | 3:dff241b66f84 | 50 | broadcaster.sendTransform(t); |
| nucho | 3:dff241b66f84 | 51 | nh.spinOnce(); |
| nucho | 3:dff241b66f84 | 52 | |
| nucho | 3:dff241b66f84 | 53 | wait_ms(10); |
| nucho | 3:dff241b66f84 | 54 | } |
| nucho | 3:dff241b66f84 | 55 | } |
| nucho | 3:dff241b66f84 | 56 | #endif |