22 #include "openrobotino_com_thread.h"
23 #ifdef HAVE_OPENROBOTINO_API_1
24 # include <rec/iocontrol/remotestate/SensorState.h>
25 # include <rec/iocontrol/robotstate/State.h>
26 # include <rec/robotino/com/Com.h>
27 # include <rec/sharedmemory/sharedmemory.h>
29 # include <rec/robotino/api2/AnalogInputArray.h>
30 # include <rec/robotino/api2/Bumper.h>
31 # include <rec/robotino/api2/Com.h>
32 # include <rec/robotino/api2/DigitalInputArray.h>
33 # include <rec/robotino/api2/DistanceSensorArray.h>
34 # include <rec/robotino/api2/ElectricalGripper.h>
35 # include <rec/robotino/api2/Gyroscope.h>
36 # include <rec/robotino/api2/MotorArray.h>
37 # include <rec/robotino/api2/Odometry.h>
38 # include <rec/robotino/api2/PowerManagement.h>
40 #include <baseapp/run.h>
41 #include <core/threading/mutex.h>
42 #include <core/threading/mutex_locker.h>
44 #include <utils/math/angle.h>
45 #include <utils/time/wait.h>
59 #ifdef HAVE_OPENROBOTINO_API_1
72 cfg_hostname_ =
config->
get_string(
"/hardware/robotino/openrobotino/hostname");
73 cfg_quit_on_disconnect_ =
config->
get_bool(
"/hardware/robotino/openrobotino/quit_on_disconnect");
74 cfg_sensor_update_cycle_time_ =
config->
get_uint(
"/hardware/robotino/cycle-time");
75 cfg_gripper_enabled_ =
config->
get_bool(
"/hardware/robotino/gripper/enable_gripper");
76 cfg_enable_gyro_ =
config->
get_bool(
"/hardware/robotino/gyro/enable");
78 #ifdef HAVE_OPENROBOTINO_API_1
79 statemem_ =
new rec::sharedmemory::SharedMemory<rec::iocontrol::robotstate::State>(
80 rec::iocontrol::robotstate::State::sharedMemoryKey);
81 state_ = statemem_->getData();
82 state_mutex_ =
new Mutex();
83 set_state_ =
new rec::iocontrol::remotestate::SetState();
84 set_state_->gripper_isEnabled = cfg_gripper_enabled_;
89 time_wait_ =
new TimeWait(
clock, cfg_sensor_update_cycle_time_ * 1000);
91 if (cfg_enable_gyro_) {
92 data_.imu_enabled =
true;
94 for (
int i = 0; i < 3; ++i)
95 data_.imu_angular_velocity[i] = 0.;
96 for (
int i = 0; i < 4; ++i)
97 data_.imu_orientation[i] = 0.;
98 for (
int i = 0; i < 9; ++i)
99 data_.imu_angular_velocity_covariance[i] = 0.;
102 data_.imu_angular_velocity_covariance[8] =
deg2rad(0.1);
105 #ifdef HAVE_OPENROBOTINO_API_1
106 com_->setAddress(cfg_hostname_.c_str());
107 com_->setMinimumUpdateCycleTime(cfg_sensor_update_cycle_time_);
108 com_->connect(
false);
110 com_ =
new rec::robotino::api2::Com(
"Fawkes");
111 com_->setAddress(cfg_hostname_.c_str());
112 com_->setAutoReconnectEnabled(
false);
113 com_->connectToServer(
true);
115 analog_inputs_com_ =
new rec::robotino::api2::AnalogInputArray();
116 bumper_com_ =
new rec::robotino::api2::Bumper();
117 digital_inputs_com_ =
new rec::robotino::api2::DigitalInputArray();
118 distances_com_ =
new rec::robotino::api2::DistanceSensorArray();
119 gripper_com_ =
new rec::robotino::api2::ElectricalGripper();
120 gyroscope_com_ =
new rec::robotino::api2::Gyroscope();
121 motors_com_ =
new rec::robotino::api2::MotorArray();
122 odom_com_ =
new rec::robotino::api2::Odometry();
123 power_com_ =
new rec::robotino::api2::PowerManagement();
125 analog_inputs_com_->setComId(com_->id());
126 bumper_com_->setComId(com_->id());
127 digital_inputs_com_->setComId(com_->id());
128 distances_com_->setComId(com_->id());
129 gripper_com_->setComId(com_->id());
130 gyroscope_com_->setComId(com_->id());
131 motors_com_->setComId(com_->id());
132 odom_com_->setComId(com_->id());
133 power_com_->setComId(com_->id());
141 #ifdef HAVE_OPENROBOTINO_API_1
142 set_state_->speedSetPoint[0] = 0.;
143 set_state_->speedSetPoint[1] = 0.;
144 set_state_->speedSetPoint[2] = 0.;
145 set_state_->gripper_isEnabled =
false;
146 com_->setSetState(*set_state_);
152 float speeds[3] = {0, 0, 0};
153 motors_com_->setSpeedSetPoints(speeds, 3);
155 delete analog_inputs_com_;
157 delete digital_inputs_com_;
158 delete distances_com_;
160 delete gyroscope_com_;
179 if (com_->isConnected()) {
181 #ifdef HAVE_OPENROBOTINO_API_1
187 #ifdef HAVE_OPENROBOTINO_API_1
188 }
else if (com_->connectionState() == rec::robotino::com::Com::NotConnected) {
192 if (cfg_quit_on_disconnect_) {
194 fawkes::runtime::quit();
197 #ifdef HAVE_OPENROBOTINO_API_1
198 com_->connect(
false);
200 com_->connectToServer(
true);
209 OpenRobotinoComThread::process_api_v1()
211 #ifdef HAVE_OPENROBOTINO_API_1
212 state_mutex_->lock();
214 rec::iocontrol::remotestate::SensorState sensor_state = sensor_states_[active_state_];
215 state_mutex_->unlock();
217 if (sensor_state.sequenceNumber != last_seqnum_) {
219 last_seqnum_ = sensor_state.sequenceNumber;
222 for (
int i = 0; i < 3; ++i) {
223 data_.mot_velocity[i] = sensor_state.actualVelocity[i];
224 data_.mot_position[i] = sensor_state.actualPosition[i];
225 data_.mot_current[i] = sensor_state.motorCurrent[i];
227 data_.bumper = sensor_state.bumper;
228 data_.bumper_estop_enabled = state_->emergencyStop.isEnabled;
229 for (
int i = 0; i < 8; ++i) {
230 data_.digital_in[i] = sensor_state.dIn[i];
231 data_.analog_in[i] = sensor_state.aIn[i];
233 if (cfg_enable_gyro_) {
234 if (state_->gyro.port == rec::serialport::UNDEFINED) {
235 if (fabs(
data_.imu_angular_velocity[0] + 1.) > 0.00001) {
236 data_.imu_angular_velocity[0] = -1.;
237 data_.imu_angular_velocity[2] = 0.;
238 data_.imu_orientation[0] = -1.;
241 data_.imu_angular_velocity[0] = 0.;
242 data_.imu_angular_velocity[2] = state_->gyro.rate;
244 tf::Quaternion q = tf::create_quaternion_from_yaw(state_->gyro.angle);
245 data_.imu_orientation[0] = q.x();
246 data_.imu_orientation[1] = q.y();
247 data_.imu_orientation[2] = q.z();
248 data_.imu_orientation[3] = q.w();
252 for (
int i = 0; i < NUM_IR_SENSORS; ++i) {
253 data_.ir_voltages[i] = sensor_state.distanceSensor[i];
256 data_.bat_voltage = roundf(sensor_state.voltage * 1000.);
257 data_.bat_current = roundf(sensor_state.current);
260 float soc = (sensor_state.voltage - 21.0f) / 5.f;
261 soc = std::min(1.f, std::max(0.f, soc));
262 data_.bat_absolute_soc = soc;
268 OpenRobotinoComThread::process_api_v2()
270 #ifdef HAVE_OPENROBOTINO_API_2
271 com_->processComEvents();
273 double odo_x = 0, odo_y = 0, odo_phi = 0;
274 unsigned int odo_seq = 0;
276 odom_com_->readings(&odo_x, &odo_y, &odo_phi, &odo_seq);
278 if (odo_seq != last_seqnum_) {
280 last_seqnum_ = odo_seq;
282 if (motors_com_->numMotors() != 3) {
284 "Invalid number of motors, got %u, expected 3",
285 motors_com_->numMotors());
288 motors_com_->actualVelocities(
data_.mot_velocity);
289 motors_com_->actualPositions(
data_.mot_position);
290 motors_com_->motorCurrents(
data_.mot_current);
292 data_.bumper = bumper_com_->value();
294 if (digital_inputs_com_->numDigitalInputs() != 8) {
296 "Invalid number of digital inputs, got %u, expected 8",
297 digital_inputs_com_->numDigitalInputs());
300 int digin_readings[8];
301 digital_inputs_com_->values(digin_readings);
302 for (
unsigned int i = 0; i < 8; ++i)
303 data_.digital_in[i] = (digin_readings[i] != 0);
305 if (analog_inputs_com_->numAnalogInputs() != 8) {
307 "Invalid number of analog inputs, got %u, expected 8",
308 analog_inputs_com_->numAnalogInputs());
311 analog_inputs_com_->values(
data_.analog_in);
313 if (distances_com_->numDistanceSensors() != NUM_IR_SENSORS) {
315 "Invalid number of distance sensors, got %u, expected 9",
316 distances_com_->numDistanceSensors());
322 distances_com_->voltages(
data_.ir_voltages);
324 float pow_current = power_com_->current() * 1000.;
325 float pow_voltage = power_com_->voltage() * 1000.;
327 float gyro_angle = gyroscope_com_->angle();
328 float gyro_rate = gyroscope_com_->rate();
330 data_.bumper_estop_enabled =
false;
331 data_.imu_angular_velocity[0] = 0.;
332 data_.imu_angular_velocity[2] = gyro_rate;
334 tf::Quaternion q = tf::create_quaternion_from_yaw(gyro_angle);
335 data_.imu_orientation[0] = q.x();
336 data_.imu_orientation[1] = q.y();
337 data_.imu_orientation[2] = q.z();
338 data_.imu_orientation[3] = q.w();
340 data_.bat_voltage = roundf(pow_voltage);
342 data_.bat_current = roundf(pow_current);
345 float soc = (power_com_->voltage() - 22.0f) / 2.5f;
346 soc = std::min(1.f, std::max(0.f, soc));
347 data_.bat_absolute_soc = soc;
352 #ifdef HAVE_OPENROBOTINO_API_1
355 OpenRobotinoComThread::updateEvent()
357 unsigned int next_state = 1 - active_state_;
358 sensor_states_[next_state] = sensorState();
359 times_[next_state].stamp();
362 active_state_ = next_state;
370 if (com_->isConnected()) {
371 #ifdef HAVE_OPENROBOTINO_API_1
372 set_state_->setOdometry =
true;
373 set_state_->odometryX = set_state_->odometryY = set_state_->odometryPhi = 0.;
374 com_->setSetState(*set_state_);
375 set_state_->setOdometry =
false;
377 odom_com_->set(0., 0., 0.,
true);
386 "Setting motor accel limits for OpenRobotino driver not supported, configure controld3");
392 logger->
log_error(
name(),
"Setting digital outputs not supported with openrobotino driver");
401 return com_->isConnected();
420 #ifdef HAVE_OPENROBOTINO_API_1
421 state_mutex_->lock();
422 t = times_[active_state_];
423 rec::iocontrol::remotestate::SensorState sensor_state = sensor_states_[active_state_];
424 state_mutex_->unlock();
427 a1 = sensor_state.actualVelocity[0] / 1000.f;
428 a2 = sensor_state.actualVelocity[1] / 1000.f;
429 a3 = sensor_state.actualVelocity[2] / 1000.f;
430 seq = sensor_state.sequenceNumber;
434 float mot_act_vel[motors_com_->numMotors()];
435 motors_com_->actualVelocities(mot_act_vel);
437 double odo_x = 0, odo_y = 0, odo_phi = 0;
438 odom_com_->readings(&odo_x, &odo_y, &odo_phi, &seq);
456 #ifdef HAVE_OPENROBOTINO_API_1
457 state_mutex_->lock();
458 rec::iocontrol::remotestate::SensorState sensor_state = sensor_states_[active_state_];
459 state_mutex_->unlock();
461 x = sensor_state.odometryX / 1000.f;
462 y = sensor_state.odometryY / 1000.f;
463 phi =
deg2rad(sensor_state.odometryPhi);
467 odom_com_->readings(&x, &y, &phi, &seq);
479 #ifdef HAVE_OPENROBOTINO_API_1
480 state_mutex_->lock();
481 rec::iocontrol::remotestate::SensorState sensor_state = sensor_states_[active_state_];
482 state_mutex_->unlock();
484 return sensor_state.isGripperOpened;
486 return gripper_com_->isOpened();
498 #ifdef HAVE_OPENROBOTINO_API_1
499 set_state_->speedSetPoint[0] = s1;
500 set_state_->speedSetPoint[1] = s2;
501 set_state_->speedSetPoint[2] = s3;
503 com_->setSetState(*set_state_);
505 float speeds[3] = {s1, s2, s3};
506 motors_com_->setSpeedSetPoints(speeds, 3);
516 #ifdef HAVE_OPENROBOTINO_API_1
517 set_state_->gripper_close = !opened;
518 com_->setSetState(*set_state_);
521 gripper_com_->open();
523 gripper_com_->close();
531 #ifdef HAVE_OPENROBOTINO_API_1
532 logger->
log_info(
name(),
"%sabling bumper estop on request", msg->is_enabled() ?
"En" :
"Dis");
533 state_->emergencyStop.isEnabled = msg->is_enabled();
virtual void set_digital_output(unsigned int digital_out, bool enable)
Set digital output state.
virtual void set_gripper(bool opened)
Open or close gripper.
virtual bool is_connected()
Check if we are connected to OpenRobotino.
virtual void init()
Initialize the thread.
virtual void once()
Execute an action exactly once.
virtual void loop()
Code to execute in the thread.
OpenRobotinoComThread()
Constructor.
virtual void set_bumper_estop_enabled(bool enabled)
Enable or disable emergency stop on bumper contact.
virtual void get_odometry(double &x, double &y, double &phi)
Get current odometry.
virtual void reset_odometry()
Reset odometry to zero.
virtual void set_speed_points(float s1, float s2, float s3)
Set speed points for wheels.
virtual void get_act_velocity(float &a1, float &a2, float &a3, unsigned int &seq, fawkes::Time &t)
Get actual velocity.
virtual void set_motor_accel_limits(float min_accel, float max_accel)
Set acceleration limits of motors.
virtual bool is_gripper_open()
Check if gripper is open.
virtual ~OpenRobotinoComThread()
Destructor.
virtual void finalize()
Finalize the thread.
Virtual base class for thread that communicates with a Robotino.
fawkes::Mutex * data_mutex_
Mutex to protect data_.
SensorData data_
Data struct that must be updated whenever new data is available.
bool new_data_
Flag to indicate new data, set to true if data_ is modified.
Clock * clock
By means of this member access to the clock is given.
Configuration * config
This is the Configuration member used to access the configuration.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
Mutex mutual exclusion lock.
const char * name() const
Get name of thread.
void mark_start()
Mark start of loop.
void wait()
Wait until minimum loop time has been reached.
A class for handling time.
Time & stamp()
Set this time to the current time.
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.