Select Git revision
sot_loader.cpp 6.20 KiB
/*
* Copyright 2011,
* Olivier Stasse,
*
* CNRS
*
* This file is part of sot-core.
* sot-core is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
* sot-core is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
* with sot-core. If not, see <http://www.gnu.org/licenses/>.
*/
/* -------------------------------------------------------------------------- */
/* --- INCLUDES ------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
#include <dynamic_graph_bridge/sot_loader.hh>
#include "dynamic_graph_bridge/ros_init.hh"
// POSIX.1-2001
#include <dlfcn.h>
#include <boost/thread/condition.hpp>
boost::condition_variable cond;
using namespace std;
using namespace dynamicgraph::sot;
namespace po = boost::program_options;
struct DataToLog
{
const std::size_t N;
std::size_t idx;
std::vector<double> times;
DataToLog (std::size_t N_)
: N (N_)
, idx (0)
, times (N, 0)
{}
void record (const double t)
{
times[idx] = t;
++idx;
if (idx == N) idx = 0;
}
void save (const char* prefix)
{
std::ostringstream oss;
oss << prefix << "-times.log";
std::ofstream aof(oss.str().c_str());
if (aof.is_open()) {
for (std::size_t k = 0; k < N; ++k) {
aof << times[ (idx + k) % N ] << '\n';
}
}
aof.close();
}
};
void workThreadLoader(SotLoader *aSotLoader)
{
unsigned period = 1000; // micro seconds
if (ros::param::has("/sot_controller/dt")) {
double periodd;
ros::param::get("/sot_controller/dt", periodd);
period = unsigned(1e6 * periodd);
}
DataToLog dataToLog (5000);
while(aSotLoader->isDynamicGraphStopped())
{
usleep(period);
}
struct timeval start, stop;
ros::NodeHandle nh ("/geometric_simu");
bool paused;
unsigned long long dt;
while(!aSotLoader->isDynamicGraphStopped())
{
nh.param<bool> ("paused", paused, false);
if (!paused) {
gettimeofday(&start,0);
aSotLoader->oneIteration();
gettimeofday(&stop,0);
dt = 1000000 * (stop.tv_sec - start.tv_sec) + (stop.tv_usec - start.tv_usec);
dataToLog.record ((double)dt * 1e-6);
} else
dt = 0;
if (period > dt) {
usleep(period - (unsigned)dt);
}
}
dataToLog.save ("/tmp/geometric_simu");
cond.notify_all();
ros::waitForShutdown();
}
SotLoader::SotLoader():
sensorsIn_ (),
controlValues_ (),
angleEncoder_ (),
angleControl_ (),
forces_ (),
torques_ (),
baseAtt_ (),
accelerometer_ (3),
gyrometer_ (3),
thread_ ()
{
readSotVectorStateParam();
initPublication();
}
SotLoader::~SotLoader()
{
dynamic_graph_stopped_ = true;
thread_.join();
}
void SotLoader::startControlLoop()
{
thread_ = boost::thread (workThreadLoader, this);
}
void SotLoader::initializeRosNode(int argc, char *argv[])
{
SotLoaderBasic::initializeRosNode(argc, argv);
//Temporary fix. TODO: where should nbOfJoints_ be initialized from?
if (ros::param::has("/sot/state_vector_map")) {
angleEncoder_.resize(nbOfJoints_);
}
startControlLoop();
}
void
SotLoader::fillSensors(map<string,dgs::SensorValues> & sensorsIn)
{
// Update joint values.w
assert(angleControl_.size() == angleEncoder_.size());
sensorsIn["joints"].setName("angle");
for(unsigned int i=0;i<angleControl_.size();i++)
angleEncoder_[i] = angleControl_[i];
sensorsIn["joints"].setValues(angleEncoder_);
}
void
SotLoader::readControl(map<string,dgs::ControlValues> &controlValues)
{
// Update joint values.
angleControl_ = controlValues["control"].getValues();
//Debug
std::map<std::string,dgs::ControlValues>::iterator it = controlValues.begin();
sotDEBUG (30)<<"ControlValues to be broadcasted:"<<std::endl;
for(;it!=controlValues.end(); it++){
sotDEBUG (30)<<it->first<<":";
std::vector<double> ctrlValues_ = it->second.getValues();
std::vector<double>::iterator it_d = ctrlValues_.begin();
for(;it_d!=ctrlValues_.end();it_d++) sotDEBUG (30)<<*it_d<<" ";
sotDEBUG (30)<<std::endl;
}
sotDEBUG (30)<<"End ControlValues"<<std::endl;
// Check if the size if coherent with the robot description.
if (angleControl_.size()!=(unsigned int)nbOfJoints_)
{
std::cerr << " angleControl_"<<angleControl_.size()
<< " and nbOfJoints"<<(unsigned int)nbOfJoints_
<< " are different !"
<< std::endl;
exit(-1);
}
// Publish the data.
joint_state_.header.stamp = ros::Time::now();
for(int i=0;i<nbOfJoints_;i++)
{
joint_state_.position[i] = angleControl_[i];
}
for(unsigned int i=0;i<parallel_joints_to_state_vector_.size();i++)
{
joint_state_.position[i+nbOfJoints_] =
coefficient_parallel_joints_[i]*angleControl_[parallel_joints_to_state_vector_[i]];
}
joint_pub_.publish(joint_state_);
//Publish robot pose
//get the robot pose values
std::vector<double> poseValue_ = controlValues["baseff"].getValues();
freeFlyerPose_.setOrigin(tf::Vector3(poseValue_[0],
poseValue_[1],
poseValue_[2]));
tf::Quaternion poseQ_(poseValue_[4],
poseValue_[5],
poseValue_[6],
poseValue_[3]);
freeFlyerPose_.setRotation(poseQ_);
//Publish
freeFlyerPublisher_.sendTransform(tf::StampedTransform(freeFlyerPose_,
ros::Time::now(),
"odom",
"base_link"));
}
void SotLoader::setup()
{
fillSensors(sensorsIn_);
sotController_->setupSetSensors(sensorsIn_);
sotController_->getControl(controlValues_);
readControl(controlValues_);
}
void SotLoader::oneIteration()
{
fillSensors(sensorsIn_);
try
{
sotController_->nominalSetSensors(sensorsIn_);
sotController_->getControl(controlValues_);
}
catch(std::exception &e) { throw e;}
readControl(controlValues_);
}