Commit 0b6b99d7 authored by olivier stasse's avatar olivier stasse
Browse files

Port to ROS. Tested under OpenHRP 3.0.7

parent 591fd2f7
......@@ -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)
......
......@@ -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
......
......@@ -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();
}
......
......@@ -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"
......
......@@ -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();
}
......
......@@ -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_;
};
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment