Statistics
| Branch: | Revision:

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
}