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;
28using 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. */
36ROSCmdVelThread::ROSCmdVelThread() : Thread("ROSCmdVelThread", Thread::OPMODE_WAITFORWAKEUP)
37{
38}
39
40void
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
48void
49ROSCmdVelThread::twist_msg_cb(const geometry_msgs::Twist::ConstPtr &msg)
50{
51 send_transrot(msg->linear.x, msg->linear.y, msg->angular.z);
52}
53
54bool
56{
57 stop();
58 return true;
59}
60
61void
63{
64 blackboard->close(motor_if_);
65 sub_.shutdown();
66}
67
68void
69ROSCmdVelThread::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
79void
80ROSCmdVelThread::stop()
81{
82 send_transrot(0., 0., 0.);
83}
84
85void
87{
88}
ROSCmdVelThread()
Constructor.
virtual void loop()
Code to execute in the thread.
virtual bool prepare_finalize_user()
Prepare finalization user implementation.
virtual void finalize()
Finalize the thread.
virtual void init()
Initialize the thread.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
virtual void close(Interface *interface)=0
Close interface.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
unsigned int msgq_enqueue(Message *message, bool proxy=false)
Enqueue message at end of queue.
Definition: interface.cpp:915
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:848
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
TransRotMessage Fawkes BlackBoard Interface Message.
MotorInterface Fawkes BlackBoard Interface.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Definition: ros.h:47
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
Fawkes library namespace.