54 lines
923 B
Plaintext
54 lines
923 B
Plaintext
/*
|
|
* rosserial Planar Odometry Example
|
|
*/
|
|
|
|
#include <ros.h>
|
|
#include <ros/time.h>
|
|
#include <tf/tf.h>
|
|
#include <tf/transform_broadcaster.h>
|
|
|
|
ros::NodeHandle nh;
|
|
|
|
geometry_msgs::TransformStamped t;
|
|
tf::TransformBroadcaster broadcaster;
|
|
|
|
double x = 1.0;
|
|
double y = 0.0;
|
|
double theta = 1.57;
|
|
|
|
char base_link[] = "/base_link";
|
|
char odom[] = "/odom";
|
|
|
|
void setup()
|
|
{
|
|
nh.initNode();
|
|
broadcaster.init(nh);
|
|
}
|
|
|
|
void loop()
|
|
{
|
|
// drive in a circle
|
|
double dx = 0.2;
|
|
double dtheta = 0.18;
|
|
x += cos(theta)*dx*0.1;
|
|
y += sin(theta)*dx*0.1;
|
|
theta += dtheta*0.1;
|
|
if(theta > 3.14)
|
|
theta=-3.14;
|
|
|
|
// tf odom->base_link
|
|
t.header.frame_id = odom;
|
|
t.child_frame_id = base_link;
|
|
|
|
t.transform.translation.x = x;
|
|
t.transform.translation.y = y;
|
|
|
|
t.transform.rotation = tf::createQuaternionFromYaw(theta);
|
|
t.header.stamp = nh.now();
|
|
|
|
broadcaster.sendTransform(t);
|
|
nh.spinOnce();
|
|
|
|
delay(10);
|
|
}
|