Fawkes API  Fawkes Development Version
odometry_thread.cpp
00001 /***************************************************************************
00002  *  odometry_thread.h - Thread to publish odometry to ROS
00003  *
00004  *  Created: Fri Jun 1 13:29:39 CEST
00005  *  Copyright  2012  Sebastian Reuter
00006  ****************************************************************************/
00007 
00008 /*  This program is free software; you can redistribute it and/or modify
00009  *  it under the terms of the GNU General Public License as published by
00010  *  the Free Software Foundation; either version 2 of the License, or
00011  *  (at your option) any later version.
00012  *
00013  *  This program is distributed in the hope that it will be useful,
00014  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00015  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00016  *  GNU Library General Public License for more details.
00017  *
00018  *  Read the full text in the LICENSE.GPL file in the doc directory.
00019  */
00020 
00021 #include "odometry_thread.h"
00022 #include <interfaces/MotorInterface.h>
00023 #include <nav_msgs/Odometry.h>
00024 #include <tf/types.h>
00025 
00026 using namespace fawkes;
00027 
00028 /** @class ROSOdometryThread "odometry_thread.h"
00029  * Thread to publish odometry to ROS.
00030  * @author Sebastian Reuter
00031  */
00032 
00033 /** Constructor. */
00034 ROSOdometryThread::ROSOdometryThread()
00035   : Thread("OdometryThread", Thread::OPMODE_WAITFORWAKEUP),
00036     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_POST_LOOP)
00037 {
00038 }
00039 
00040 void
00041 ROSOdometryThread::init()
00042 {
00043   std::string motor_if_id = config->get_string("/ros/odometry/motor_interface_id");
00044   cfg_odom_frame_id_ = config->get_string("/ros/odometry/odom_frame_id");
00045   cfg_base_frame_id_ = config->get_string("/ros/odometry/base_frame_id");
00046   motor_if_ = blackboard->open_for_reading<MotorInterface>(motor_if_id.c_str());
00047   pub_ = rosnode->advertise<nav_msgs::Odometry>("odom", 100, this);
00048 }
00049 
00050 void
00051 ROSOdometryThread::finalize()
00052 {
00053   blackboard->close(motor_if_);
00054   pub_.shutdown();
00055 }
00056 
00057 
00058 void
00059 ROSOdometryThread::publish_odom()
00060 {
00061   nav_msgs::Odometry odom;
00062   //Header
00063   odom.header.stamp = ros::Time::now();
00064   odom.header.frame_id = cfg_odom_frame_id_;
00065   //Position
00066   odom.pose.pose.position.x = (double)motor_if_->odometry_position_x();
00067   odom.pose.pose.position.y = (double) motor_if_->odometry_position_y();
00068   odom.pose.pose.position.z = 0.0;
00069   fawkes::tf::Quaternion q(motor_if_->odometry_orientation(), 0, 0);
00070   geometry_msgs::Quaternion odom_quat;
00071   odom_quat.x = q.x();
00072   odom_quat.y = q.y();
00073   odom_quat.z = q.z();
00074   odom_quat.w = q.w();
00075   odom.pose.pose.orientation = odom_quat;
00076   //Motion
00077   odom.child_frame_id = cfg_base_frame_id_;
00078   odom.twist.twist.linear.x = (double)motor_if_->vx();
00079   odom.twist.twist.linear.y = (double)motor_if_->vy();
00080   odom.twist.twist.angular.z = (double)motor_if_->omega();
00081   pub_.publish(odom);
00082 }
00083 
00084 void  ROSOdometryThread::loop() {
00085   motor_if_->read();
00086   publish_odom();
00087 }