test_slam.cc
Go to the documentation of this file.
1 /*
2  * This file is part of the rc_dynamics_api package.
3  *
4  * Copyright (c) 2017 Roboception GmbH
5  * All rights reserved
6  *
7  * Author: Christian Emmerich
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions are met:
11  *
12  * 1. Redistributions of source code must retain the above copyright notice,
13  * this list of conditions and the following disclaimer.
14  *
15  * 2. Redistributions in binary form must reproduce the above copyright notice,
16  * this list of conditions and the following disclaimer in the documentation
17  * and/or other materials provided with the distribution.
18  *
19  * 3. Neither the name of the copyright holder nor the names of its contributors
20  * may be used to endorse or promote products derived from this software without
21  * specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
24  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
25  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
26  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
27  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
28  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
29  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
30  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
31  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
32  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  */
35 
37 
38 #include <signal.h>
39 
40 #ifdef WIN32
41 #include <winsock2.h>
42 #undef min
43 #undef max
44 #endif
45 
46 using namespace std;
47 namespace rcdyn = rc::dynamics;
48 
52 static bool caught_signal = false;
53 void signal_callback_handler(int signum)
54 {
55  printf("Caught signal %d, stopping program!\n", signum);
56  caught_signal = true;
57 }
58 
62 void printUsage(char* arg)
63 {
64  cout << "\nStarts rc_visard's dynamics and slam modules for a certain time "
65  "\nperiod, retrieves the Slam trajectory and simply prints it to std out."
66  << "\n\nUsage: \n"
67  << arg << " -v <rcVisardIP> [-t <timePeriodSecs>]" << endl;
68 }
69 
70 int main(int argc, char* argv[])
71 {
72 #ifdef WIN32
73  WSADATA wsaData;
74  WSAStartup(MAKEWORD(2, 2), &wsaData);
75 #endif
76 
77  // Register signals and signal handler for proper program escape
78  signal(SIGINT, signal_callback_handler);
79  signal(SIGTERM, signal_callback_handler);
80 
84  string visardIP, networkIface = "", streamName = "";
85  bool userSetIp = false;
86  unsigned int maxTimeSecs = 5;
87 
88  int i = 1;
89  while (i < argc)
90  {
91  std::string p = argv[i++];
92 
93  if (p == "-v" && i < argc)
94  {
95  visardIP = string(argv[i++]);
96  userSetIp = true;
97  }
98  else if (p == "-t" && i < argc)
99  {
100  maxTimeSecs = (unsigned int)std::max(0, atoi(argv[i++]));
101  }
102  else if (p == "-h")
103  {
104  printUsage(argv[0]);
105  return EXIT_SUCCESS;
106  }
107  else
108  {
109  printUsage(argv[0]);
110  return EXIT_FAILURE;
111  }
112  }
113  if (!userSetIp)
114  {
115  cerr << "Please specify rc_visard IP." << endl;
116  printUsage(argv[0]);
117  return EXIT_FAILURE;
118  }
119 
123  cout << "connecting rc_visard " << visardIP << "..." << endl;
124  auto rcvisardDynamics = rcdyn::RemoteInterface::create(visardIP);
125 
126  try
127  {
128  // start the rc::dynamics module with slam on the rc_visard
129  cout << "starting rc_dynamics module with slam on rc_visard..." << endl;
130  rcvisardDynamics->startSlam();
131  }
132  catch (exception& e)
133  {
134  cout << "ERROR! Could not start rc_dynamics module on rc_visard: " << e.what() << endl;
135  return EXIT_FAILURE;
136  }
137 
141  cout << "running..." << endl;
142 
143 #ifdef WIN32
144  Sleep(1000 * maxTimeSecs);
145 #else
146  usleep(1000 * 1000 * maxTimeSecs);
147 #endif
148 
152  try
153  {
154  cout << "stopping rc_dynamics module on rc_visard..." << endl;
155  rcvisardDynamics->stop();
156 
157  // get the full trajectory and print number of recorded poses
158  roboception::msgs::Trajectory traj = rcvisardDynamics->getSlamTrajectory();
159  cout << "The full trajectory contains " << traj.poses().size() << " waypoints." << endl;
160 
161  // print the last second of the trajectory to cout
162  traj = rcvisardDynamics->getSlamTrajectory(rc::TrajectoryTime::RelativeToEnd(1));
163  cout << "The last second of the trajectory contains " << traj.poses().size() << " waypoints and looks like:" << endl
164  << traj.DebugString() << endl;
165  }
166  catch (exception& e)
167  {
168  cout << "ERROR! Could not start rc_dynamics module on rc_visard: " << e.what() << endl;
169  }
170 
171 #ifdef WIN32
172  ::WSACleanup();
173 #endif
174 
175  return EXIT_SUCCESS;
176 }
static Ptr create(const std::string &rc_visard_ip, unsigned int requests_timeout=5000)
Creates a local instance of rc_visard&#39;s remote pose interface.
void signal_callback_handler(int signum)
Definition: test_slam.cc:53
static bool caught_signal
catching signals for proper program escape
Definition: test_slam.cc:52
int main(int argc, char *argv[])
Definition: test_slam.cc:70
static TrajectoryTime RelativeToEnd(unsigned long sec=0, unsigned long nsec=0)
Creates a time stamp from the given values as an offset from the end point of the trajectory...
void printUsage(char *arg)
Print usage of example including command line args.
Definition: test_slam.cc:62


rc_dynamics_api
Author(s): Heiko Hirschmueller , Christian Emmerich , Felix Endres
autogenerated on Thu May 9 2019 02:51:36