23 #include "act_thread.h"
24 #include "sensor_thread.h"
26 #include <interfaces/MotorInterface.h>
27 #include <utils/math/angle.h>
29 #include <rec/robotino/com/Com.h>
30 #include <rec/robotino/com/OmniDrive.h>
31 #include <rec/iocontrol/remotestate/SensorState.h>
47 :
Thread(
"RobotinoActThread",
Thread::OPMODE_WAITFORWAKEUP),
53 sensor_thread_ = sensor_thread;
60 com_ = sensor_thread_->com_;
61 omni_drive_ =
new rec::robotino::com::OmniDrive();
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);
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.;
85 com_->setSetState( set_state );
94 if (com_->isConnected()) {
95 rec::iocontrol::remotestate::SetState set_state;
96 bool send_set_state =
false;
103 omni_drive_->project(&m1, &m2, &m3,
104 msg->vx() * 1000., msg->vy() * 1000.,
107 set_state.speedSetPoint[0] = m1;
108 set_state.speedSetPoint[1] = m2;
109 set_state.speedSetPoint[2] = m3;
110 send_set_state =
true;
114 set_state.setOdometry =
true;
115 set_state.odometryX = set_state.odometryY = set_state.odometryPhi = 0;
116 send_set_state =
true;
122 if (send_set_state) com_->setSetState(set_state);
124 rec::iocontrol::remotestate::SensorState sensor_state = com_->sensorState();
125 if (sensor_state.sequenceNumber != last_seqnum_) {
127 omni_drive_->unproject(&vx, &vy, &omega,
128 sensor_state.actualVelocity[0],
129 sensor_state.actualVelocity[1],
130 sensor_state.actualVelocity[2]);
133 motor_if_->
set_vx(vx / 1000.);
134 motor_if_->
set_vy(vy / 1000.);
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,
151 tf_publisher->send_transform(t, now,
"/robotino_odometry",
"/base_link");
154 last_seqnum_ = sensor_state.sequenceNumber;
bool msgq_empty()
Check if queue is empty.
TransRotMessage Fawkes BlackBoard Interface Message.
void set_odometry_position_y(const float new_odometry_position_y)
Set odometry_position_y value.
Fawkes library namespace.
void set_odometry_orientation(const float new_odometry_orientation)
Set odometry_orientation value.
A class for handling time.
Thread class encapsulation of pthreads.
void write()
Write from local copy into BlackBoard memory.
Logger * logger
This is the Logger member used to access the logger.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier)=0
Open interface for writing.
RobotinoActThread(RobotinoSensorThread *sensor_thread)
Constructor.
ResetOdometryMessage Fawkes BlackBoard Interface Message.
Clock * clock
By means of this member access to the clock is given.
Thread aspect to use blocked timing.
void msgq_pop()
Erase first message from queue.
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.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Robotino sensor hook integration thread.
const char * name() const
Get name of thread.
float rad2deg(float rad)
Convert an angle given in radians to degrees.
virtual void init()
Initialize the thread.
virtual void loop()
Code to execute in the thread.
virtual void finalize()
Finalize the thread.
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
void msgq_flush()
Flush all messages.
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.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual void close(Interface *interface)=0
Close interface.