22 #include <netcomm/fawkes/client.h> 23 #include <blackboard/remote.h> 24 #include <utils/system/argparser.h> 25 #include <core/threading/thread.h> 26 #include <netcomm/fawkes/client_handler.h> 28 #include <config/netconf.h> 34 #include <interfaces/LocalizationInterface.h> 39 print_usage(
const char *program_name)
41 printf(
"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,
unsigned short int port, std::string &interface_id,
57 double translation[3],
double rotation[4],
double covariance[36])
68 printf(
"WARNING: no frame set and failed to get frame from remote.\n");
81 throw Exception(
"No writer for interface %s, aborting",
87 ipm->set_frame(frame.c_str());
88 ipm->set_translation(translation);
89 ipm->set_rotation(rotation);
90 ipm->set_covariance(covariance);
107 main(
int argc,
char **argv)
111 if ( argp.has_arg(
"h") ) {
112 print_usage(argv[0]);
116 char *host_s = (
char *)
"localhost";
117 unsigned short int port = 1910;
118 bool free_host = argp.parse_hostport(
"r", &host_s, &port);
121 std::string host = host_s;
122 if ( free_host ) free(host_s);
124 std::string interface_id =
"AMCL";
125 if (argp.has_arg(
"i")) {
126 interface_id = argp.arg(
"i");
130 double translation[3] = {0, 0, 0};
131 double rotation[4] = {0, 0, 0, 1};
132 double covariance[36];
133 for (
int i = 0; i < 36; ++i) covariance[i] = 0.f;
134 covariance[6*0+0] = 0.5 * 0.5;
135 covariance[6*1+1] = 0.5 * 0.5;
136 covariance[6*5+5] = M_PI/12.0 * M_PI/12.0;
138 if (argp.num_items() != 3 && argp.num_items() != 7) {
139 fprintf(stderr,
"Invalid pose");
140 print_usage(argv[0]);
144 if (argp.num_items() == 3) {
145 translation[0] = argp.parse_item_float(0);
146 translation[1] = argp.parse_item_float(1);
147 tf::Quaternion q = tf::create_quaternion_from_yaw(argp.parse_item_float(2));
148 for (
int i = 0; i < 4; ++i) rotation[i] = q[i];
150 for (
int i = 0; i < 3; ++i) translation[i] = argp.parse_item_float(i);
151 for (
int i = 0; i < 4; ++i) rotation[i] = argp.parse_item_float(i+3);
154 if (argp.has_arg(
"t")) {
155 try_sec = argp.parse_float(
"t");
158 if (argp.has_arg(
"f")) {
159 frame = argp.arg(
"f");
164 bool localized =
false;
165 while (! localized) {
168 try_localize(host, port, interface_id, frame, translation, rotation, covariance);
171 if ((now - &start) > try_sec) {
172 fprintf(stderr,
"Failed to localize %s:%u: %s\n",
LocalizationInterface Fawkes BlackBoard Interface.
Simple Fawkes network client.
Fawkes library namespace.
void connect()
Connect to remote.
Parse command line arguments.
A class for handling time.
virtual std::string get_string(const char *path)
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
SetInitialPoseMessage Fawkes BlackBoard Interface Message.
bool has_writer() const
Check if there is a writer for the interface.
const char * uid() const
Get unique identifier of interface.
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
void print_trace()
Prints trace to stderr.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
The BlackBoard abstract class.
Remote configuration via Fawkes net.
virtual void close(Interface *interface)=0
Close interface.