Add ROS_INFO and ROS_ERROR informations when using runPython.

src/sot-pyrene-controller.cpp

@@ -30,6 +30,7 @@ void SoTPyreneController::startupPython()
 {
   SoTTalosController::startupPython();
   std::ofstream aof(LOG_PYTHON_PYRENE.c_str());
+  
   runPython
     (aof,
      "from dynamic_graph.sot.pyrene.prologue import robot",

src/sot-talos-controller.cpp

@@ -20,6 +20,9 @@
 
 #include <boost/thread/thread.hpp>
 #include <boost/thread/condition.hpp>
+
+#include <ros/console.h>
+
 const std::string SoTTalosController::LOG_PYTHON="/tmp/TalosController_python.out";
 
 using namespace std;
@@ -133,8 +136,9 @@ runPython(std::ostream& file,
 	  dynamicgraph::Interpreter& interpreter)
 {
   file << ">>> " << command << std::endl;
-  std::string lerr(""),lout(""),lres("");
+  std::string lres(""),lout(""),lerr("");
   interpreter.runCommand(command,lres,lout,lerr);
+
   if (lres != "None")
     {
       if (lres=="<NULL>")
@@ -142,9 +146,14 @@ runPython(std::ostream& file,
 	  file << lout << std::endl;
 	  file << "------" << std::endl;
 	  file << lerr << std::endl;
+	  ROS_INFO(lout.c_str());
+	  ROS_ERROR(lerr.c_str());
 	}
       else
-	file << lres << std::endl;
+	{
+	  file << lres << std::endl;
+	  ROS_INFO(lres.c_str());
+	}
     }
 }
 

src/sot-talos-controller.hh

@@ -52,7 +52,8 @@ class SoTTalosController: public
   // Update output port with the control computed from the
   // dynamic graph.
   void updateRobotState(std::vector<double> &anglesIn);
-  
+
+  /// Run a python command 
   void runPython(std::ostream& file,
 		 const std::string& command,
 		 dynamicgraph::Interpreter& interpreter);