Fawkes API  Fawkes Development Version
cmdvel_thread.cpp
00001 /***************************************************************************
00002  *  cmdvel_plugin.cpp - Translate ROS Twist messages to Navgiator transrot
00003  *
00004  *  Created: Fri Jun 1 13:29:39 CEST 2012
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 "cmdvel_thread.h"
00022 #include <interfaces/MotorInterface.h>
00023 #include <ros/node_handle.h>
00024 #include <geometry_msgs/Twist.h>
00025 
00026 //using namespace ros;
00027 using namespace fawkes;
00028 
00029 /** @class ROSCmdVelThread "cmdvel_thread.h"
00030  * Thread to translate ROS twist messages to navigator transrot messages.
00031  * @author Sebastian Reuter
00032  */
00033 
00034 /** Constructor. */
00035 ROSCmdVelThread::ROSCmdVelThread()
00036   : Thread("ROSCmdVelThread", Thread::OPMODE_WAITFORWAKEUP)
00037 {
00038 }
00039 
00040 void
00041 ROSCmdVelThread::init()
00042 {
00043   std::string motor_if_id = config->get_string("/ros/cmdvel/motor_interface_id");
00044   motor_if_ = blackboard->open_for_reading<MotorInterface>(motor_if_id.c_str());
00045   sub_ = rosnode->subscribe("cmd_vel", 10, &ROSCmdVelThread::twist_msg_cb, this);
00046 }
00047 
00048 void
00049 ROSCmdVelThread::twist_msg_cb(const geometry_msgs::Twist::ConstPtr &msg)
00050 {
00051   send_transrot(msg->linear.x, msg->linear.y, msg->angular.z);
00052 }
00053 
00054 bool
00055 ROSCmdVelThread::prepare_finalize_user()
00056 {
00057   stop();
00058   return true;
00059 }
00060 
00061 void
00062 ROSCmdVelThread::finalize()
00063 {
00064   blackboard->close(motor_if_);
00065   sub_.shutdown();
00066 }
00067 
00068 void
00069 ROSCmdVelThread::send_transrot(float vx, float vy, float omega)
00070 {
00071   MotorInterface::TransRotMessage *msg =
00072     new MotorInterface::TransRotMessage(vx, vy, omega);
00073   motor_if_->msgq_enqueue(msg);
00074 }
00075 
00076 void
00077 ROSCmdVelThread::stop()
00078 {
00079   send_transrot(0., 0., 0.);
00080 }
00081 
00082 void
00083 ROSCmdVelThread::loop()
00084 {
00085 }