Fawkes API
Fawkes Development Version
|
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 }