From 4b5e6e010c87eff1281b07470648a17bea049f93 Mon Sep 17 00:00:00 2001
From: Thomas Moulard <thomas.moulard@gmail.com>
Date: Tue, 28 Feb 2012 00:57:34 +0100
Subject: [PATCH] Implement RunCommand service.
---
.gitignore | 1 +
CMakeLists.txt | 18 +++++
.../dynamic_graph_bridge/ros_interpreter.hh | 38 ++++++++++
scripts/dgbridge-remote | 74 +++++++++++++++++++
src/interpreter.cpp | 9 +++
src/ros_interpreter.cpp | 27 +++++++
srv/RunCommand.srv | 5 ++
7 files changed, 172 insertions(+)
create mode 100644 include/dynamic_graph_bridge/ros_interpreter.hh
create mode 100755 scripts/dgbridge-remote
create mode 100644 src/interpreter.cpp
create mode 100644 src/ros_interpreter.cpp
create mode 100644 srv/RunCommand.srv
diff --git a/.gitignore b/.gitignore
index 74effdc..f51b7cb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -5,3 +5,4 @@ msg_gen/
src/dynamic_graph_bridge/
bin/
build/
+srv_gen/
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1ecec3b..2f4dd41 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -11,11 +11,13 @@ set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
rosbuild_genmsg()
+rosbuild_gensrv()
rosbuild_add_boost_directories()
pkg_check_modules(JRL_MAL REQUIRED jrl-mal)
pkg_check_modules(DYNAMIC_GRAPH REQUIRED dynamic-graph)
+pkg_check_modules(DYNAMIC_GRAPH_PYTHON REQUIRED dynamic-graph-python)
pkg_check_modules(SOT_CORE REQUIRED sot-core)
include_directories(include)
@@ -60,12 +62,28 @@ target_link_libraries(ros_joint_state ros_bridge)
target_link_libraries(ros_joint_state
${JRL_MAL_LIBRARIES} ${DYNAMIC_GRAPH_LIBRARIES} ${SOT_CORE_LIBRARIES})
+rosbuild_add_library(ros_interpreter
+ src/ros_interpreter.cpp)
+rosbuild_add_compile_flags(ros_interpreter
+ ${JRL_MAL_CFLAGS} ${DYNAMIC_GRAPH_CFLAGS} ${DYNAMIC_GRAPH_PYTHON_CFLAGS}
+ ${SOT_CORE_CFLAGS})
+rosbuild_add_link_flags(ros_interpreter ${JRL_MAL_LDFLAGS}
+ ${DYNAMIC_GRAPH_LDFLAGS} ${DYNAMIC_GRAPH_PYTHON_LDFLAGS} ${SOT_CORE_LDFLAGS})
+target_link_libraries(ros_interpreter ros_bridge)
+target_link_libraries(ros_interpreter
+ ${JRL_MAL_LIBRARIES} ${DYNAMIC_GRAPH_LIBRARIES}
+ ${DYNAMIC_GRAPH_PYTHON_LIBRARIES} ${SOT_CORE_LIBRARIES})
+# Stand alone remote dynamic-graph Python interpreter.
+rosbuild_add_executable(interpreter src/interpreter.cpp)
+target_link_libraries(interpreter ros_interpreter)
INSTALL(TARGETS ros_bridge DESTINATION lib)
INSTALL(TARGETS ros_import DESTINATION lib)
INSTALL(TARGETS ros_export DESTINATION lib)
INSTALL(TARGETS ros_joint_state DESTINATION lib)
+INSTALL(TARGETS interpreter DESTINATION lib)
+INSTALL(TARGETS ros_interpreter DESTINATION bin)
INCLUDE(cmake/python.cmake)
diff --git a/include/dynamic_graph_bridge/ros_interpreter.hh b/include/dynamic_graph_bridge/ros_interpreter.hh
new file mode 100644
index 0000000..ed6173e
--- /dev/null
+++ b/include/dynamic_graph_bridge/ros_interpreter.hh
@@ -0,0 +1,38 @@
+#ifndef DYNAMIC_GRAPH_BRIDGE_INTERPRETER_HH
+# define DYNAMIC_GRAPH_BRIDGE_INTERPRETER_HH
+# include <ros/ros.h>
+# include <dynamic_graph_bridge/RunCommand.h>
+
+# include <dynamic-graph/python/interpreter.hh>
+
+namespace dynamicgraph
+{
+ /// \brief This class wraps the implementation of the runCommand
+ /// service.
+ ///
+ /// This takes as input a ROS node handle and do not handle the
+ /// callback so that the service behavior can be controlled from
+ /// the outside.
+ class Interpreter
+ {
+ public:
+ typedef boost::function<
+ bool (dynamic_graph_bridge::RunCommand::Request&,
+ dynamic_graph_bridge::RunCommand::Response&)>
+ runCommandCallback_t;
+
+ explicit Interpreter (ros::NodeHandle& nodeHandle);
+
+ protected:
+ /// \brief Run a Python command and return result, stderr and stdout.
+ bool runCommandCallback (dynamic_graph_bridge::RunCommand::Request& req,
+ dynamic_graph_bridge::RunCommand::Response& res);
+
+ private:
+ python::Interpreter interpreter_;
+ ros::NodeHandle& nodeHandle_;
+ ros::ServiceServer runCommandSrv_;
+ };
+} // end of namespace dynamicgraph.
+
+#endif //! DYNAMIC_GRAPH_BRIDGE_INTERPRETER_HH
diff --git a/scripts/dgbridge-remote b/scripts/dgbridge-remote
new file mode 100755
index 0000000..9aac7c2
--- /dev/null
+++ b/scripts/dgbridge-remote
@@ -0,0 +1,74 @@
+#!/usr/bin/env python
+
+import roslib; roslib.load_manifest('dynamic_graph_bridge')
+import rospy
+
+import dynamic_graph_bridge.srv
+
+import sys
+import code
+from code import InteractiveConsole
+import readline
+
+class RosShell(InteractiveConsole):
+ def __init__(self):
+ self.cache = ""
+ InteractiveConsole.__init__(self)
+
+ rospy.loginfo('waiting for service...')
+ rospy.wait_for_service('run_command')
+ self.client = rospy.ServiceProxy(
+ 'run_command', dynamic_graph_bridge.srv.RunCommand, True)
+
+
+ def runcode(self, code):
+ source = self.cache[:-1]
+ self.cache = ""
+ if source != "":
+ try:
+ if not self.client:
+ print("Connection to remote server lost. Reconnecting...")
+ self.client = rospy.ServiceProxy(
+ 'run_command', dynamic_graph_bridge.srv.RunCommand, True)
+ return
+ response = self.client(str(source))
+ if response.stdout != "":
+ print response.stdout[:-1]
+ if response.stderr != "":
+ print response.stderr[:-1]
+ elif response.result != "None":
+ print response.result
+ except rospy.ServiceException, e:
+ print("Connection to remote server lost. Reconnecting...")
+ self.client = rospy.ServiceProxy(
+ 'run_command', dynamic_graph_bridge.srv.RunCommand, True)
+
+ def push(self,line):
+ self.cache += line + "\n"
+ return InteractiveConsole.push(self,line)
+
+if __name__ == '__main__':
+ import optparse
+ manifest = roslib.manifest.load_manifest('dynamic_graph_bridge')
+ parser = optparse.OptionParser(
+ usage='\n\t%prog [options]',
+ version='%%prog %s' % manifest.version)
+ (options, args) = parser.parse_args(sys.argv[1:])
+
+ sh = RosShell()
+
+ if args[:]:
+ infile = args[0]
+ source = open(infile).read()
+ if not sh.client:
+ print("Connection to remote server has been lost.")
+ sys.exit(1)
+ response = sh.client(str(source))
+ if response.stdout != "":
+ print response.stdout[:-1]
+ if response.stderr != "":
+ print response.stderr[:-1]
+ elif response.result != "None":
+ print response.result
+ else:
+ sh.interact("Interacting with remote server.")
diff --git a/src/interpreter.cpp b/src/interpreter.cpp
new file mode 100644
index 0000000..a9b4e8a
--- /dev/null
+++ b/src/interpreter.cpp
@@ -0,0 +1,9 @@
+#include <dynamic_graph_bridge/ros_init.hh>
+#include <dynamic_graph_bridge/ros_interpreter.hh>
+
+int main ()
+{
+ ros::NodeHandle& nodeHandle = dynamicgraph::rosInit ();
+ dynamicgraph::Interpreter interpreter (nodeHandle);
+ ros::spin ();
+}
diff --git a/src/ros_interpreter.cpp b/src/ros_interpreter.cpp
new file mode 100644
index 0000000..fac3a75
--- /dev/null
+++ b/src/ros_interpreter.cpp
@@ -0,0 +1,27 @@
+#include "dynamic_graph_bridge/ros_interpreter.hh"
+
+namespace dynamicgraph
+{
+ static const int queueSize = 5;
+
+ Interpreter::Interpreter (ros::NodeHandle& nodeHandle)
+ : interpreter_ (),
+ nodeHandle_ (nodeHandle),
+ runCommandSrv_ ()
+ {
+ runCommandCallback_t runCommandCb =
+ boost::bind (&Interpreter::runCommandCallback, this, _1, _2);
+
+ runCommandSrv_ =
+ nodeHandle_.advertiseService ("run_command", runCommandCb);
+ }
+
+ bool
+ Interpreter::runCommandCallback(dynamic_graph_bridge::RunCommand::Request& req,
+ dynamic_graph_bridge::RunCommand::Response& res)
+ {
+ interpreter_.python(req.input, res.result, res.stdout, res.stderr);
+ return true;
+ }
+} // end of namespace dynamicgraph.
+
diff --git a/srv/RunCommand.srv b/srv/RunCommand.srv
new file mode 100644
index 0000000..4207e9b
--- /dev/null
+++ b/srv/RunCommand.srv
@@ -0,0 +1,5 @@
+string input
+---
+string result
+string stdout
+string stderr
--
GitLab