Revert "Remove useless thread to start and stop dynamic graph"

This reverts commit 924c301.

Author: olivier-stasse

include/dynamic_graph_bridge/sot_loader_basic.hh

@@ -72,12 +72,6 @@ protected:
   /// \brief Coefficient between parallel joints and the state vector.
   std::vector<double> coefficient_parallel_joints_;
 
-  /// Advertises start_dynamic_graph services
-  ros::ServiceServer service_start_;
-
-  /// Advertises stop_dynamic_graph services
-  ros::ServiceServer service_stop_;
-
   // Joint state publication.
   ros::Publisher joint_pub_;
 
@@ -88,6 +82,7 @@ protected:
   int nbOfJoints_;
   parallel_joints_to_state_vector_t::size_type nbOfParallelJoints_;
 
+
 public:
   SotLoaderBasic();
   ~SotLoaderBasic() {};

src/sot_loader_basic.cpp

@@ -37,6 +37,24 @@ using namespace std;
 using namespace dynamicgraph::sot; 
 namespace po = boost::program_options;
 
+void createRosSpin(SotLoaderBasic *aSotLoaderBasic)
+{
+  ROS_INFO("createRosSpin started\n");
+  ros::NodeHandle n;
+
+  ros::ServiceServer service = n.advertiseService("start_dynamic_graph",
+                                                  &SotLoaderBasic::start_dg,
+                                                  aSotLoaderBasic);
+
+  ros::ServiceServer service2 = n.advertiseService("stop_dynamic_graph",
+                                                  &SotLoaderBasic::stop_dg,
+                                                  aSotLoaderBasic);
+
+
+  ros::waitForShutdown();
+}
+
+
 SotLoaderBasic::SotLoaderBasic():
   dynamic_graph_stopped_(true),
   sotRobotControllerLibrary_(0)
@@ -60,15 +78,8 @@ int SotLoaderBasic::initPublication()
 void SotLoaderBasic::initializeRosNode(int , char *[])
 {
   ROS_INFO("Ready to start dynamic graph.");
-  ros::NodeHandle n;
-
-  service_start_ = n.advertiseService("start_dynamic_graph",
-                                     &SotLoaderBasic::start_dg,
-                                     this);
-
-  service_stop_  = n.advertiseService("stop_dynamic_graph",
-                                     &SotLoaderBasic::stop_dg,
-                                     this);
+  boost::unique_lock<boost::mutex> lock(mut);
+  boost::thread thr2(createRosSpin,this);
 
 }