root / quad2 / arduino / src / ros_lib / examples / Odom / Odom.pde @ c1426757
History | View | Annotate | Download (923 Bytes)
1 |
/* |
---|---|
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 |
} |