21 #include "laser_calibration.h"
22 #include "pitch_calibration.h"
23 #include "roll_calibration.h"
24 #include "time_offset_calibration.h"
25 #include "yaw_calibration.h"
27 #include <blackboard/remote.h>
28 #include <config/netconf.h>
29 #include <interfaces/Laser360Interface.h>
30 #include <interfaces/MotorInterface.h>
31 #include <netcomm/fawkes/client.h>
32 #include <tf/transform_listener.h>
33 #include <tf/transformer.h>
34 #include <utils/system/argparser.h>
43 print_usage(
const char *program_name)
45 printf(
"Usage: %s [-h] [-r host[:port]]\n"
46 " -h This help message\n"
47 " -r host[:port] Remote host (and optionally port) to connect to\n"
48 " -f front-laser-id The ID of the front laser blackboard interface\n"
49 " -b back-laser-id The ID of the back laser blackboard interface\n"
50 " -R Skip roll calibration\n"
51 " -P Skip pitch calibration\n"
52 " -Y Skip yaw calibration\n"
53 " -T Skip time offset calibration\n",
65 main(
int argc,
char **argv)
68 if (arg_parser.has_arg(
"h")) {
80 string host =
"localhost";
81 unsigned short int port = FAWKES_TCP_PORT;
82 if (arg_parser.has_arg(
"r")) {
83 arg_parser.parse_hostport(
"r", host, port);
85 string front_laser_interface_id =
"Laser front 360";
86 if (arg_parser.has_arg(
"f")) {
87 front_laser_interface_id = string(arg_parser.arg(
"f"));
89 string back_laser_interface_id =
"Laser back 360";
90 if (arg_parser.has_arg(
"b")) {
91 back_laser_interface_id = string(arg_parser.arg(
"b"));
93 bool calibrate_roll =
true;
94 if (arg_parser.has_arg(
"R")) {
95 calibrate_roll =
false;
97 bool calibrate_pitch =
true;
98 if (arg_parser.has_arg(
"P")) {
99 calibrate_pitch =
false;
101 bool calibrate_yaw =
true;
102 if (arg_parser.has_arg(
"Y")) {
103 calibrate_yaw =
false;
105 bool calibrate_time_offset =
true;
106 if (arg_parser.has_arg(
"T")) {
107 calibrate_time_offset =
false;
118 printf(
"Failed to connect to remote host at %s:%u\n", host.c_str(), port);
127 printf(
"Failed to open Blackboard interface '%s'\n", back_laser_interface_id.c_str());
132 printf(
"Laser '%s' does not have a writer!\n", back_laser_interface_id.c_str());
139 printf(
"Failed to open Blackboard interface '%s'\n", front_laser_interface_id.c_str());
144 printf(
"Laser '%s' does not have a writer!\n", front_laser_interface_id.c_str());
148 string motor_interface_id =
"Robotino";
152 printf(
"Failed to open Blackboard interface '%s'\n", motor_interface_id.c_str());
157 printf(
"motor '%s' does not have a writer!\n", motor_interface_id.c_str());
161 const string cfg_transforms_prefix =
"/plugins/static-transforms/transforms/back_laser/";
163 RollCalibration roll_calibration(laser, transformer, netconf, cfg_transforms_prefix +
"rot_roll");
167 cfg_transforms_prefix +
"rot_pitch");
169 laser, front_laser, transformer, netconf, cfg_transforms_prefix +
"rot_yaw");
172 front_laser, motor, transformer, netconf,
"/hardware/laser/front/time_offset");
174 laser, motor, transformer, netconf,
"/hardware/laser/back/time_offset");
175 if (calibrate_pitch || calibrate_roll) {
176 cout <<
"Please put the robot in a position such that you only have ground "
177 <<
"behind the robot." << endl;
179 if (calibrate_pitch) {
180 cout <<
"To start pitch calibration, press enter" << endl;
182 pitch_calibration.calibrate();
183 printf(
"--------------------\n");
185 if (calibrate_roll) {
186 cout <<
"To start roll calibration, press enter" << endl;
188 roll_calibration.calibrate();
189 printf(
"--------------------\n");
192 cout <<
"Please move the robot such that it can see a wall." << endl
193 <<
"To start yaw calibration, press enter." << endl;
195 yaw_calibration.calibrate();
196 printf(
"--------------------\n");
198 if (calibrate_time_offset) {
199 cout <<
"Move the robot into a corner and make sure that it can rotate "
200 <<
"without hitting any obstacles." << endl
201 <<
"Careful: The robot will start rotating in the next step." << endl
202 <<
"Press Enter to start time offset calibration." << endl;
204 printf(
"Starting time offset calibration for front laser.\n");
205 time_offset_front_calibration.calibrate();
206 printf(
"--------------------\n");
207 printf(
"Starting time offset calibration for back laser.\n");
208 time_offset_back_calibration.calibrate();
Calibrate the pitch angle of the laser.
Calibrate the roll angle of a laser.
Calibrate the time offset of a laser.
Calibrate the yaw angle of the back laser using the front laser.
Parse command line arguments.
The BlackBoard abstract class.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
Base class for exceptions in Fawkes.
void print_trace() noexcept
Prints trace to stderr.
Simple Fawkes network client.
void connect()
Connect to remote.
bool has_writer() const
Check if there is a writer for the interface.
Laser360Interface Fawkes BlackBoard Interface.
MotorInterface Fawkes BlackBoard Interface.
Remote configuration via Fawkes net.
Fawkes library namespace.