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 
36 using namespace fawkes;
37 using namespace std;
38 
39 /** Print the usage message.
40  * @param program_name The path of the program.
41  */
42 void
43 print_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  */
64 int
65 main(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 }
TimeOffsetCalibration
Definition: time_offset_calibration.h:25
RollCalibration
Definition: roll_calibration.h:25
fawkes::FawkesNetworkClient::connect
void connect()
Connect to remote.
Definition: client.cpp:428
fawkes::tf::TransformListener
Definition: transform_listener.h:73
fawkes::tf::Transformer
Definition: transformer.h:71
fawkes::BlackBoard
Definition: blackboard.h:48
PitchCalibration
Definition: pitch_calibration.h:25
fawkes::RemoteBlackBoard
Definition: remote.h:51
fawkes::MotorInterface
Definition: MotorInterface.h:37
fawkes::NetworkConfiguration
Definition: netconf.h:53
fawkes
fawkes::ArgumentParser
Definition: argparser.h:67
fawkes::Interface::has_writer
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:817
fawkes::Exception::print_trace
void print_trace()
Prints trace to stderr.
Definition: exception.cpp:600
YawCalibration
Definition: yaw_calibration.h:27
fawkes::BlackBoard::open_for_reading
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
fawkes::Laser360Interface
Definition: Laser360Interface.h:37
fawkes::FawkesNetworkClient
Definition: client.h:55
fawkes::Exception
Definition: exception.h:39