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>
29 #include <utils/system/argparser.h>
38 print_usage(
const char *program_name)
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",
55 try_localize(
const std::string &host,
56 unsigned short int port,
57 std::string & interface_id,
59 double translation[3],
61 double covariance[36])
72 printf(
"WARNING: no frame set and failed to get frame from remote.\n");
84 throw Exception(
"No writer for interface %s, aborting", loc_if->
uid());
108 main(
int argc,
char **argv)
112 if (argp.has_arg(
"h")) {
113 print_usage(argv[0]);
117 char * host_s = (
char *)
"localhost";
118 unsigned short int port = 1910;
119 bool free_host = argp.parse_hostport(
"r", &host_s, &port);
122 std::string host = host_s;
126 std::string interface_id =
"AMCL";
127 if (argp.has_arg(
"i")) {
128 interface_id = argp.arg(
"i");
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)
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;
141 if (argp.num_items() != 3 && argp.num_items() != 7) {
142 fprintf(stderr,
"Invalid pose");
143 print_usage(argv[0]);
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)
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);
160 if (argp.has_arg(
"t")) {
161 try_sec = argp.parse_float(
"t");
164 if (argp.has_arg(
"f")) {
165 frame = argp.arg(
"f");
170 bool localized =
false;
174 try_localize(host, port, interface_id, frame, translation, rotation, covariance);
177 if ((now - &start) > try_sec) {
179 stderr,
"Failed to localize %s:%u: %s\n", host.c_str(), port, e.
what_no_backtrace());
186 return localized ? 0 : -1;
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.
virtual void close(Interface *interface)=0
Close interface.
Base class for exceptions in Fawkes.
void print_trace() noexcept
Prints trace to stderr.
virtual const char * what_no_backtrace() const noexcept
Get primary string (does not implicitly print the back trace).
Simple Fawkes network client.
void connect()
Connect to remote.
unsigned int msgq_enqueue(Message *message, bool proxy=false)
Enqueue message at end of queue.
const char * uid() const
Get unique identifier of interface.
bool has_writer() const
Check if there is a writer for the interface.
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.
virtual std::string get_string(const char *path)
Get value from configuration which is of type string.
A class for handling time.
Fawkes library namespace.