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 
35 using namespace fawkes;
36 
37 void
38 print_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 
54 void
55 try_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  */
107 int
108 main(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 }
fawkes::FawkesNetworkClient::connect
void connect()
Connect to remote.
Definition: client.cpp:430
fawkes::BlackBoard
Definition: blackboard.h:50
fawkes::LocalizationInterface::SetInitialPoseMessage
Definition: LocalizationInterface.h:65
fawkes::LocalizationInterface::SetInitialPoseMessage::set_rotation
void set_rotation(unsigned int index, const double new_rotation)
Set rotation value at given index.
Definition: LocalizationInterface.cpp:296
fawkes::RemoteBlackBoard
Definition: remote.h:53
fawkes::BlackBoard::close
virtual void close(Interface *interface)=0
fawkes::LocalizationInterface
Definition: LocalizationInterface.h:39
fawkes::NetworkConfiguration
Definition: netconf.h:55
fawkes::LocalizationInterface::SetInitialPoseMessage::set_covariance
void set_covariance(unsigned int index, const double new_covariance)
Set covariance value at given index.
Definition: LocalizationInterface.cpp:440
fawkes
fawkes::ArgumentParser
Definition: argparser.h:69
fawkes::NetworkConfiguration::get_string
virtual std::string get_string(const char *path)
Definition: netconf.cpp:463
fawkes::Interface::has_writer
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:819
fawkes::Exception::print_trace
void print_trace()
Prints trace to stderr.
Definition: exception.cpp:601
fawkes::LocalizationInterface::SetInitialPoseMessage::set_frame
void set_frame(const char *new_frame)
Set frame value.
Definition: LocalizationInterface.cpp:231
fawkes::Interface::uid
const char * uid() const
Get unique identifier of interface.
Definition: interface.cpp:679
fawkes::Exception::what_no_backtrace
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:663
fawkes::Time
Definition: time.h:98
fawkes::BlackBoard::open_for_reading
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
fawkes::Interface::msgq_enqueue
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
Definition: interface.cpp:884
fawkes::FawkesNetworkClient
Definition: client.h:57
fawkes::LocalizationInterface::SetInitialPoseMessage::set_translation
void set_translation(unsigned int index, const double new_translation)
Set translation value at given index.
Definition: LocalizationInterface.cpp:362
fawkes::Exception
Definition: exception.h:41