Skip to content

Commit

Permalink
Format
Browse files Browse the repository at this point in the history
  • Loading branch information
@nim65s
nim65s committed Nov 5, 2019
1 parent 43892dd commit bc826e5
Show file tree
Hide file tree
Showing 7 changed files with 284 additions and 419 deletions.
19 changes: 10 additions & 9 deletions doc/additionalHeader/sot-talos-doc.hh
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
/* Copyright 2018, CNRS
* Author: O. Stasse
*
*
BSD 2-Clause License
Copyright (c) 2017, Stack Of Tasks development team
Expand Down Expand Up @@ -33,16 +33,17 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
\mainpage
\section sec_intro Introduction
This library implements a class called Device for the TALOS PAL-Robotics humanoid robots to be controlled with the Stack-Of-Tasks.
A derived class is used for the first prototype of this line of humanoid robot, Pyrene.
This library implements a class called Device for the TALOS PAL-Robotics humanoid robots to be controlled with the
Stack-Of-Tasks. A derived class is used for the first prototype of this line of humanoid robot, Pyrene.
This class is implementing a perception-action loop.
To achieve this goal it is providing a Hardware Abstract Layer to communicate with the SoT control infra-structure and the robot or a simulator.
It is fully compatible with the roscontrol_sot package to run on a TALOS humanoid robot or with the Gazebo simulator.
To achieve this goal it is providing a Hardware Abstract Layer to communicate with the SoT control infra-structure and
the robot or a simulator. It is fully compatible with the roscontrol_sot package to run on a TALOS humanoid robot or
with the Gazebo simulator.
The sot-talos package contains also the class sot-talos-controller. This class is used to start the multithreading environment which is handling ROS request, starts
the python interpreter to control the SoT and finally handle
the control states: initialization, nominal usage, stopping or cancellation.
The sot-talos package contains also the class sot-talos-controller. This class is used to start the multithreading
environment which is handling ROS request, starts the python interpreter to control the SoT and finally handle the
control states: initialization, nominal usage, stopping or cancellation.
It also provides python scripts to be used to interact with the robot.
Expand All @@ -57,6 +58,6 @@ It also provides python scripts to be used to interact with the robot.
<li> motor_angles </li>
<li> p_gains </li>
<li> d_gains</li>
\section sot-talos-controller
\section sot-talos-controller
*/
41 changes: 12 additions & 29 deletions src/sot-pyrene-controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@
* LAAS, CNRS
*
* This file is part of TALOSController.
* TALOSController is a free software,
* TALOSController is a free software,
*
*/

Expand All @@ -17,43 +17,26 @@

#include "sot-pyrene-controller.hh"

const std::string SoTPyreneController::LOG_PYTHON_PYRENE="/tmp/PyreneController_python.out";
const std::string SoTPyreneController::LOG_PYTHON_PYRENE = "/tmp/PyreneController_python.out";

SoTPyreneController::SoTPyreneController():
SoTTalosController(ROBOTNAME)
{
SoTPyreneController::SoTPyreneController() : SoTTalosController(ROBOTNAME) {
startupPython();
interpreter_->startRosService ();
interpreter_->startRosService();
}

void SoTPyreneController::startupPython()
{
void SoTPyreneController::startupPython() {
SoTTalosController::startupPython();
std::ofstream aof(LOG_PYTHON_PYRENE.c_str());

runPython
(aof,
"from dynamic_graph.sot.pyrene.prologue import makeRobot",
*interpreter_);
runPython
(aof,
"robot = makeRobot ()",
*interpreter_);

runPython(aof, "from dynamic_graph.sot.pyrene.prologue import makeRobot", *interpreter_);
runPython(aof, "robot = makeRobot ()", *interpreter_);
aof.close();
}

extern "C"
{
dgsot::AbstractSotExternalInterface * createSotExternalInterface()
{
return new SoTPyreneController;
}
extern "C" {
dgsot::AbstractSotExternalInterface *createSotExternalInterface() { return new SoTPyreneController; }
}

extern "C"
{
void destroySotExternalInterface(dgsot::AbstractSotExternalInterface *p)
{
delete p;
}
extern "C" {
void destroySotExternalInterface(dgsot::AbstractSotExternalInterface *p) { delete p; }
}
13 changes: 4 additions & 9 deletions src/sot-pyrene-controller.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@
* LAAS, CNRS
*
* This file is part of TALOSController.
* TALOSController is a free software,
* TALOSController is a free software,
*
*
*/
Expand All @@ -15,22 +15,17 @@
#define _SOT_PYRENE_Controller_H_

#include "sot-talos-controller.hh"
namespace dgsot=dynamicgraph::sot;
namespace dgsot = dynamicgraph::sot;

class SoTPyreneController: public SoTTalosController
{
class SoTPyreneController : public SoTTalosController {
public:
static const std::string LOG_PYTHON_PYRENE;

SoTPyreneController();
virtual ~SoTPyreneController() {};

virtual ~SoTPyreneController(){};

protected:

virtual void startupPython();


};

#endif /* _SOTTalosController_H_ */
149 changes: 51 additions & 98 deletions src/sot-talos-controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,143 +23,96 @@

#include <ros/console.h>

const std::string SoTTalosController::LOG_PYTHON="/tmp/TalosController_python.out";
const std::string SoTTalosController::LOG_PYTHON = "/tmp/TalosController_python.out";

using namespace std;

SoTTalosController::SoTTalosController(std::string RobotName):
device_(new SoTTalosDevice(RobotName))
{
init();
}
SoTTalosController::SoTTalosController(std::string RobotName) : device_(new SoTTalosDevice(RobotName)) { init(); }

SoTTalosController::SoTTalosController(const char robotName[]):
device_(new SoTTalosDevice(robotName))
{
init();
}
SoTTalosController::SoTTalosController(const char robotName[]) : device_(new SoTTalosDevice(robotName)) { init(); }

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

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

sotDEBUG(25) << __FILE__ << ":"
<< __FUNCTION__ <<"(#"
<< __LINE__ << " )" << std::endl;
sotDEBUG(25) << __FILE__ << ":" << __FUNCTION__ << "(#" << __LINE__ << " )" << std::endl;

double ts = ros::param::param<double> ("/sot_controller/dt", SoTTalosDevice::TIMESTEP_DEFAULT);
double ts = ros::param::param<double>("/sot_controller/dt", SoTTalosDevice::TIMESTEP_DEFAULT);
device_->timeStep(ts);
}

SoTTalosController::~SoTTalosController()
{
SoTTalosController::~SoTTalosController() {
// device_ will be deleted by dynamicgraph::PoolStorage::destroy()
}

void SoTTalosController::
setupSetSensors(map<string,dgsot::SensorValues> &SensorsIn)
{
void SoTTalosController::setupSetSensors(map<string, dgsot::SensorValues> &SensorsIn) {
device_->setupSetSensors(SensorsIn);
}


void SoTTalosController::
nominalSetSensors(map<string,dgsot::SensorValues> &SensorsIn)
{
void SoTTalosController::nominalSetSensors(map<string, dgsot::SensorValues> &SensorsIn) {
device_->nominalSetSensors(SensorsIn);
}

void SoTTalosController::
cleanupSetSensors(map<string, dgsot::SensorValues> &SensorsIn)
{
void SoTTalosController::cleanupSetSensors(map<string, dgsot::SensorValues> &SensorsIn) {
device_->cleanupSetSensors(SensorsIn);
}


void SoTTalosController::
getControl(map<string,dgsot::ControlValues> &controlOut)
{
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;
}
void SoTTalosController::getControl(map<string, dgsot::ControlValues> &controlOut) {
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;
}
}

void SoTTalosController::
setNoIntegration(void)
{
device_->setNoIntegration();
}
void SoTTalosController::setNoIntegration(void) { device_->setNoIntegration(); }

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

void SoTTalosController::
runPython(std::ostream& file,
const std::string& command,
dynamicgraph::Interpreter& interpreter)
{
void SoTTalosController::runPython(std::ostream &file, const std::string &command,
dynamicgraph::Interpreter &interpreter) {
file << ">>> " << command << std::endl;
std::string lres(""),lout(""),lerr("");
interpreter.runCommand(command,lres,lout,lerr);

if (lres != "None")
{
if (lres=="<NULL>")
{
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;
ROS_INFO(lres.c_str());
}
std::string lres(""), lout(""), lerr("");
interpreter.runCommand(command, lres, lout, lerr);

if (lres != "None") {
if (lres == "<NULL>") {
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;
ROS_INFO(lres.c_str());
}
}
}

void SoTTalosController::
startupPython()
{
void SoTTalosController::startupPython() {
std::ofstream aof(LOG_PYTHON.c_str());
runPython (aof, "import sys, os", *interpreter_);
runPython (aof, "pythonpath = os.environ.get('PYTHONPATH', '')", *interpreter_);
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_);
runPython(aof, "import sys, os", *interpreter_);
runPython(aof, "pythonpath = os.environ.get('PYTHONPATH', '')", *interpreter_);
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);
dynamicgraph::rosInit(true);
aof.close();
}
18 changes: 6 additions & 12 deletions src/sot-talos-controller.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,23 +22,20 @@

#include "sot-talos-device.hh"
#include <dynamic_graph_bridge/ros_interpreter.hh>
namespace dgsot=dynamicgraph::sot;
namespace dgsot = dynamicgraph::sot;

class SoTTalosController: public
dgsot::AbstractSotExternalInterface
{
class SoTTalosController : public dgsot::AbstractSotExternalInterface {
public:

static const std::string LOG_PYTHON;

SoTTalosController();
SoTTalosController(const char robotName[]);
SoTTalosController(std::string robotName);
virtual ~SoTTalosController();

void setupSetSensors(std::map<std::string,dgsot::SensorValues> &sensorsIn);
void setupSetSensors(std::map<std::string, dgsot::SensorValues> &sensorsIn);

void nominalSetSensors(std::map<std::string,dgsot::SensorValues> &sensorsIn);
void nominalSetSensors(std::map<std::string, dgsot::SensorValues> &sensorsIn);

void cleanupSetSensors(std::map<std::string, dgsot::SensorValues> &sensorsIn);

Expand All @@ -59,16 +56,13 @@ class SoTTalosController: public
void updateRobotState(std::vector<double> &anglesIn);

/// Run a python command
void runPython(std::ostream& file,
const std::string& command,
dynamicgraph::Interpreter& interpreter);
void runPython(std::ostream &file, const std::string &command, dynamicgraph::Interpreter &interpreter);

virtual void startupPython();

void init();

SoTTalosDevice* device_;
SoTTalosDevice *device_;
};

#endif /* _SOT_TalosController_H_ */

Loading

0 comments on commit bc826e5

Please sign in to comment.