Fawkes API  Fawkes Development Version
cmdvel_thread.cpp
1 /***************************************************************************
2  * cmdvel_plugin.cpp - Translate ROS Twist messages to Navgiator transrot
3  *
4  * Created: Fri Jun 1 13:29:39 CEST 2012
5  * Copyright 2012 Sebastian Reuter
6  ****************************************************************************/
7 
8 /* This program is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU Library General Public License for more details.
17  *
18  * Read the full text in the LICENSE.GPL file in the doc directory.
19  */
20 
21 #include "cmdvel_thread.h"
22 
23 #include <geometry_msgs/Twist.h>
24 #include <interfaces/MotorInterface.h>
25 #include <ros/node_handle.h>
26 
27 //using namespace ros;
28 using namespace fawkes;
29 
30 /** @class ROSCmdVelThread "cmdvel_thread.h"
31  * Thread to translate ROS twist messages to navigator transrot messages.
32  * @author Sebastian Reuter
33  */
34 
35 /** Constructor. */
36 ROSCmdVelThread::ROSCmdVelThread() : Thread("ROSCmdVelThread", Thread::OPMODE_WAITFORWAKEUP)
37 {
38 }
39 
40 void
42 {
43  std::string motor_if_id = config->get_string("/ros/cmdvel/motor_interface_id");
44  motor_if_ = blackboard->open_for_reading<MotorInterface>(motor_if_id.c_str());
45  sub_ = rosnode->subscribe("cmd_vel", 10, &ROSCmdVelThread::twist_msg_cb, this);
46 }
47 
48 void
49 ROSCmdVelThread::twist_msg_cb(const geometry_msgs::Twist::ConstPtr &msg)
50 {
51  send_transrot(msg->linear.x, msg->linear.y, msg->angular.z);
52 }
53 
54 bool
56 {
57  stop();
58  return true;
59 }
60 
61 void
63 {
64  blackboard->close(motor_if_);
65  sub_.shutdown();
66 }
67 
68 void
69 ROSCmdVelThread::send_transrot(float vx, float vy, float omega)
70 {
71  if (motor_if_->has_writer()) {
73  motor_if_->msgq_enqueue(msg);
74  } else {
75  logger->log_warn(name(), "Cannot send transrot, no writer on motor interface");
76  }
77 }
78 
79 void
80 ROSCmdVelThread::stop()
81 {
82  send_transrot(0., 0., 0.);
83 }
84 
85 void
87 {
88 }
ROSCmdVelThread::init
virtual void init()
Initialize the thread.
Definition: cmdvel_thread.cpp:41
fawkes::MotorInterface::TransRotMessage
Definition: MotorInterface.h:366
ROSCmdVelThread::ROSCmdVelThread
ROSCmdVelThread()
Constructor.
Definition: cmdvel_thread.cpp:36
fawkes::ROSAspect::rosnode
LockPtr< ros::NodeHandle > rosnode
Definition: ros.h:47
fawkes::MotorInterface
Definition: MotorInterface.h:39
fawkes::Thread::name
const char * name() const
Definition: thread.h:100
ROSCmdVelThread::finalize
virtual void finalize()
Finalize the thread.
Definition: cmdvel_thread.cpp:62
fawkes::LoggingAspect::logger
Logger * logger
Definition: logging.h:53
fawkes::BlackBoard::close
virtual void close(Interface *interface)=0
fawkes
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
ROSCmdVelThread::loop
virtual void loop()
Code to execute in the thread.
Definition: cmdvel_thread.cpp:86
fawkes::Interface::has_writer
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:819
fawkes::ConfigurableAspect::config
Configuration * config
Definition: configurable.h:53
ROSCmdVelThread::prepare_finalize_user
virtual bool prepare_finalize_user()
Prepare finalization user implementation.
Definition: cmdvel_thread.cpp:55
fawkes::Thread
Definition: thread.h:45
fawkes::BlackBoardAspect::blackboard
BlackBoard * blackboard
Definition: blackboard.h:49
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
fawkes::BlackBoard::open_for_reading
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
fawkes::Interface::msgq_enqueue
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
Definition: interface.cpp:884