55 printf(
"Caught signal %d, stopping program!\n", signum);
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." 67 << arg <<
" -v <rcVisardIP> [-t <timePeriodSecs>]" << endl;
70 int main(
int argc,
char* argv[])
74 WSAStartup(MAKEWORD(2, 2), &wsaData);
84 string visardIP, networkIface =
"", streamName =
"";
85 bool userSetIp =
false;
86 unsigned int maxTimeSecs = 5;
91 std::string p = argv[i++];
93 if (p ==
"-v" && i < argc)
95 visardIP = string(argv[i++]);
98 else if (p ==
"-t" && i < argc)
100 maxTimeSecs = (
unsigned int)std::max(0, atoi(argv[i++]));
115 cerr <<
"Please specify rc_visard IP." << endl;
123 cout <<
"connecting rc_visard " << visardIP <<
"..." << endl;
129 cout <<
"starting rc_dynamics module with slam on rc_visard..." << endl;
130 rcvisardDynamics->startSlam();
134 cout <<
"ERROR! Could not start rc_dynamics module on rc_visard: " << e.what() << endl;
141 cout <<
"running..." << endl;
144 Sleep(1000 * maxTimeSecs);
146 usleep(1000 * 1000 * maxTimeSecs);
154 cout <<
"stopping rc_dynamics module on rc_visard..." << endl;
155 rcvisardDynamics->stop();
158 roboception::msgs::Trajectory traj = rcvisardDynamics->getSlamTrajectory();
159 cout <<
"The full trajectory contains " << traj.poses().size() <<
" waypoints." << endl;
163 cout <<
"The last second of the trajectory contains " << traj.poses().size() <<
" waypoints and looks like:" << endl
164 << traj.DebugString() << endl;
168 cout <<
"ERROR! Could not start rc_dynamics module on rc_visard: " << e.what() << endl;
static Ptr create(const std::string &rc_visard_ip, unsigned int requests_timeout=5000)
Creates a local instance of rc_visard's remote pose interface.
void signal_callback_handler(int signum)
static bool caught_signal
catching signals for proper program escape
int main(int argc, char *argv[])
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.