22 #include "openrobotino_com_thread.h" 23 #ifdef HAVE_OPENROBOTINO_API_1 24 # include <rec/robotino/com/Com.h> 25 # include <rec/sharedmemory/sharedmemory.h> 26 # include <rec/iocontrol/remotestate/SensorState.h> 27 # include <rec/iocontrol/robotstate/State.h> 29 # include <rec/robotino/api2/Com.h> 30 # include <rec/robotino/api2/AnalogInputArray.h> 31 # include <rec/robotino/api2/Bumper.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 <utils/math/angle.h> 42 #include <core/threading/mutex.h> 43 #include <core/threading/mutex_locker.h> 44 #include <utils/time/wait.h> 61 #ifdef HAVE_OPENROBOTINO_API_1 76 cfg_hostname_ =
config->
get_string(
"/hardware/robotino/openrobotino/hostname");
77 cfg_quit_on_disconnect_ =
config->
get_bool(
"/hardware/robotino/openrobotino/quit_on_disconnect");
78 cfg_sensor_update_cycle_time_ =
80 cfg_gripper_enabled_ =
config->
get_bool(
"/hardware/robotino/gripper/enable_gripper");
81 cfg_enable_gyro_ =
config->
get_bool(
"/hardware/robotino/gyro/enable");
83 #ifdef HAVE_OPENROBOTINO_API_1 84 statemem_ =
new rec::sharedmemory::SharedMemory<rec::iocontrol::robotstate::State>
85 (rec::iocontrol::robotstate::State::sharedMemoryKey);
86 state_ = statemem_->getData();
87 state_mutex_ =
new Mutex();
88 set_state_ =
new rec::iocontrol::remotestate::SetState();
89 set_state_->gripper_isEnabled = cfg_gripper_enabled_;
94 time_wait_ =
new TimeWait(
clock, cfg_sensor_update_cycle_time_ * 1000);
96 if (cfg_enable_gyro_) {
97 data_.imu_enabled =
true;
99 for (
int i = 0; i < 3; ++i)
data_.imu_angular_velocity[i] = 0.;
100 for (
int i = 0; i < 4; ++i)
data_.imu_orientation[i] = 0.;
101 for (
int i = 0; i < 9; ++i)
data_.imu_angular_velocity_covariance[i] = 0.;
104 data_.imu_angular_velocity_covariance[8] =
deg2rad(0.1);
107 #ifdef HAVE_OPENROBOTINO_API_1 108 com_->setAddress(cfg_hostname_.c_str());
109 com_->setMinimumUpdateCycleTime(cfg_sensor_update_cycle_time_);
110 com_->connect(
false);
112 com_ =
new rec::robotino::api2::Com(
"Fawkes");
113 com_->setAddress(cfg_hostname_.c_str());
114 com_->setAutoReconnectEnabled(
false);
115 com_->connectToServer(
true);
117 analog_inputs_com_ =
new rec::robotino::api2::AnalogInputArray();
118 bumper_com_ =
new rec::robotino::api2::Bumper();
119 digital_inputs_com_ =
new rec::robotino::api2::DigitalInputArray();
120 distances_com_ =
new rec::robotino::api2::DistanceSensorArray();
121 gripper_com_ =
new rec::robotino::api2::ElectricalGripper();
122 gyroscope_com_ =
new rec::robotino::api2::Gyroscope();
123 motors_com_ =
new rec::robotino::api2::MotorArray();
124 odom_com_ =
new rec::robotino::api2::Odometry();
125 power_com_ =
new rec::robotino::api2::PowerManagement();
127 analog_inputs_com_->setComId(com_->id());
128 bumper_com_->setComId(com_->id());
129 digital_inputs_com_->setComId(com_->id());
130 distances_com_->setComId(com_->id());
131 gripper_com_->setComId(com_->id());
132 gyroscope_com_->setComId(com_->id());
133 motors_com_->setComId(com_->id());
134 odom_com_->setComId(com_->id());
135 power_com_->setComId(com_->id());
145 #ifdef HAVE_OPENROBOTINO_API_1 146 set_state_->speedSetPoint[0] = 0.;
147 set_state_->speedSetPoint[1] = 0.;
148 set_state_->speedSetPoint[2] = 0.;
149 set_state_->gripper_isEnabled =
false;
150 com_->setSetState(*set_state_);
156 float speeds[3] = { 0, 0, 0 };
157 motors_com_->setSpeedSetPoints(speeds, 3);
159 delete analog_inputs_com_;
161 delete digital_inputs_com_;
162 delete distances_com_;
164 delete gyroscope_com_;
183 if (com_->isConnected()) {
185 #ifdef HAVE_OPENROBOTINO_API_1 192 #ifdef HAVE_OPENROBOTINO_API_1 193 }
else if (com_->connectionState() == rec::robotino::com::Com::NotConnected) {
197 if (cfg_quit_on_disconnect_) {
199 fawkes::runtime::quit();
202 #ifdef HAVE_OPENROBOTINO_API_1 203 com_->connect(
false);
205 com_->connectToServer(
true);
215 OpenRobotinoComThread::process_api_v1()
217 #ifdef HAVE_OPENROBOTINO_API_1 218 state_mutex_->lock();
220 rec::iocontrol::remotestate::SensorState sensor_state = sensor_states_[active_state_];
221 state_mutex_->unlock();
223 if (sensor_state.sequenceNumber != last_seqnum_) {
225 last_seqnum_ = sensor_state.sequenceNumber;
228 for (
int i = 0; i < 3; ++i) {
229 data_.mot_velocity[i] = sensor_state.actualVelocity[i];
230 data_.mot_position[i] = sensor_state.actualPosition[i];
231 data_.mot_current[i] = sensor_state.motorCurrent[i];
233 data_.bumper = sensor_state.bumper;
234 data_.bumper_estop_enabled = state_->emergencyStop.isEnabled;
235 for (
int i = 0; i < 8; ++i) {
236 data_.digital_in[i] = sensor_state.dIn[i];
237 data_.analog_in[i] = sensor_state.aIn[i];
239 if (cfg_enable_gyro_) {
240 if (state_->gyro.port == rec::serialport::UNDEFINED) {
241 if (fabs(
data_.imu_angular_velocity[0] + 1.) > 0.00001) {
242 data_.imu_angular_velocity[0] = -1.;
243 data_.imu_angular_velocity[2] = 0.;
244 data_.imu_orientation[0] = -1.;
247 data_.imu_angular_velocity[0] = 0.;
248 data_.imu_angular_velocity[2] = state_->gyro.rate;
250 tf::Quaternion q = tf::create_quaternion_from_yaw(state_->gyro.angle);
251 data_.imu_orientation[0] = q.x();
252 data_.imu_orientation[1] = q.y();
253 data_.imu_orientation[2] = q.z();
254 data_.imu_orientation[3] = q.w();
258 for (
int i = 0; i < NUM_IR_SENSORS; ++i) {
259 data_.ir_voltages[i] = sensor_state.distanceSensor[i];
262 data_.bat_voltage = roundf(sensor_state.voltage * 1000.);
263 data_.bat_current = roundf(sensor_state.current);
266 float soc = (sensor_state.voltage - 21.0f) / 5.f;
267 soc = std::min(1.f, std::max(0.f, soc));
268 data_.bat_absolute_soc = soc;
275 OpenRobotinoComThread::process_api_v2()
277 #ifdef HAVE_OPENROBOTINO_API_2 278 com_->processComEvents();
280 double odo_x = 0, odo_y = 0, odo_phi = 0;
281 unsigned int odo_seq = 0;
283 odom_com_->readings(&odo_x, &odo_y, &odo_phi, &odo_seq);
285 if (odo_seq != last_seqnum_) {
287 last_seqnum_ = odo_seq;
289 if (motors_com_->numMotors() != 3) {
291 motors_com_->numMotors());
294 motors_com_->actualVelocities(
data_.mot_velocity);
295 motors_com_->actualPositions(
data_.mot_position);
296 motors_com_->motorCurrents(
data_.mot_current);
298 data_.bumper = bumper_com_->value();
300 if (digital_inputs_com_->numDigitalInputs() != 8) {
302 digital_inputs_com_->numDigitalInputs());
305 int digin_readings[8];
306 digital_inputs_com_->values(digin_readings);
307 for (
unsigned int i = 0; i < 8; ++i)
data_.digital_in[i] = (digin_readings[i] != 0);
309 if (analog_inputs_com_->numAnalogInputs() != 8) {
311 analog_inputs_com_->numAnalogInputs());
314 analog_inputs_com_->values(
data_.analog_in);
316 if (distances_com_->numDistanceSensors() != NUM_IR_SENSORS) {
318 distances_com_->numDistanceSensors());
324 distances_com_->voltages(
data_.ir_voltages);
326 float pow_current = power_com_->current() * 1000.;
327 float pow_voltage = power_com_->voltage() * 1000.;
329 float gyro_angle = gyroscope_com_->angle();
330 float gyro_rate = gyroscope_com_->rate();
332 data_.bumper_estop_enabled =
false;
333 data_.imu_angular_velocity[0] = 0.;
334 data_.imu_angular_velocity[2] = gyro_rate;
336 tf::Quaternion q = tf::create_quaternion_from_yaw(gyro_angle);
337 data_.imu_orientation[0] = q.x();
338 data_.imu_orientation[1] = q.y();
339 data_.imu_orientation[2] = q.z();
340 data_.imu_orientation[3] = q.w();
342 data_.bat_voltage = roundf(pow_voltage);;
343 data_.bat_current = roundf(pow_current);
346 float soc = (power_com_->voltage() - 22.0f) / 2.5f;
347 soc = std::min(1.f, std::max(0.f, soc));
348 data_.bat_absolute_soc = soc;
353 #ifdef HAVE_OPENROBOTINO_API_1 356 OpenRobotinoComThread::updateEvent()
358 unsigned int next_state = 1 - active_state_;
359 sensor_states_[next_state] = sensorState();
360 times_[next_state].stamp();
363 active_state_ = next_state;
372 if (com_->isConnected()) {
373 #ifdef HAVE_OPENROBOTINO_API_1 374 set_state_->setOdometry =
true;
375 set_state_->odometryX = set_state_->odometryY = set_state_->odometryPhi = 0.;
376 com_->setSetState(*set_state_);
377 set_state_->setOdometry =
false;
379 odom_com_->set(0., 0., 0.,
true);
388 throw Exception(
"Setting motor accel limits for OpenRobotino driver not supported, configure controld3");
394 logger->
log_error(
name(),
"Setting digital outputs not supported with openrobotino driver");
404 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);
457 #ifdef HAVE_OPENROBOTINO_API_1 458 state_mutex_->lock();
459 rec::iocontrol::remotestate::SensorState sensor_state = sensor_states_[active_state_];
460 state_mutex_->unlock();
462 x = sensor_state.odometryX / 1000.f;
463 y = sensor_state.odometryY / 1000.f;
464 phi =
deg2rad(sensor_state.odometryPhi);
468 odom_com_->readings(&x, &y, &phi, &seq);
481 #ifdef HAVE_OPENROBOTINO_API_1 482 state_mutex_->lock();
483 rec::iocontrol::remotestate::SensorState sensor_state = sensor_states_[active_state_];
484 state_mutex_->unlock();
486 return sensor_state.isGripperOpened;
488 return gripper_com_->isOpened();
501 #ifdef HAVE_OPENROBOTINO_API_1 502 set_state_->speedSetPoint[0] = s1;
503 set_state_->speedSetPoint[1] = s2;
504 set_state_->speedSetPoint[2] = s3;
506 com_->setSetState(*set_state_);
508 float speeds[3] = { s1, s2, s3 };
509 motors_com_->setSpeedSetPoints(speeds, 3);
521 #ifdef HAVE_OPENROBOTINO_API_1 522 set_state_->gripper_close = ! opened;
523 com_->setSetState(*set_state_);
526 gripper_com_->open();
528 gripper_com_->close();
538 #ifdef HAVE_OPENROBOTINO_API_1 540 msg->is_enabled() ?
"En" :
"Dis");
541 state_->emergencyStop.isEnabled = msg->is_enabled();
SensorData data_
Data struct that must be updated whenever new data is available.
void wait()
Wait until minimum loop time has been reached.
bool new_data_
Flag to indicate new data, set to true if data_ is modified.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
virtual void once()
Execute an action exactly once.
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual void init()
Initialize the thread.
A class for handling time.
virtual void set_gripper(bool opened)
Open or close gripper.
OpenRobotinoComThread()
Constructor.
Logger * logger
This is the Logger member used to access the logger.
fawkes::Mutex * data_mutex_
Mutex to protect data_.
Clock * clock
By means of this member access to the clock is given.
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 void set_bumper_estop_enabled(bool enabled)
Enable or disable emergency stop on bumper contact.
Base class for exceptions in Fawkes.
virtual void reset_odometry()
Reset odometry to zero.
virtual bool is_connected()
Check if we are connected to OpenRobotino.
Virtual base class for thread that communicates with a Robotino.
const char * name() const
Get name of thread.
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.
void mark_start()
Mark start of loop.
virtual void set_speed_points(float s1, float s2, float s3)
Set speed points for wheels.
virtual void finalize()
Finalize the thread.
Time & stamp()
Set this time to the current time.
virtual void set_digital_output(unsigned int digital_out, bool enable)
Set digital output state.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
virtual bool is_gripper_open()
Check if gripper is open.
Mutex mutual exclusion lock.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
virtual void loop()
Code to execute in the thread.
virtual ~OpenRobotinoComThread()
Destructor.
Configuration * config
This is the Configuration member used to access the configuration.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
virtual void get_odometry(double &x, double &y, double &phi)
Get current odometry.