Fawkes API Fawkes Development Version
main.cpp
1/***************************************************************************
2 * main.cpp - Laser calibration tool
3 *
4 * Created: Tue 18 Jul 2017 15:47:58 CEST 15:47
5 * Copyright 2017-2018 Till Hofmann <hofmann@kbsg.rwth-aachen.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#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"
26
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>
35
36using namespace fawkes;
37using namespace std;
38
39/** Print the usage message.
40 * @param program_name The path of the program.
41 */
42void
43print_usage(const char *program_name)
44{
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",
54 program_name);
55}
56
57/** Run all calibrations.
58 * The command line options allow to enable/disable certain calibrations. By
59 * default, calibrate everything.
60 * @param argc Number of commandline arguments
61 * @param argc The commandline arguments
62 * @return 0 on success, -1 if an error occured.
63 */
64int
65main(int argc, char **argv)
66{
67 ArgumentParser arg_parser(argc, argv, "hr:f:b:RPYT");
68 if (arg_parser.has_arg("h")) {
69 print_usage(argv[0]);
70 return 0;
71 }
72
73 FawkesNetworkClient * client = NULL;
74 BlackBoard * blackboard = NULL;
75 NetworkConfiguration *netconf = NULL;
76 tf::Transformer * transformer = NULL;
77 // Mark the tf listener as unused, we only use its callbacks.
78 tf::TransformListener *tf_listener __attribute__((unused)) = NULL;
79
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);
84 }
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"));
88 }
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"));
92 }
93 bool calibrate_roll = true;
94 if (arg_parser.has_arg("R")) {
95 calibrate_roll = false;
96 }
97 bool calibrate_pitch = true;
98 if (arg_parser.has_arg("P")) {
99 calibrate_pitch = false;
100 }
101 bool calibrate_yaw = true;
102 if (arg_parser.has_arg("Y")) {
103 calibrate_yaw = false;
104 }
105 bool calibrate_time_offset = true;
106 if (arg_parser.has_arg("T")) {
107 calibrate_time_offset = false;
108 }
109
110 try {
111 client = new FawkesNetworkClient(host.c_str(), port);
112 client->connect();
113 blackboard = new RemoteBlackBoard(client);
114 netconf = new NetworkConfiguration(client);
115 transformer = new tf::Transformer();
116 tf_listener = new tf::TransformListener(blackboard, transformer, true);
117 } catch (Exception &e) {
118 printf("Failed to connect to remote host at %s:%u\n", host.c_str(), port);
119 e.print_trace();
120 return -1;
121 }
122
123 LaserInterface *laser = NULL;
124 try {
125 laser = blackboard->open_for_reading<LaserInterface>(back_laser_interface_id.c_str());
126 } catch (Exception &e) {
127 printf("Failed to open Blackboard interface '%s'\n", back_laser_interface_id.c_str());
128 e.print_trace();
129 return -1;
130 }
131 if (!laser->has_writer()) {
132 printf("Laser '%s' does not have a writer!\n", back_laser_interface_id.c_str());
133 return -1;
134 }
135 LaserInterface *front_laser = NULL;
136 try {
137 front_laser = blackboard->open_for_reading<LaserInterface>(front_laser_interface_id.c_str());
138 } catch (Exception &e) {
139 printf("Failed to open Blackboard interface '%s'\n", front_laser_interface_id.c_str());
140 e.print_trace();
141 return -1;
142 }
143 if (!front_laser->has_writer()) {
144 printf("Laser '%s' does not have a writer!\n", front_laser_interface_id.c_str());
145 return -1;
146 }
147 MotorInterface *motor = NULL;
148 string motor_interface_id = "Robotino";
149 try {
150 motor = blackboard->open_for_reading<MotorInterface>(motor_interface_id.c_str());
151 } catch (Exception &e) {
152 printf("Failed to open Blackboard interface '%s'\n", motor_interface_id.c_str());
153 e.print_trace();
154 return -1;
155 }
156 if (!motor->has_writer()) {
157 printf("motor '%s' does not have a writer!\n", motor_interface_id.c_str());
158 return -1;
159 }
160
161 const string cfg_transforms_prefix = "/plugins/static-transforms/transforms/back_laser/";
162
163 RollCalibration roll_calibration(laser, transformer, netconf, cfg_transforms_prefix + "rot_roll");
164 PitchCalibration pitch_calibration(laser,
165 transformer,
166 netconf,
167 cfg_transforms_prefix + "rot_pitch");
168 YawCalibration yaw_calibration(
169 laser, front_laser, transformer, netconf, cfg_transforms_prefix + "rot_yaw");
170 // TODO: make config path a commandline argument
171 TimeOffsetCalibration time_offset_front_calibration(
172 front_laser, motor, transformer, netconf, "/hardware/laser/front/time_offset");
173 TimeOffsetCalibration time_offset_back_calibration(
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;
178 }
179 if (calibrate_pitch) {
180 cout << "To start pitch calibration, press enter" << endl;
181 cin.get();
182 pitch_calibration.calibrate();
183 printf("--------------------\n");
184 }
185 if (calibrate_roll) {
186 cout << "To start roll calibration, press enter" << endl;
187 cin.get();
188 roll_calibration.calibrate();
189 printf("--------------------\n");
190 }
191 if (calibrate_yaw) {
192 cout << "Please move the robot such that it can see a wall." << endl
193 << "To start yaw calibration, press enter." << endl;
194 cin.get();
195 yaw_calibration.calibrate();
196 printf("--------------------\n");
197 }
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;
203 cin.get();
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();
209 }
210
211 delete tf_listener;
212 delete transformer;
213 delete netconf;
214 delete blackboard;
215 delete client;
216
217 return 0;
218}
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.
Definition: argparser.h:64
The BlackBoard abstract class.
Definition: blackboard.h:46
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.
Definition: exception.h:36
void print_trace() noexcept
Prints trace to stderr.
Definition: exception.cpp:601
Simple Fawkes network client.
Definition: client.h:52
void connect()
Connect to remote.
Definition: client.cpp:424
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:848
Laser360Interface Fawkes BlackBoard Interface.
MotorInterface Fawkes BlackBoard Interface.
Remote configuration via Fawkes net.
Definition: netconf.h:50
Remote BlackBoard.
Definition: remote.h:50
Receive transforms and answer queries.
Coordinate transforms between any two frames in a system.
Definition: transformer.h:65
Fawkes library namespace.