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 <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>
27 #include <tf/types.h>
28 #include <config/netconf.h>
29 
30 #include <cstdio>
31 #include <cmath>
32 #include <unistd.h>
33 
34 #include <interfaces/LocalizationInterface.h>
35 
36 using namespace fawkes;
37 
38 void
39 print_usage(const char *program_name)
40 {
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",
51  program_name);
52 }
53 
54 void
55 try_localize(const std::string &host, unsigned short int port, std::string &interface_id,
56  std::string frame,
57  double translation[3], double rotation[4], double covariance[36])
58 {
59  FawkesNetworkClient *c = new FawkesNetworkClient(host.c_str(), port);
60  c->connect();
61 
62  if (frame == "") {
63  NetworkConfiguration *netconf = NULL;
64  try {
65  netconf = new NetworkConfiguration(c);
66  frame = netconf->get_string("/frames/fixed");
67  } catch (Exception &e) {
68  printf("WARNING: no frame set and failed to get frame from remote.\n");
69  e.print_trace();
70  }
71  delete netconf;
72  }
73 
74  BlackBoard *bb = new RemoteBlackBoard(c);
75  LocalizationInterface *loc_if =
76  bb->open_for_reading<LocalizationInterface>(interface_id.c_str());
77 
78  if (! loc_if->has_writer()) {
79  bb->close(loc_if);
80  delete bb;
81  throw Exception("No writer for interface %s, aborting",
82  loc_if->uid());
83  }
84 
87  ipm->set_frame(frame.c_str());
88  ipm->set_translation(translation);
89  ipm->set_rotation(rotation);
90  ipm->set_covariance(covariance);
91  loc_if->msgq_enqueue(ipm);
92 
93  // allow for some time so message is actually sent
94  usleep(500000);
95 
96  bb->close(loc_if);
97  delete bb;
98  delete c;
99 }
100 
101 
102 /** Config tool main.
103  * @param argc argument count
104  * @param argv arguments
105  */
106 int
107 main(int argc, char **argv)
108 {
109  ArgumentParser argp(argc, argv, "hr:i:t:");
110 
111  if ( argp.has_arg("h") ) {
112  print_usage(argv[0]);
113  exit(0);
114  }
115 
116  char *host_s = (char *)"localhost";
117  unsigned short int port = 1910;
118  bool free_host = argp.parse_hostport("r", &host_s, &port);
119  float try_sec = 0.0;
120 
121  std::string host = host_s;
122  if ( free_host ) free(host_s);
123 
124  std::string interface_id = "AMCL";
125  if (argp.has_arg("i")) {
126  interface_id = argp.arg("i");
127  }
128 
129  std::string frame;
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;
137 
138  if (argp.num_items() != 3 && argp.num_items() != 7) {
139  fprintf(stderr, "Invalid pose");
140  print_usage(argv[0]);
141  return -1;
142  }
143 
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];
149  } else {
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);
152  }
153 
154  if (argp.has_arg("t")) {
155  try_sec = argp.parse_float("t");
156  }
157 
158  if (argp.has_arg("f")) {
159  frame = argp.arg("f");
160  }
161 
162  fawkes::Time start;
163  fawkes::Time now(start);
164  bool localized = false;
165  while (! localized) {
166  now.stamp();
167  try {
168  try_localize(host, port, interface_id, frame, translation, rotation, covariance);
169  localized = true;
170  } catch (Exception &e) {
171  if ((now - &start) > try_sec) {
172  fprintf(stderr, "Failed to localize %s:%u: %s\n",
173  host.c_str(), port, e.what_no_backtrace());
174  break;
175  }
176  }
177  }
178 
179  return 0;
180 }
LocalizationInterface Fawkes BlackBoard Interface.
Simple Fawkes network client.
Definition: client.h:52
Fawkes library namespace.
void connect()
Connect to remote.
Definition: client.cpp:417
Parse command line arguments.
Definition: argparser.h:66
A class for handling time.
Definition: time.h:91
virtual std::string get_string(const char *path)
Get value from configuration which is of type string.
Definition: netconf.cpp:482
Base class for exceptions in Fawkes.
Definition: exception.h:36
SetInitialPoseMessage Fawkes BlackBoard Interface Message.
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:834
const char * uid() const
Get unique identifier of interface.
Definition: interface.cpp:687
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:686
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
Definition: interface.cpp:903
void print_trace()
Prints trace to stderr.
Definition: exception.cpp:619
Remote BlackBoard.
Definition: remote.h:48
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.
Definition: blackboard.h:48
Remote configuration via Fawkes net.
Definition: netconf.h:49
virtual void close(Interface *interface)=0
Close interface.