Fawkes API Fawkes Development Version
ffset_pose.cpp
1
2/***************************************************************************
3 * ffset_pose.cpp - tool to set pose
4 *
5 * Created: Mon Mar 23 14:20:07 2015
6 * Copyright 2015 Tim Niemueller [www.niemueller.de]
7 ****************************************************************************/
8
9/* This program is free software; you can redistribute it and/or modify
10 * it under the terms of the GNU General Public License as published by
11 * the Free Software Foundation; either version 2 of the License, or
12 * (at your option) any later version.
13 *
14 * This program is distributed in the hope that it will be useful,
15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17 * GNU Library General Public License for more details.
18 *
19 * Read the full text in the LICENSE.GPL file in the doc directory.
20 */
21
22#include <blackboard/remote.h>
23#include <config/netconf.h>
24#include <core/threading/thread.h>
25#include <interfaces/LocalizationInterface.h>
26#include <netcomm/fawkes/client.h>
27#include <netcomm/fawkes/client_handler.h>
28#include <tf/types.h>
29#include <utils/system/argparser.h>
30
31#include <cmath>
32#include <cstdio>
33#include <unistd.h>
34
35using namespace fawkes;
36
37void
38print_usage(const char *program_name)
39{
40 printf(
41 "Usage: %s [-h] [-r host[:port]] [-i ID] [-t SEC] -f FRAME -- <x y theta|x y z qx qy qz qw>\n"
42 " -h This help message\n"
43 " -r host[:port] Remote host (and optionally port) to connect to\n"
44 " -i ID Blackboard interface ID, defaults to \"AMCL\"\n"
45 " -t SEC Try connecting for SEC seconds\n"
46 " -f FRAME Frame in which the coordinates are given, defaults to /map\n"
47 "<x y theta> Set 2D position on ground plane with coordinates\n"
48 " (x,y) and orientation theta.\n"
49 "<x y z qx qy qz qw> Set full 3D pose with position (x,y,z) and\n"
50 " orientation quaternion (qx,qy,qz,qw)\n",
51 program_name);
52}
53
54void
55try_localize(const std::string &host,
56 unsigned short int port,
57 std::string & interface_id,
58 std::string frame,
59 double translation[3],
60 double rotation[4],
61 double covariance[36])
62{
63 FawkesNetworkClient *c = new FawkesNetworkClient(host.c_str(), port);
64 c->connect();
65
66 if (frame == "") {
67 NetworkConfiguration *netconf = NULL;
68 try {
69 netconf = new NetworkConfiguration(c);
70 frame = netconf->get_string("/frames/fixed");
71 } catch (Exception &e) {
72 printf("WARNING: no frame set and failed to get frame from remote.\n");
73 e.print_trace();
74 }
75 delete netconf;
76 }
77
78 BlackBoard * bb = new RemoteBlackBoard(c);
79 LocalizationInterface *loc_if = bb->open_for_reading<LocalizationInterface>(interface_id.c_str());
80
81 if (!loc_if->has_writer()) {
82 bb->close(loc_if);
83 delete bb;
84 throw Exception("No writer for interface %s, aborting", loc_if->uid());
85 }
86
89 ipm->set_frame(frame.c_str());
90 ipm->set_translation(translation);
91 ipm->set_rotation(rotation);
92 ipm->set_covariance(covariance);
93 loc_if->msgq_enqueue(ipm);
94
95 // allow for some time so message is actually sent
96 usleep(500000);
97
98 bb->close(loc_if);
99 delete bb;
100 delete c;
101}
102
103/** Config tool main.
104 * @param argc argument count
105 * @param argv arguments
106 */
107int
108main(int argc, char **argv)
109{
110 ArgumentParser argp(argc, argv, "hr:i:t:f:");
111
112 if (argp.has_arg("h")) {
113 print_usage(argv[0]);
114 exit(0);
115 }
116
117 char * host_s = (char *)"localhost";
118 unsigned short int port = 1910;
119 bool free_host = argp.parse_hostport("r", &host_s, &port);
120 float try_sec = 0.0;
121
122 std::string host = host_s;
123 if (free_host)
124 free(host_s);
125
126 std::string interface_id = "AMCL";
127 if (argp.has_arg("i")) {
128 interface_id = argp.arg("i");
129 }
130
131 std::string frame;
132 double translation[3] = {0, 0, 0};
133 double rotation[4] = {0, 0, 0, 1};
134 double covariance[36];
135 for (int i = 0; i < 36; ++i)
136 covariance[i] = 0.f;
137 covariance[6 * 0 + 0] = 0.5 * 0.5;
138 covariance[6 * 1 + 1] = 0.5 * 0.5;
139 covariance[6 * 5 + 5] = M_PI / 12.0 * M_PI / 12.0;
140
141 if (argp.num_items() != 3 && argp.num_items() != 7) {
142 fprintf(stderr, "Invalid pose");
143 print_usage(argv[0]);
144 return -1;
145 }
146
147 if (argp.num_items() == 3) {
148 translation[0] = argp.parse_item_float(0);
149 translation[1] = argp.parse_item_float(1);
150 tf::Quaternion q = tf::create_quaternion_from_yaw(argp.parse_item_float(2));
151 for (int i = 0; i < 4; ++i)
152 rotation[i] = q[i];
153 } else {
154 for (int i = 0; i < 3; ++i)
155 translation[i] = argp.parse_item_float(i);
156 for (int i = 0; i < 4; ++i)
157 rotation[i] = argp.parse_item_float(i + 3);
158 }
159
160 if (argp.has_arg("t")) {
161 try_sec = argp.parse_float("t");
162 }
163
164 if (argp.has_arg("f")) {
165 frame = argp.arg("f");
166 }
167
168 fawkes::Time start;
169 fawkes::Time now(start);
170 bool localized = false;
171 while (!localized) {
172 now.stamp();
173 try {
174 try_localize(host, port, interface_id, frame, translation, rotation, covariance);
175 localized = true;
176 } catch (Exception &e) {
177 if ((now - &start) > try_sec) {
178 fprintf(
179 stderr, "Failed to localize %s:%u: %s\n", host.c_str(), port, e.what_no_backtrace());
180 break;
181 }
182 usleep(1000000);
183 }
184 }
185
186 return localized ? 0 : -1;
187}
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.
virtual void close(Interface *interface)=0
Close interface.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void print_trace() noexcept
Prints trace to stderr.
Definition: exception.cpp:601
virtual const char * what_no_backtrace() const noexcept
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:663
Simple Fawkes network client.
Definition: client.h:52
void connect()
Connect to remote.
Definition: client.cpp:424
unsigned int msgq_enqueue(Message *message, bool proxy=false)
Enqueue message at end of queue.
Definition: interface.cpp:915
const char * uid() const
Get unique identifier of interface.
Definition: interface.cpp:686
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:848
SetInitialPoseMessage Fawkes BlackBoard Interface Message.
void set_translation(unsigned int index, const double new_translation)
Set translation value at given index.
void set_rotation(unsigned int index, const double new_rotation)
Set rotation value at given index.
void set_frame(const char *new_frame)
Set frame value.
void set_covariance(unsigned int index, const double new_covariance)
Set covariance value at given index.
LocalizationInterface Fawkes BlackBoard Interface.
Remote configuration via Fawkes net.
Definition: netconf.h:50
virtual std::string get_string(const char *path)
Get value from configuration which is of type string.
Definition: netconf.cpp:463
Remote BlackBoard.
Definition: remote.h:50
A class for handling time.
Definition: time.h:93
Fawkes library namespace.