Commit 7a24cd7c authored by olivier stasse's avatar olivier stasse
Browse files

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).
parent a4809043
......@@ -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
......
......@@ -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
......
......@@ -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();
}
......
......@@ -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();
}
......
......@@ -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
......
......@@ -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_;
};
......
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