odometry_helper_ros.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2008, 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: TKruse
00036  *********************************************************************/
00037 #include <base_local_planner/odometry_helper_ros.h>
00038 
00039 namespace base_local_planner {
00040 
00041 OdometryHelperRos::OdometryHelperRos(std::string odom_topic) {
00042   setOdomTopic( odom_topic );
00043 }
00044 
00045 void OdometryHelperRos::odomCallback(const nav_msgs::Odometry::ConstPtr& msg) {
00046     ROS_INFO_ONCE("odom received!");
00047 
00048   //we assume that the odometry is published in the frame of the base
00049   boost::mutex::scoped_lock lock(odom_mutex_);
00050   base_odom_.twist.twist.linear.x = msg->twist.twist.linear.x;
00051   base_odom_.twist.twist.linear.y = msg->twist.twist.linear.y;
00052   base_odom_.twist.twist.angular.z = msg->twist.twist.angular.z;
00053   base_odom_.child_frame_id = msg->child_frame_id;
00054 //  ROS_DEBUG_NAMED("dwa_local_planner", "In the odometry callback with velocity values: (%.2f, %.2f, %.2f)",
00055 //      base_odom_.twist.twist.linear.x, base_odom_.twist.twist.linear.y, base_odom_.twist.twist.angular.z);
00056 }
00057 
00058 //copy over the odometry information
00059 void OdometryHelperRos::getOdom(nav_msgs::Odometry& base_odom) {
00060   boost::mutex::scoped_lock lock(odom_mutex_);
00061   base_odom = base_odom_;
00062 }
00063 
00064 
00065 void OdometryHelperRos::getRobotVel(tf::Stamped<tf::Pose>& robot_vel) {
00066   // Set current velocities from odometry
00067   geometry_msgs::Twist global_vel;
00068   {
00069     boost::mutex::scoped_lock lock(odom_mutex_);
00070     global_vel.linear.x = base_odom_.twist.twist.linear.x;
00071     global_vel.linear.y = base_odom_.twist.twist.linear.y;
00072     global_vel.angular.z = base_odom_.twist.twist.angular.z;
00073 
00074     robot_vel.frame_id_ = base_odom_.child_frame_id;
00075   }
00076   robot_vel.setData(tf::Transform(tf::createQuaternionFromYaw(global_vel.angular.z), tf::Vector3(global_vel.linear.x, global_vel.linear.y, 0)));
00077   robot_vel.stamp_ = ros::Time();
00078 }
00079 
00080 void OdometryHelperRos::setOdomTopic(std::string odom_topic)
00081 {
00082   if( odom_topic != odom_topic_ )
00083   {
00084     odom_topic_ = odom_topic;
00085 
00086     if( odom_topic_ != "" )
00087     {
00088       ros::NodeHandle gn;
00089       odom_sub_ = gn.subscribe<nav_msgs::Odometry>( odom_topic_, 1, boost::bind( &OdometryHelperRos::odomCallback, this, _1 ));
00090     }
00091     else
00092     {
00093       odom_sub_.shutdown();
00094     }
00095   }
00096 }
00097 
00098 } /* namespace base_local_planner */


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Thu Aug 27 2015 14:07:08