Fix a problem of integration in ART-OpenRTM.

The ros spinner is now multi-threaded. The interpreter and the ros-node
are NOT anymore started in the main thread (breaking ART).

CMakeLists.txt

@@ -56,7 +56,7 @@ ADD_REQUIRED_DEPENDENCY("dynamic-graph-python")
 ADD_ROSPACK_DEPENDENCY("dynamic_graph_bridge")
 # Search for dependencies.
 # Boost
-SET(BOOST_COMPONENTS filesystem system)
+SET(BOOST_COMPONENTS filesystem system thread)
 SEARCH_FOR_BOOST()
 
 # Handle rpath necessary to handle ROS multiplace packages

src/CMakeLists.txt

@@ -92,6 +92,11 @@ MACRO(build_hrp2_controller robotnumber)
 
   ROSPACK_USE_DEPENDENCY(${CONTROLLER_NAME} "dynamic_graph_bridge")
 
+  
+  IF(UNIX AND NOT APPLE)
+    TARGET_LINK_LIBRARIES(${CONTROLLER_NAME} ${Boost_LIBRARIES})
+  ENDIF(UNIX AND NOT APPLE)
+
   INSTALL(TARGETS ${CONTROLLER_NAME} DESTINATION lib)
 
   # build python submodule

src/sot-hrp2-10-controller.cpp

@@ -37,7 +37,7 @@ void SoTHRP2_10_Controller::startupPython()
   runPython
     (aof,
      "from dynamic_graph.sot.hrp2_10.prologue import robot, solver",
-     interpreter_);
+     *interpreter_);
   aof.close();
 }
 

src/sot-hrp2-14-controller.cpp

@@ -36,7 +36,7 @@ void SoTHRP2_14_Controller::startupPython()
   runPython
     (aof,
      "from dynamic_graph.sot.hrp2_14.prologue import robot, solver",
-     interpreter_);
+     *interpreter_);
   aof.close();
 }
 

src/sot-hrp2-controller.cpp

@@ -20,19 +20,44 @@
 #include <dynamic_graph_bridge/ros_init.hh>
 #include "sot-hrp2-controller.hh"
 
+#include <boost/thread/thread.hpp>
+#include <boost/thread/condition.hpp>
 const std::string SoTHRP2Controller::LOG_PYTHON="/tmp/HRP2Controller_python.out";
 
 using namespace std;
 
+boost::condition_variable cond;
+boost::mutex mut;
+bool data_ready;
+
+void workThread(SoTHRP2Controller *aSoTHRP2C)
+{
+  
+  dynamicgraph::Interpreter aLocalInterpreter(dynamicgraph::rosInit(false,true));
+
+  aSoTHRP2C->interpreter_ = 
+    boost::make_shared<dynamicgraph::Interpreter>(aLocalInterpreter);
+  std::cout << "Going through the thread." << std::endl;
+  {
+    boost::lock_guard<boost::mutex> lock(mut);
+    data_ready=true;
+  }
+  cond.notify_all();
+  ros::waitForShutdown();
+}
+
 SoTHRP2Controller::SoTHRP2Controller(std::string RobotName):
-  interpreter_(dynamicgraph::rosInit(false)),
   device_(RobotName)
 {
 
+  std::cout << "Going through SoTHRP2Controller." << std::endl;
+  boost::thread thr(workThread,this);
   sotDEBUG(25) << __FILE__ << ":" 
 	       << __FUNCTION__ <<"(#" 
 	       << __LINE__ << " )" << std::endl;
 
+  boost::unique_lock<boost::mutex> lock(mut);
+  cond.wait(lock);
 }
 
 SoTHRP2Controller::~SoTHRP2Controller()
@@ -105,15 +130,15 @@ void SoTHRP2Controller::
 startupPython()
 {
   std::ofstream aof(LOG_PYTHON.c_str());
-  runPython (aof, "import sys, os", interpreter_);
-  runPython (aof, "pythonpath = os.environ['PYTHONPATH']", interpreter_);
-  runPython (aof, "path = []", interpreter_);
+  runPython (aof, "import sys, os", *interpreter_);
+  runPython (aof, "pythonpath = os.environ['PYTHONPATH']", *interpreter_);
+  runPython (aof, "path = []", *interpreter_);
   runPython (aof,
 	     "for p in pythonpath.split(':'):\n"
 	     "  if p not in sys.path:\n"
-	     "    path.append(p)", interpreter_);
-  runPython (aof, "path.extend(sys.path)", interpreter_);
-  runPython (aof, "sys.path = path", interpreter_);
+	     "    path.append(p)", *interpreter_);
+  runPython (aof, "path.extend(sys.path)", *interpreter_);
+  runPython (aof, "sys.path = path", *interpreter_);
 
   // Calling again rosInit here to start the spinner. It will
   // deal with topics and services callbacks in a separate, non

src/sot-hrp2-controller.hh

@@ -48,6 +48,9 @@ class SoTHRP2Controller: public
 
   void getControl(std::map<std::string, dgsot::ControlValues> &anglesOut);
 
+  /// Embedded python interpreter accessible via Corba
+  boost::shared_ptr<dynamicgraph::Interpreter> interpreter_;
+
  protected:
   // Update output port with the control computed from the
   // dynamic graph.
@@ -59,8 +62,6 @@ class SoTHRP2Controller: public
   
   virtual void startupPython();
     
-  /// Embedded python interpreter accessible via Corba
-  dynamicgraph::Interpreter interpreter_;
 
   SoTHRP2Device device_;
 };