root / quad2 / arduino / src / ros_lib / examples / Odom / Odom.pde @ c1426757
History | View | Annotate | Download (923 Bytes)
1 | c1426757 | Tom Mullins | /* |
---|---|---|---|
2 | * rosserial Planar Odometry Example |
||
3 | */ |
||
4 | |||
5 | #include <ros.h> |
||
6 | #include <ros/time.h> |
||
7 | #include <tf/tf.h> |
||
8 | #include <tf/transform_broadcaster.h> |
||
9 | |||
10 | ros::NodeHandle nh; |
||
11 | |||
12 | geometry_msgs::TransformStamped t; |
||
13 | tf::TransformBroadcaster broadcaster; |
||
14 | |||
15 | double x = 1.0; |
||
16 | double y = 0.0; |
||
17 | double theta = 1.57; |
||
18 | |||
19 | char base_link[] = "/base_link"; |
||
20 | char odom[] = "/odom"; |
||
21 | |||
22 | void setup() |
||
23 | { |
||
24 | nh.initNode(); |
||
25 | broadcaster.init(nh); |
||
26 | } |
||
27 | |||
28 | void loop() |
||
29 | { |
||
30 | // drive in a circle |
||
31 | double dx = 0.2; |
||
32 | double dtheta = 0.18; |
||
33 | x += cos(theta)*dx*0.1; |
||
34 | y += sin(theta)*dx*0.1; |
||
35 | theta += dtheta*0.1; |
||
36 | if(theta > 3.14) |
||
37 | theta=-3.14; |
||
38 | |||
39 | // tf odom->base_link |
||
40 | t.header.frame_id = odom; |
||
41 | t.child_frame_id = base_link; |
||
42 | |||
43 | t.transform.translation.x = x; |
||
44 | t.transform.translation.y = y; |
||
45 | |||
46 | t.transform.rotation = tf::createQuaternionFromYaw(theta); |
||
47 | t.header.stamp = nh.now(); |
||
48 | |||
49 | broadcaster.sendTransform(t); |
||
50 | nh.spinOnce(); |
||
51 | |||
52 | delay(10); |
||
53 | } |