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