sot-hrp2-controller.cpp 3.66 KB
Newer Older
olivier stasse's avatar
olivier stasse committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18
/*
 * Copyright 2011,
 *
 * Olivier Stasse
 *
 * LAAS, CNRS
 *
 * This file is part of HRP2Controller.
 * HRP2Controller is not a free software, 
 * it contains information related to HRP-2 which involves
 * that you either purchased the proper license to havec access to
 * those informations, or that you signed the appropriate
 * Non-Disclosure agreement.
 *
 *
 */

#include <sot/core/debug.hh>
19
#include <sot/core/exception-abstract.hh>
20
#include <dynamic_graph_bridge/ros_init.hh>
21
#include "sot-hrp2-controller.hh"
olivier stasse's avatar
olivier stasse committed
22

23 24
#include <boost/thread/thread.hpp>
#include <boost/thread/condition.hpp>
olivier stasse's avatar
olivier stasse committed
25 26 27 28
const std::string SoTHRP2Controller::LOG_PYTHON="/tmp/HRP2Controller_python.out";

using namespace std;

29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48
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();
}

49
SoTHRP2Controller::SoTHRP2Controller(std::string RobotName):
50
  device_(RobotName)
olivier stasse's avatar
olivier stasse committed
51
{
52

53
  std::cout << "Going through SoTHRP2Controller for " <<RobotName<< std::endl;
54
  boost::thread thr(workThread,this);
55 56 57
  sotDEBUG(25) << __FILE__ << ":" 
	       << __FUNCTION__ <<"(#" 
	       << __LINE__ << " )" << std::endl;
58

59 60
  boost::unique_lock<boost::mutex> lock(mut);
  cond.wait(lock);
olivier stasse's avatar
olivier stasse committed
61 62 63 64 65 66
}

SoTHRP2Controller::~SoTHRP2Controller()
{
}

67 68
void SoTHRP2Controller::
setupSetSensors(map<string,dgsot::SensorValues> &SensorsIn)
olivier stasse's avatar
olivier stasse committed
69
{
70
  device_.setupSetSensors(SensorsIn);
olivier stasse's avatar
olivier stasse committed
71 72 73
}


74 75
void SoTHRP2Controller::
nominalSetSensors(map<string,dgsot::SensorValues> &SensorsIn)
olivier stasse's avatar
olivier stasse committed
76
{
77
  device_.nominalSetSensors(SensorsIn);
olivier stasse's avatar
olivier stasse committed
78 79
}

80 81
void SoTHRP2Controller::
cleanupSetSensors(map<string, dgsot::SensorValues> &SensorsIn)
olivier stasse's avatar
olivier stasse committed
82
{
83
  device_.cleanupSetSensors(SensorsIn);
olivier stasse's avatar
olivier stasse committed
84 85 86
}


87 88
void SoTHRP2Controller::
getControl(map<string,dgsot::ControlValues> &controlOut)
olivier stasse's avatar
olivier stasse committed
89
{
90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105
  try 
    {
      sotDEBUG(25) << __FILE__ << __FUNCTION__ << "(#" << __LINE__ << ")" << endl;
      device_.getControl(controlOut);
      sotDEBUG(25) << __FILE__ << __FUNCTION__ << "(#" << __LINE__ << ")" << endl;
    }
  catch ( dynamicgraph::sot::ExceptionAbstract & err)
    {

      std::cout << __FILE__ << " " 
		<< __FUNCTION__ << " (" 
		<< __LINE__ << ") " 
		<< err.getStringMessage() 
		<<  endl;
      throw err;
    }
olivier stasse's avatar
olivier stasse committed
106 107
}

108 109 110
void SoTHRP2Controller::
runPython(std::ostream& file,
	  const std::string& command,
111
	  dynamicgraph::Interpreter& interpreter)
olivier stasse's avatar
olivier stasse committed
112 113
{
  file << ">>> " << command << std::endl;
114 115 116 117 118 119 120 121 122 123 124 125 126
  std::string lerr(""),lout(""),lres("");
  interpreter.runCommand(command,lres,lout,lerr);
  if (lres != "None")
    {
      if (lres=="<NULL>")
	{
	  file << lout << std::endl;
	  file << "------" << std::endl;
	  file << lerr << std::endl;
	}
      else
	file << lres << std::endl;
    }
olivier stasse's avatar
olivier stasse committed
127 128
}

129 130
void SoTHRP2Controller::
startupPython()
olivier stasse's avatar
olivier stasse committed
131 132
{
  std::ofstream aof(LOG_PYTHON.c_str());
133 134 135
  runPython (aof, "import sys, os", *interpreter_);
  runPython (aof, "pythonpath = os.environ['PYTHONPATH']", *interpreter_);
  runPython (aof, "path = []", *interpreter_);
olivier stasse's avatar
olivier stasse committed
136 137 138
  runPython (aof,
	     "for p in pythonpath.split(':'):\n"
	     "  if p not in sys.path:\n"
139 140 141
	     "    path.append(p)", *interpreter_);
  runPython (aof, "path.extend(sys.path)", *interpreter_);
  runPython (aof, "sys.path = path", *interpreter_);
142 143 144 145 146 147 148

  // 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);

149
  aof.close();
olivier stasse's avatar
olivier stasse committed
150 151 152
}