12#ifndef _SOT_LOADER_HH_
13#define _SOT_LOADER_HH_
22#include <pinocchio/fwd.hpp>
25#include <boost/foreach.hpp>
26#include <boost/format.hpp>
27#include <boost/program_options.hpp>
28#include <boost/thread/thread.hpp>
31#include <sensor_msgs/JointState.h>
32#include <tf2_ros/transform_broadcaster.h>
35#include "std_srvs/Empty.h"
38#include <sot/core/abstract-sot-external-interface.hh>
39#include <sot/core/debug.hh>
44namespace po = boost::program_options;
45namespace dgs = dynamicgraph::sot;
94 void fillSensors(std::map<std::string, dgs::SensorValues> &sensorsIn);
97 void readControl(std::map<std::string, dgs::ControlValues> &controlValues);
Definition sot_loader_basic.hh:42
Definition sot_loader.hh:47
boost::thread thread_
The thread running dynamic graph.
Definition sot_loader.hh:73
std::vector< double > angleEncoder_
Angular values read by encoders.
Definition sot_loader.hh:55
void readControl(std::map< std::string, dgs::ControlValues > &controlValues)
std::map< std::string, dgs::ControlValues > controlValues_
Map of control values.
Definition sot_loader.hh:52
void fillSensors(std::map< std::string, dgs::SensorValues > &sensorsIn)
tf2_ros::TransformBroadcaster freeFlyerPublisher_
Definition sot_loader.hh:79
std::map< std::string, dgs::SensorValues > sensorsIn_
Map of sensor readings.
Definition sot_loader.hh:50
std::vector< double > baseAtt_
Attitude of the robot computed by extended Kalman filter.
Definition sot_loader.hh:63
std::string robot_desc_string_
URDF string description of the robot.
Definition sot_loader.hh:70
std::vector< double > accelerometer_
Accelerations read by Accelerometers.
Definition sot_loader.hh:65
std::vector< double > forces_
Forces read by force sensors.
Definition sot_loader.hh:59
std::vector< double > gyrometer_
Angular velocity read by gyrometers.
Definition sot_loader.hh:67
std::vector< double > torques_
Torques.
Definition sot_loader.hh:61
geometry_msgs::TransformStamped freeFlyerPose_
Definition sot_loader.hh:80
std::vector< double > angleControl_
Angular values sent to motors.
Definition sot_loader.hh:57
void initializeRosNode(int argc, char *argv[])
virtual void startControlLoop()