Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
act_thread.cpp
1 
2 /***************************************************************************
3  * act_thread.cpp - Robotino act thread
4  *
5  * Created: Sun Nov 13 16:07:40 2011
6  * Copyright 2011 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "act_thread.h"
24 #include "sensor_thread.h"
25 
26 #include <interfaces/MotorInterface.h>
27 #include <utils/math/angle.h>
28 
29 #include <rec/robotino/com/Com.h>
30 #include <rec/robotino/com/OmniDrive.h>
31 #include <rec/iocontrol/remotestate/SensorState.h>
32 
33 
34 using namespace fawkes;
35 
36 /** @class RobotinoActThread "act_thread.h"
37  * Robotino act hook integration thread.
38  * This thread integrates into the Fawkes main loop at the ACT hook and
39  * executes motion commands.
40  * @author Tim Niemueller
41  */
42 
43 /** Constructor.
44  * @param sensor_thread sensor thread from which to get the Com instance
45  */
47  : Thread("RobotinoActThread", Thread::OPMODE_WAITFORWAKEUP),
48 #ifdef HAVE_TF
49  TransformAspect(TransformAspect::ONLY_PUBLISHER, "Robotino Odometry"),
50 #endif
52 {
53  sensor_thread_ = sensor_thread;
54 }
55 
56 
57 void
59 {
60  com_ = sensor_thread_->com_;
61  omni_drive_ = new rec::robotino::com::OmniDrive();
62 
63  last_seqnum_ = 0;
64 
65  // reset odometry once on startup
66  rec::iocontrol::remotestate::SetState set_state;
67  set_state.setOdometry = true;
68  set_state.odometryX = set_state.odometryY = set_state.odometryPhi = 0;
69  com_->setSetState(set_state);
70 
71  motor_if_ = blackboard->open_for_writing<MotorInterface>("Robotino");
72 }
73 
74 
75 void
77 {
78  blackboard->close(motor_if_);
79  if (com_->isConnected()) {
80  rec::iocontrol::remotestate::SetState set_state;
81  set_state.speedSetPoint[0] = 0.;
82  set_state.speedSetPoint[1] = 0.;
83  set_state.speedSetPoint[2] = 0.;
84 
85  com_->setSetState( set_state );
86  }
87  com_ = NULL;
88 }
89 
90 
91 void
93 {
94  if (com_->isConnected()) {
95  rec::iocontrol::remotestate::SetState set_state;
96  bool send_set_state = false;
97  while (! motor_if_->msgq_empty()) {
98 
100  motor_if_->msgq_first_safe(msg))
101  {
102  float m1, m2, m3;
103  omni_drive_->project(&m1, &m2, &m3,
104  msg->vx() * 1000., msg->vy() * 1000.,
105  rad2deg(msg->omega()));
106 
107  set_state.speedSetPoint[0] = m1;
108  set_state.speedSetPoint[1] = m2;
109  set_state.speedSetPoint[2] = m3;
110  send_set_state = true;
111  }
113  {
114  set_state.setOdometry = true;
115  set_state.odometryX = set_state.odometryY = set_state.odometryPhi = 0;
116  send_set_state = true;
117  }
118 
119  motor_if_->msgq_pop();
120  }
121 
122  if (send_set_state) com_->setSetState(set_state);
123 
124  rec::iocontrol::remotestate::SensorState sensor_state = com_->sensorState();
125  if (sensor_state.sequenceNumber != last_seqnum_) {
126  float vx, vy, omega;
127  omni_drive_->unproject(&vx, &vy, &omega,
128  sensor_state.actualVelocity[0],
129  sensor_state.actualVelocity[1],
130  sensor_state.actualVelocity[2]);
131 
132  // div by 1000 to convert from mm to m
133  motor_if_->set_vx(vx / 1000.);
134  motor_if_->set_vy(vy / 1000.);
135  motor_if_->set_omega(deg2rad(omega));
136 
137  motor_if_->set_odometry_position_x(sensor_state.odometryX / 1000.f);
138  motor_if_->set_odometry_position_y(sensor_state.odometryY / 1000.f);
139  motor_if_->set_odometry_orientation(deg2rad(sensor_state.odometryPhi));
140  motor_if_->write();
141 
142 #ifdef HAVE_TF
143  fawkes::Time now(clock);
144 
145  tf::Transform t(tf::Quaternion(tf::Vector3(0,0,1),
146  deg2rad(sensor_state.odometryPhi)),
147  tf::Vector3(sensor_state.odometryX / 1000.f,
148  sensor_state.odometryY / 1000.f,
149  0));
150 
151  tf_publisher->send_transform(t, now, "/robotino_odometry", "/base_link");
152 #endif
153 
154  last_seqnum_ = sensor_state.sequenceNumber;
155  }
156 
157  } else {
158  if (! motor_if_->msgq_empty()) {
159  logger->log_warn(name(), "Motor commands received while not connected");
160  motor_if_->msgq_flush();
161  }
162  }
163 }
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:972
TransRotMessage Fawkes BlackBoard Interface Message.
void set_odometry_position_y(const float new_odometry_position_y)
Set odometry_position_y value.
void set_odometry_orientation(const float new_odometry_orientation)
Set odometry_orientation value.
A class for handling time.
Definition: time.h:91
Thread class encapsulation of pthreads.
Definition: thread.h:42
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:495
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
virtual Interface * open_for_writing(const char *interface_type, const char *identifier)=0
Open interface for writing.
RobotinoActThread(RobotinoSensorThread *sensor_thread)
Constructor.
Definition: act_thread.cpp:46
Thread aspect to access the transform system.
Definition: tf.h:38
ResetOdometryMessage Fawkes BlackBoard Interface Message.
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:45
Thread aspect to use blocked timing.
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1118
void set_vy(const float new_vy)
Set vy value.
void set_vx(const float new_vx)
Set vx value.
void set_omega(const float new_omega)
Set omega value.
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:305
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Robotino sensor hook integration thread.
Definition: sensor_thread.h:58
const char * name() const
Get name of thread.
Definition: thread.h:95
float rad2deg(float rad)
Convert an angle given in radians to degrees.
Definition: angle.h:48
virtual void init()
Initialize the thread.
Definition: act_thread.cpp:58
virtual void loop()
Code to execute in the thread.
Definition: act_thread.cpp:92
virtual void finalize()
Finalize the thread.
Definition: act_thread.cpp:76
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
Definition: interface.h:293
void msgq_flush()
Flush all messages.
Definition: interface.cpp:988
void set_odometry_position_x(const float new_odometry_position_x)
Set odometry_position_x value.
MotorInterface Fawkes BlackBoard Interface.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:37
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:43
virtual void close(Interface *interface)=0
Close interface.