Odom.cpp
Go to the documentation of this file.
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 
11 
12 geometry_msgs::TransformStamped t;
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 int main(void) {
23  nh.initNode();
24  broadcaster.init(nh);
25 
26 
27  while (1) {
28  // drive in a circle
29  double dx = 0.2;
30  double dtheta = 0.18;
31  x += cos(theta)*dx*0.1;
32  y += sin(theta)*dx*0.1;
33  theta += dtheta*0.1;
34  if (theta > 3.14)
35  theta=-3.14;
36 
37  // tf odom->base_link
38  t.header.frame_id = odom;
39  t.child_frame_id = base_link;
40 
41  t.transform.translation.x = x;
42  t.transform.translation.y = y;
43 
44  t.transform.rotation = tf::createQuaternionFromYaw(theta);
45  t.header.stamp = nh.now();
46 
47  broadcaster.sendTransform(t);
48  nh.spinOnce();
49 
50  wait_ms(10);
51  }
52 }
53 
int main(void)
Definition: Odom.cpp:22
char odom[]
Definition: Odom.cpp:20
double theta
Definition: Odom.cpp:17
ros::NodeHandle nh
Definition: Odom.cpp:10
double y
Definition: Odom.cpp:16
geometry_msgs::TransformStamped t
Definition: Odom.cpp:12
static Quaternion createQuaternionFromYaw(double yaw)
double x
Definition: Odom.cpp:15
void sendTransform(const StampedTransform &transform)
char base_link[]
Definition: Odom.cpp:19
tf::TransformBroadcaster broadcaster
Definition: Odom.cpp:13


rosserial_mbed
Author(s): Gary Servin
autogenerated on Mon Jun 10 2019 14:53:26