Fawkes API Fawkes Development Version
direct_com_thread.h
1/***************************************************************************
2 * direct_com_thread.h - Robotino com thread for direct communication
3 *
4 * Created: Mon Apr 04 11:48:36 2016
5 * Copyright 2011-2016 Tim Niemueller [www.niemueller.de]
6 ****************************************************************************/
7
8/* This program is free software; you can redistribute it and/or modify
9 * it under the terms of the GNU General Public License as published by
10 * the Free Software Foundation; either version 2 of the License, or
11 * (at your option) any later version.
12 *
13 * This program is distributed in the hope that it will be useful,
14 * but WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16 * GNU Library General Public License for more details.
17 *
18 * Read the full text in the LICENSE.GPL file in the doc directory.
19 */
20
21#ifndef _PLUGINS_ROBOTINO_DIRECT_COM_THREAD_H_
22#define _PLUGINS_ROBOTINO_DIRECT_COM_THREAD_H_
23
24#include "com_thread.h"
25#include "direct_com_message.h"
26
27#include <aspect/blackboard.h>
28#include <aspect/configurable.h>
29#include <core/threading/thread.h>
30#include <utils/time/time.h>
31
32#include <boost/asio.hpp>
33#include <boost/thread/mutex.hpp>
34#include <memory>
35
37
38namespace fawkes {
39class Mutex;
40class TimeWait;
41
42class BatteryInterface;
43class RobotinoSensorInterface;
44class IMUInterface;
45} // namespace fawkes
46
48{
49public:
52
53 virtual void init();
54 virtual void once();
55 virtual void loop();
57
58 virtual void finalize();
59
60 virtual bool is_connected();
61
62 virtual void set_gripper(bool opened);
63 virtual bool is_gripper_open();
64 virtual void set_speed_points(float s1, float s2, float s3);
65 virtual void
66 get_act_velocity(float &a1, float &a2, float &a3, unsigned int &seq, fawkes::Time &t);
67 virtual void get_odometry(double &x, double &y, double &phi);
68
69 virtual void reset_odometry();
70 virtual void set_bumper_estop_enabled(bool enabled);
71 virtual void set_motor_accel_limits(float min_accel, float max_accel);
72 virtual void set_digital_output(unsigned int digital_out, bool enable);
73
74 virtual void set_desired_vel(float vx, float vy, float omega);
75
76 /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
77protected:
78 virtual void
80 {
81 Thread::run();
82 }
83
84private:
85 std::string find_device_udev();
86 bool find_controld3();
87 void open_device(bool wait_replies);
88 void close_device();
89 void flush_device();
90 void check_deadline();
91 void request_data();
92 void handle_request_data(const boost::system::error_code &ec);
93 void handle_nodata(const boost::system::error_code &ec);
94 void update_nodata_timer();
95
96 void drive();
97 void handle_drive(const boost::system::error_code &ec);
98
100 void send_message(DirectRobotinoComMessage &msg);
102 void process_message(DirectRobotinoComMessage::pointer m);
103
104private:
105 std::string cfg_device_;
106 bool cfg_enable_gyro_;
107 unsigned int cfg_sensor_update_cycle_time_;
108 bool cfg_gripper_enabled_;
109 float cfg_rpm_max_;
110 unsigned int cfg_nodata_timeout_;
111 unsigned int cfg_drive_update_interval_;
112 unsigned int cfg_read_timeout_;
113 bool cfg_log_checksum_errors_;
114 unsigned int cfg_checksum_error_recover_;
115 unsigned int cfg_checksum_error_critical_;
116
117 bool opened_;
118 unsigned int open_tries_;
119
120 unsigned int checksum_errors_;
121
122 uint8_t digital_outputs_;
123
124 boost::asio::io_service io_service_;
125 boost::asio::serial_port serial_;
126 boost::asio::io_service::work io_service_work_;
127 boost::asio::deadline_timer deadline_;
128 boost::asio::streambuf input_buffer_;
129 boost::mutex io_mutex_;
130
131 boost::asio::deadline_timer request_timer_;
132 boost::asio::deadline_timer nodata_timer_;
133 boost::asio::deadline_timer drive_timer_;
134};
135
136#endif
Robotino communication message.
std::shared_ptr< DirectRobotinoComMessage > pointer
shared pointer to direct com message
Thread to communicate with Robotino via OpenRobotino API (v1 or v2).
virtual void once()
Execute an action exactly once.
virtual void get_act_velocity(float &a1, float &a2, float &a3, unsigned int &seq, fawkes::Time &t)
Get actual velocity.
virtual void set_digital_output(unsigned int digital_out, bool enable)
Set digital output state.
virtual bool is_gripper_open()
Check if gripper is open.
virtual void init()
Initialize the thread.
virtual void loop()
Code to execute in the thread.
virtual void set_gripper(bool opened)
Open or close gripper.
virtual void set_bumper_estop_enabled(bool enabled)
Enable or disable emergency stop on bumper contact.
virtual void set_motor_accel_limits(float min_accel, float max_accel)
Set acceleration limits of motors.
virtual void set_desired_vel(float vx, float vy, float omega)
Set desired velocities.
virtual void get_odometry(double &x, double &y, double &phi)
Get latest odometry value.
virtual void reset_odometry()
Reset odometry to zero.
virtual void run()
Stub to see name in backtrace for easier debugging.
virtual void finalize()
Finalize the thread.
virtual ~DirectRobotinoComThread()
Destructor.
virtual bool is_connected()
Check if we are connected to OpenRobotino.
DirectRobotinoComThread()
Constructor.
bool prepare_finalize_user()
Prepare finalization user implementation.
virtual void set_speed_points(float s1, float s2, float s3)
Set speed points for wheels.
Virtual base class for thread that communicates with a Robotino.
Definition: com_thread.h:41
Thread aspect to access configuration data.
Definition: configurable.h:33
A class for handling time.
Definition: time.h:93
Fawkes library namespace.