1 |
|
|
/* |
2 |
|
|
* Copyright 2011, |
3 |
|
|
* Olivier Stasse, |
4 |
|
|
* |
5 |
|
|
* CNRS |
6 |
|
|
* |
7 |
|
|
*/ |
8 |
|
|
#include <ros/console.h> |
9 |
|
|
|
10 |
|
|
#include <iostream> |
11 |
|
|
|
12 |
|
|
#define ENABLE_RT_LOG |
13 |
|
|
#include <dynamic-graph/real-time-logger.h> |
14 |
|
|
|
15 |
|
|
#include <dynamic_graph_bridge/sot_loader.hh> |
16 |
|
|
|
17 |
|
|
int main(int argc, char *argv[]) { |
18 |
|
|
::dynamicgraph::RealTimeLogger::instance().addOutputStream( |
19 |
|
|
::dynamicgraph::LoggerStreamPtr_t( |
20 |
|
|
new dynamicgraph::LoggerIOStream(std::cout))); |
21 |
|
|
|
22 |
|
|
ros::init(argc, argv, "sot_ros_encapsulator"); |
23 |
|
|
SotLoader aSotLoader; |
24 |
|
|
if (aSotLoader.parseOptions(argc, argv) < 0) return -1; |
25 |
|
|
|
26 |
|
|
// Advertize service "(start|stop)_dynamic_graph" and |
27 |
|
|
// load parameter "robot_description in SoT. |
28 |
|
|
aSotLoader.initializeRosNode(argc, argv); |
29 |
|
|
// Load dynamic library and run python prologue. |
30 |
|
|
aSotLoader.Initialization(); |
31 |
|
|
|
32 |
|
|
ros::waitForShutdown(); |
33 |
|
|
return 0; |
34 |
|
|
} |