Implement RunCommand service.

.gitignore

@@ -5,3 +5,4 @@ msg_gen/
 src/dynamic_graph_bridge/
 bin/
 build/
+srv_gen/

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)
 

include/dynamic_graph_bridge/ros_interpreter.hh

+#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

scripts/dgbridge-remote

+#!/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.")

src/interpreter.cpp

+#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 ();
+}

src/ros_interpreter.cpp

+#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.
+

srv/RunCommand.srv

+string input
+---
+string result
+string stdout
+string stderr