odometry_publisher.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2009, Willow Garage, Inc.
00006 *  All rights reserved.
00007 *
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  are met:
00011 *
00012 *   * Redistributions of source code must retain the above copyright
00013 *     notice, this list of conditions and the following disclaimer.
00014 *   * Redistributions in binary form must reproduce the above
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of Willow Garage, Inc. nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 *
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *
00035 * Author: Eitan Marder-Eppstein
00036 *********************************************************************/
00037 #include <ros/ros.h>
00038 #include <tf/transform_broadcaster.h>
00039 #include <nav_msgs/Odometry.h>
00040 
00041 int main(int argc, char** argv){
00042   ros::init(argc, argv, "odometry_publisher");
00043 
00044   ros::NodeHandle n;
00045   ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
00046   tf::TransformBroadcaster odom_broadcaster;
00047 
00048   double x = 0.0;
00049   double y = 0.0;
00050   double th = 0.0;
00051 
00052   double vx = 0.1;
00053   double vy = -0.1;
00054   double vth = 0.1;
00055 
00056   ros::Time current_time, last_time;
00057   current_time = ros::Time::now();
00058   last_time = ros::Time::now();
00059 
00060   ros::Rate r(1.0);
00061   while(n.ok()){
00062     current_time = ros::Time::now();
00063 
00064     //compute odometry in a typical way given the velocities of the robot
00065     double dt = (current_time - last_time).toSec();
00066     double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
00067     double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
00068     double delta_th = vth * dt;
00069 
00070     x += delta_x;
00071     y += delta_y;
00072     th += delta_th;
00073 
00074     //since all odometry is 6DOF we'll need a quaternion created from yaw
00075     geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
00076 
00077     //first, we'll publish the transform over tf
00078     geometry_msgs::TransformStamped odom_trans;
00079     odom_trans.header.stamp = current_time;
00080     odom_trans.header.frame_id = "odom";
00081     odom_trans.child_frame_id = "base_link";
00082 
00083     odom_trans.transform.translation.x = x;
00084     odom_trans.transform.translation.y = y;
00085     odom_trans.transform.translation.z = 0.0;
00086     odom_trans.transform.rotation = odom_quat;
00087 
00088     //send the transform
00089     odom_broadcaster.sendTransform(odom_trans);
00090 
00091     //next, we'll publish the odometry message over ROS
00092     nav_msgs::Odometry odom;
00093     odom.header.stamp = current_time;
00094     odom.header.frame_id = "odom";
00095     odom.child_frame_id = "base_link";
00096 
00097     //set the position
00098     odom.pose.pose.position.x = x;
00099     odom.pose.pose.position.y = y;
00100     odom.pose.pose.position.z = 0.0;
00101     odom.pose.pose.orientation = odom_quat;
00102 
00103     //set the velocity
00104     odom.twist.twist.linear.x = vx;
00105     odom.twist.twist.linear.y = vy;
00106     odom.twist.twist.angular.z = vth;
00107 
00108     //publish the message
00109     odom_pub.publish(odom);
00110 
00111     last_time = current_time;
00112     r.sleep();
00113   }
00114 }


odometry_publisher_tutorial
Author(s): Eitan Marder-Eppstein
autogenerated on Thu Jan 2 2014 11:31:52