Akash Vibhute / mbed_roshydro_test

ros_serial for mbed updated to work with ROS Hydro. Library still needs to be debugged

Dependencies:   rosserial_hydro

Library and program still under development!

examples/Odom.cpp@1:225298843679, 2015-02-15 (annotated)

Committer:
akashvibhute
Date:
Sun Feb 15 10:56:37 2015 +0000
Revision:
1:225298843679
Parent:
0:cb1dffdc7d05 
rosserial_mbed for hydro testing

Who changed what in which revision?

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