sot-talos-controller.cpp 3.86 KB
Newer Older
1
/*
Olivier Stasse's avatar
Olivier Stasse committed
2
 * Copyright 2016,
3
 *
Olivier Stasse's avatar
Olivier Stasse committed
4
 * Rohan Budhiraja
5
6
7
8
 * Olivier Stasse
 *
 * LAAS, CNRS
 *
Olivier Stasse's avatar
Olivier Stasse committed
9
 * This file is part of TALOSController.
10
 * TALOSController is a free software,
Olivier Stasse's avatar
Olivier Stasse committed
11
 *
12
13
14
15
16
17
18
19
20
21
22
 */

#include <sot/core/debug.hh>
#include <sot/core/exception-abstract.hh>
#include <dynamic_graph_bridge/ros_init.hh>
#include <dynamic_graph_bridge/ros_interpreter.hh>

#include "sot-talos-controller.hh"

#include <boost/thread/thread.hpp>
#include <boost/thread/condition.hpp>
23
24
25

#include <ros/console.h>

26
27
28
29
const std::string SoTTalosController::LOG_PYTHON="/tmp/TalosController_python.out";

using namespace std;

30
SoTTalosController::SoTTalosController(std::string RobotName):
31
  device_(new SoTTalosDevice(RobotName))
32
{
Olivier Stasse's avatar
Olivier Stasse committed
33
34
  init();
}
35

Olivier Stasse's avatar
Olivier Stasse committed
36
SoTTalosController::SoTTalosController(const char robotName[]):
37
  device_(new SoTTalosDevice(robotName))
Olivier Stasse's avatar
Olivier Stasse committed
38
39
40
41
42
43
{
  init();
}

void SoTTalosController::init()
{
44
  std::cout << "Going through SoTTalosController." << std::endl;
45

46
47
48
  // rosInit is called here only to initialize ros.
  // No spinner is initialized.
  ros::NodeHandle &nh = dynamicgraph::rosInit(false,false);
49
50
51
  interpreter_ = boost::shared_ptr<dynamicgraph::Interpreter>(
      new dynamicgraph::Interpreter (nh));

52
53
  sotDEBUG(25) << __FILE__ << ":"
	       << __FUNCTION__ <<"(#"
54
55
	       << __LINE__ << " )" << std::endl;

56
  double ts = ros::param::param<double> ("/sot_controller/dt", SoTTalosDevice::TIMESTEP_DEFAULT);
57
  device_->timeStep(ts);
58
59
60
61
}

SoTTalosController::~SoTTalosController()
{
62
  // device_ will be deleted by dynamicgraph::PoolStorage::destroy()
63
64
65
66
67
}

void SoTTalosController::
setupSetSensors(map<string,dgsot::SensorValues> &SensorsIn)
{
68
  device_->setupSetSensors(SensorsIn);
69
70
71
72
73
74
}


void SoTTalosController::
nominalSetSensors(map<string,dgsot::SensorValues> &SensorsIn)
{
75
  device_->nominalSetSensors(SensorsIn);
76
77
78
79
80
}

void SoTTalosController::
cleanupSetSensors(map<string, dgsot::SensorValues> &SensorsIn)
{
81
  device_->cleanupSetSensors(SensorsIn);
82
83
84
85
86
87
}


void SoTTalosController::
getControl(map<string,dgsot::ControlValues> &controlOut)
{
88
  try
89
90
    {
      sotDEBUG(25) << __FILE__ << __FUNCTION__ << "(#" << __LINE__ << ")" << endl;
91
      device_->getControl(controlOut);
92
93
94
95
96
      sotDEBUG(25) << __FILE__ << __FUNCTION__ << "(#" << __LINE__ << ")" << endl;
    }
  catch ( dynamicgraph::sot::ExceptionAbstract & err)
    {

97
98
99
100
      std::cout << __FILE__ << " "
		<< __FUNCTION__ << " ("
		<< __LINE__ << ") "
		<< err.getStringMessage()
101
102
103
104
105
		<<  endl;
      throw err;
    }
}

106
107
108
void SoTTalosController::
setNoIntegration(void)
{
109
  device_->setNoIntegration();
110
111
112
113
114
}

void SoTTalosController::
setSecondOrderIntegration(void)
{
115
  device_->setSecondOrderIntegration();
116
117
}

118
119
120
121
122
123
void SoTTalosController::
runPython(std::ostream& file,
	  const std::string& command,
	  dynamicgraph::Interpreter& interpreter)
{
  file << ">>> " << command << std::endl;
124
  std::string lres(""),lout(""),lerr("");
125
  interpreter.runCommand(command,lres,lout,lerr);
126

127
128
129
130
131
132
133
  if (lres != "None")
    {
      if (lres=="<NULL>")
	{
	  file << lout << std::endl;
	  file << "------" << std::endl;
	  file << lerr << std::endl;
134
135
	  ROS_INFO(lout.c_str());
	  ROS_ERROR(lerr.c_str());
136
137
	}
      else
138
139
140
141
	{
	  file << lres << std::endl;
	  ROS_INFO(lres.c_str());
	}
142
143
144
145
146
147
148
149
    }
}

void SoTTalosController::
startupPython()
{
  std::ofstream aof(LOG_PYTHON.c_str());
  runPython (aof, "import sys, os", *interpreter_);
150
  runPython (aof, "pythonpath = os.environ.get('PYTHONPATH', '')", *interpreter_);
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
  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_);

  // 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);
  aof.close();
}