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