1 |
|
|
/* |
2 |
|
|
* Copyright 2016, |
3 |
|
|
* |
4 |
|
|
* Rohan Budhiraja |
5 |
|
|
* Olivier Stasse |
6 |
|
|
* |
7 |
|
|
* LAAS, CNRS |
8 |
|
|
* |
9 |
|
|
* This file is part of TIAGOController. |
10 |
|
|
* TIAGOController is a free software, |
11 |
|
|
* |
12 |
|
|
*/ |
13 |
|
|
|
14 |
|
|
#include <pinocchio/fwd.hpp> |
15 |
|
|
// include pinocchio before boost |
16 |
|
|
|
17 |
|
|
#include <boost/thread/condition.hpp> |
18 |
|
|
#include <boost/thread/thread.hpp> |
19 |
|
|
#include <dynamic_graph_bridge/ros_init.hh> |
20 |
|
|
#include <dynamic_graph_bridge/ros_interpreter.hh> |
21 |
|
|
#include <sot/core/debug.hh> |
22 |
|
|
#include <sot/core/exception-abstract.hh> |
23 |
|
|
|
24 |
|
|
#include "sot-tiago-controller.hh" |
25 |
|
|
const std::string SoTTiagoController::LOG_PYTHON = |
26 |
|
|
"/tmp/TiagoController_python.out"; |
27 |
|
|
|
28 |
|
|
using namespace std; |
29 |
|
|
|
30 |
|
|
boost::condition_variable cond; |
31 |
|
|
boost::mutex mut; |
32 |
|
|
bool data_ready; |
33 |
|
|
|
34 |
|
|
void workThread(SoTTiagoController *aSoTTiago) { |
35 |
|
|
dynamicgraph::Interpreter aLocalInterpreter( |
36 |
|
|
dynamicgraph::rosInit(false, true)); |
37 |
|
|
|
38 |
|
|
aSoTTiago->interpreter_ = |
39 |
|
|
boost::make_shared<dynamicgraph::Interpreter>(aLocalInterpreter); |
40 |
|
|
std::cout << "Going through the thread." << std::endl; |
41 |
|
|
{ |
42 |
|
|
boost::lock_guard<boost::mutex> lock(mut); |
43 |
|
|
data_ready = true; |
44 |
|
|
} |
45 |
|
|
cond.notify_all(); |
46 |
|
|
ros::waitForShutdown(); |
47 |
|
|
} |
48 |
|
|
|
49 |
|
|
SoTTiagoController::SoTTiagoController(std::string RobotName) |
50 |
|
|
: device_(new SoTTiagoDevice(RobotName)) { |
51 |
|
|
init(); |
52 |
|
|
} |
53 |
|
|
|
54 |
|
|
SoTTiagoController::SoTTiagoController(const char robotName[]) |
55 |
|
|
: device_(new SoTTiagoDevice(robotName)) { |
56 |
|
|
init(); |
57 |
|
|
} |
58 |
|
|
|
59 |
|
|
void SoTTiagoController::init() { |
60 |
|
|
std::cout << "Going through SoTTiagoController." << std::endl; |
61 |
|
|
boost::thread thr(workThread, this); |
62 |
|
|
sotDEBUG(25) << __FILE__ << ":" << __FUNCTION__ << "(#" << __LINE__ << " )" |
63 |
|
|
<< std::endl; |
64 |
|
|
|
65 |
|
|
boost::unique_lock<boost::mutex> lock(mut); |
66 |
|
|
cond.wait(lock); |
67 |
|
|
} |
68 |
|
|
|
69 |
|
|
SoTTiagoController::~SoTTiagoController() {} |
70 |
|
|
|
71 |
|
|
void SoTTiagoController::setupSetSensors( |
72 |
|
|
map<string, dgsot::SensorValues> &SensorsIn) { |
73 |
|
|
device_->setupSetSensors(SensorsIn); |
74 |
|
|
} |
75 |
|
|
|
76 |
|
|
void SoTTiagoController::nominalSetSensors( |
77 |
|
|
map<string, dgsot::SensorValues> &SensorsIn) { |
78 |
|
|
device_->nominalSetSensors(SensorsIn); |
79 |
|
|
} |
80 |
|
|
|
81 |
|
|
void SoTTiagoController::cleanupSetSensors( |
82 |
|
|
map<string, dgsot::SensorValues> &SensorsIn) { |
83 |
|
|
device_->cleanupSetSensors(SensorsIn); |
84 |
|
|
} |
85 |
|
|
|
86 |
|
|
void SoTTiagoController::getControl( |
87 |
|
|
map<string, dgsot::ControlValues> &controlOut) { |
88 |
|
|
try { |
89 |
|
|
sotDEBUG(25) << __FILE__ << __FUNCTION__ << "(#" << __LINE__ << ")" << endl; |
90 |
|
|
device_->getControl(controlOut); |
91 |
|
|
sotDEBUG(25) << __FILE__ << __FUNCTION__ << "(#" << __LINE__ << ")" << endl; |
92 |
|
|
} catch (dynamicgraph::sot::ExceptionAbstract &err) { |
93 |
|
|
std::cout << __FILE__ << " " << __FUNCTION__ << " (" << __LINE__ << ") " |
94 |
|
|
<< err.getStringMessage() << endl; |
95 |
|
|
throw err; |
96 |
|
|
} |
97 |
|
|
} |
98 |
|
|
|
99 |
|
|
void SoTTiagoController::setNoIntegration(void) { device_->setNoIntegration(); } |
100 |
|
|
|
101 |
|
|
void SoTTiagoController::setSecondOrderIntegration(void) { |
102 |
|
|
device_->setSecondOrderIntegration(); |
103 |
|
|
} |
104 |
|
|
|
105 |
|
|
void SoTTiagoController::runPython(std::ostream &file, |
106 |
|
|
const std::string &command, |
107 |
|
|
dynamicgraph::Interpreter &interpreter) { |
108 |
|
|
file << ">>> " << command << std::endl; |
109 |
|
|
std::string lerr(""), lout(""), lres(""); |
110 |
|
|
interpreter.runCommand(command, lres, lout, lerr); |
111 |
|
|
if (lres != "None") { |
112 |
|
|
if (lres == "<NULL>") { |
113 |
|
|
file << lout << std::endl; |
114 |
|
|
file << "------" << std::endl; |
115 |
|
|
file << lerr << std::endl; |
116 |
|
|
} else |
117 |
|
|
file << lres << std::endl; |
118 |
|
|
} |
119 |
|
|
} |
120 |
|
|
|
121 |
|
|
void SoTTiagoController::startupPython() { |
122 |
|
|
std::ofstream aof(LOG_PYTHON.c_str()); |
123 |
|
|
runPython(aof, "import sys, os", *interpreter_); |
124 |
|
|
runPython(aof, "pythonpath = os.environ['PYTHONPATH']", *interpreter_); |
125 |
|
|
runPython(aof, "path = []", *interpreter_); |
126 |
|
|
runPython(aof, |
127 |
|
|
"for p in pythonpath.split(':'):\n" |
128 |
|
|
" if p not in sys.path:\n" |
129 |
|
|
" path.append(p)", |
130 |
|
|
*interpreter_); |
131 |
|
|
runPython(aof, "path.extend(sys.path)", *interpreter_); |
132 |
|
|
runPython(aof, "sys.path = path", *interpreter_); |
133 |
|
|
|
134 |
|
|
// Calling again rosInit here to start the spinner. It will |
135 |
|
|
// deal with topics and services callbacks in a separate, non |
136 |
|
|
// real-time thread. See roscpp documentation for more |
137 |
|
|
// information. |
138 |
|
|
dynamicgraph::rosInit(true); |
139 |
|
|
aof.close(); |
140 |
|
|
} |