Fawkes API  Fawkes Development Version
act_thread.cpp
00001 
00002 /***************************************************************************
00003  *  act_thread.cpp - Robotino act thread
00004  *
00005  *  Created: Sun Nov 13 16:07:40 2011
00006  *  Copyright  2011  Tim Niemueller [www.niemueller.de]
00007  *
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version.
00014  *
00015  *  This program is distributed in the hope that it will be useful,
00016  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  *  GNU Library General Public License for more details.
00019  *
00020  *  Read the full text in the LICENSE.GPL file in the doc directory.
00021  */
00022 
00023 #include "act_thread.h"
00024 #include "sensor_thread.h"
00025 
00026 #include <interfaces/MotorInterface.h>
00027 #include <utils/math/angle.h>
00028 
00029 #include <rec/robotino/com/Com.h>
00030 #include <rec/robotino/com/OmniDrive.h>
00031 #include <rec/iocontrol/remotestate/SensorState.h>
00032 
00033 
00034 using namespace fawkes;
00035 
00036 /** @class RobotinoActThread "act_thread.h"
00037  * Robotino act hook integration thread.
00038  * This thread integrates into the Fawkes main loop at the ACT hook and
00039  * executes motion commands.
00040  * @author Tim Niemueller
00041  */
00042 
00043 /** Constructor.
00044  * @param sensor_thread sensor thread from which to get the Com instance
00045  */
00046 RobotinoActThread::RobotinoActThread(RobotinoSensorThread *sensor_thread)
00047   : Thread("RobotinoActThread", Thread::OPMODE_WAITFORWAKEUP),
00048 #ifdef HAVE_TF
00049     TransformAspect(TransformAspect::ONLY_PUBLISHER, "Robotino Odometry"),
00050 #endif
00051     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_ACT)
00052 {
00053   sensor_thread_ = sensor_thread;
00054 }
00055 
00056 
00057 void
00058 RobotinoActThread::init()
00059 {
00060   com_ = sensor_thread_->com_;
00061   omni_drive_ = new rec::robotino::com::OmniDrive();
00062 
00063   last_seqnum_ = 0;
00064 
00065   // reset odometry once on startup
00066   rec::iocontrol::remotestate::SetState set_state;
00067   set_state.setOdometry = true;
00068   set_state.odometryX = set_state.odometryY = set_state.odometryPhi = 0;
00069   com_->setSetState(set_state);
00070 
00071   motor_if_ = blackboard->open_for_writing<MotorInterface>("Robotino");
00072 }
00073 
00074 
00075 void
00076 RobotinoActThread::finalize()
00077 {
00078   blackboard->close(motor_if_);
00079   if (com_->isConnected()) {
00080     rec::iocontrol::remotestate::SetState set_state;
00081     set_state.speedSetPoint[0] = 0.;
00082     set_state.speedSetPoint[1] = 0.;
00083     set_state.speedSetPoint[2] = 0.;
00084 
00085     com_->setSetState( set_state );
00086   }
00087   com_ = NULL;
00088 }
00089 
00090 
00091 void
00092 RobotinoActThread::loop()
00093 {
00094   if (com_->isConnected()) {
00095     rec::iocontrol::remotestate::SetState set_state;
00096     bool send_set_state = false;
00097     while (! motor_if_->msgq_empty()) {
00098 
00099       if (MotorInterface::TransRotMessage *msg =
00100           motor_if_->msgq_first_safe(msg))
00101       {
00102         float m1, m2, m3;
00103         omni_drive_->project(&m1, &m2, &m3,
00104                              msg->vx() * 1000., msg->vy() * 1000.,
00105                              rad2deg(msg->omega()));
00106 
00107         set_state.speedSetPoint[0] = m1;
00108         set_state.speedSetPoint[1] = m2;
00109         set_state.speedSetPoint[2] = m3;
00110         send_set_state = true;
00111       }
00112       else if (motor_if_->msgq_first_is<MotorInterface::ResetOdometryMessage>())
00113       {
00114         set_state.setOdometry = true;
00115         set_state.odometryX = set_state.odometryY = set_state.odometryPhi = 0;
00116         send_set_state = true;
00117       }
00118 
00119       motor_if_->msgq_pop();
00120     }
00121 
00122     if (send_set_state)  com_->setSetState(set_state);
00123 
00124     rec::iocontrol::remotestate::SensorState sensor_state = com_->sensorState();
00125     if (sensor_state.sequenceNumber != last_seqnum_) {
00126       float vx, vy, omega;
00127       omni_drive_->unproject(&vx, &vy, &omega,
00128                              sensor_state.actualVelocity[0],
00129                              sensor_state.actualVelocity[1],
00130                              sensor_state.actualVelocity[2]);
00131 
00132       // div by 1000 to convert from mm to m
00133       motor_if_->set_vx(vx / 1000.);
00134       motor_if_->set_vy(vy / 1000.);
00135       motor_if_->set_omega(deg2rad(omega));
00136 
00137       motor_if_->set_odometry_position_x(sensor_state.odometryX / 1000.f);
00138       motor_if_->set_odometry_position_y(sensor_state.odometryY / 1000.f);
00139       motor_if_->set_odometry_orientation(deg2rad(sensor_state.odometryPhi));
00140       motor_if_->write();
00141 
00142 #ifdef HAVE_TF
00143       fawkes::Time now(clock);
00144 
00145       tf::Transform t(tf::Quaternion(tf::Vector3(0,0,1),
00146                                      deg2rad(sensor_state.odometryPhi)),
00147                       tf::Vector3(sensor_state.odometryX / 1000.f,
00148                                   sensor_state.odometryY / 1000.f,
00149                                   0));
00150 
00151       tf_publisher->send_transform(t, now, "/robotino_odometry", "/base_link");
00152 #endif
00153 
00154       last_seqnum_ = sensor_state.sequenceNumber;
00155     }
00156 
00157   } else {
00158     if (! motor_if_->msgq_empty()) {
00159       logger->log_warn(name(), "Motor commands received while not connected");
00160       motor_if_->msgq_flush();
00161     }
00162   }
00163 }