Port to ROS. Tested under OpenHRP 3.0.7

CMakeLists.txt

@@ -19,7 +19,7 @@ INCLUDE(cmake/base.cmake)
 INCLUDE(cmake/boost.cmake)
 INCLUDE(cmake/lapack.cmake)
 INCLUDE(cmake/cpack.cmake)
-
+INCLUDE(cmake/ros.cmake)
 
 SET(PROJECT_NAME sot-hrp2)
 SET(PROJECT_DESCRIPTION "dynamic-graph package for HRP-2 robot")
@@ -55,11 +55,19 @@ ADD_REQUIRED_DEPENDENCY("sot-dynamic")
 ADD_REQUIRED_DEPENDENCY("dynamic-graph-corba")
 ADD_REQUIRED_DEPENDENCY("dynamic-graph-python")
 
+ADD_ROSPACK_DEPENDENCY("dynamic_graph_bridge")
 # Search for dependencies.
 # Boost
 SET(BOOST_COMPONENTS filesystem system)
 SEARCH_FOR_BOOST()
 
+# Handle rpath necessary to handle ROS multiplace packages
+# libraries inclusion
+SET(CMAKE_SKIP_BUILD_RPATH  FALSE)
+SET(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
+SET(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib")
+SET(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
+
 # Add subdirectories.
 ADD_SUBDIRECTORY(src)
 ADD_SUBDIRECTORY(tests)

src/CMakeLists.txt

@@ -96,7 +96,9 @@ MACRO(build_hrp2_controller robotnumber)
   PKG_CONFIG_USE_DEPENDENCY(${CONTROLLER_NAME} "dynamic-graph-python")
   PKG_CONFIG_USE_DEPENDENCY(${CONTROLLER_NAME} "dynamic-graph-corba")
   PKG_CONFIG_USE_DEPENDENCY(${CONTROLLER_NAME} "sot-core")
- 
+
+  ROSPACK_USE_DEPENDENCY(${CONTROLLER_NAME} "dynamic_graph_bridge")
+
   INSTALL(TARGETS ${CONTROLLER_NAME} DESTINATION lib)
 
   # build python submodule

src/sot-hrp2-10-controller.cpp

@@ -38,7 +38,6 @@ void SoTHRP2_10_Controller::startupPython()
     (aof,
      "from dynamic_graph.sot.hrp2_10.prologue import robot, solver",
      interpreter_);
-  interpreter_.startCorbaServer ("openhrp", "", "stackOfTasks", "");
   aof.close();
 }
 

src/sot-hrp2-14-controller.cpp

@@ -37,7 +37,7 @@ void SoTHRP2_14_Controller::startupPython()
     (aof,
      "from dynamic_graph.sot.hrp2_14.prologue import robot, solver",
      interpreter_);
-  interpreter_.startCorbaServer ("openhrp", "", "stackOfTasks", "");
+  aof.close();
 }
 
 extern "C" 

src/sot-hrp2-controller.cpp

@@ -18,15 +18,14 @@
 #include <sot/core/debug.hh>
 #include <sot/core/exception-abstract.hh>
 #include "sot-hrp2-controller.hh"
-
-
+#include <dynamic_graph_bridge/ros_init.hh>
 
 const std::string SoTHRP2Controller::LOG_PYTHON="/tmp/HRP2Controller_python.out";
 
 using namespace std;
 
 SoTHRP2Controller::SoTHRP2Controller(std::string RobotName):
-  interpreter_(),
+  interpreter_(dynamicgraph::rosInit(false)),
   device_(RobotName)
 {
 
@@ -84,10 +83,10 @@ getControl(map<string,dgsot::ControlValues> &controlOut)
 void SoTHRP2Controller::
 runPython(std::ostream& file,
 	  const std::string& command,
-	  dynamicgraph::corba::Interpreter& interpreter)
+	  dynamicgraph::Interpreter& interpreter)
 {
   file << ">>> " << command << std::endl;
-  std::string value = interpreter.python (command);
+  std::string value = interpreter.runCommand(command);
   if (value != "None")
     file << value;
 }
@@ -95,7 +94,6 @@ runPython(std::ostream& file,
 void SoTHRP2Controller::
 startupPython()
 {
-  std::cout << "Went through startupPython()" << std::endl;
   std::ofstream aof(LOG_PYTHON.c_str());
   runPython (aof, "import sys, os", interpreter_);
   runPython (aof, "pythonpath = os.environ['PYTHONPATH']", interpreter_);
@@ -106,6 +104,13 @@ startupPython()
 	     "    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
+  // real-time thread. See roscpp documentation for more
+  // information.
+  dynamicgraph::rosInit (true);
+
   aof.close();
 }
 

src/sot-hrp2-controller.hh

@@ -22,7 +22,7 @@
 #include <dynamic-graph/signal.h>
 #include <dynamic-graph/signal-ptr.h>
 #include <dynamic-graph/linear-algebra.h>
-#include <dynamic-graph/corba/interpreter.hh>
+#include <dynamic_graph_bridge/ros_interpreter.hh>
 #include <sot/core/device.hh>
 #include <sot/core/abstract-sot-external-interface.hh>
 
@@ -55,12 +55,12 @@ class SoTHRP2Controller: public
   
   void runPython(std::ostream& file,
 		 const std::string& command,
-		 dynamicgraph::corba::Interpreter& interpreter);
+		 dynamicgraph::Interpreter& interpreter);
   
   virtual void startupPython();
     
   /// Embedded python interpreter accessible via Corba
-  dynamicgraph::corba::Interpreter interpreter_;
+  dynamicgraph::Interpreter interpreter_;
 
   SoTHRP2Device device_;
 };