sot_loader.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2016,
3  * Olivier Stasse,
4  *
5  * CNRS
6  *
7  */
8 /* -------------------------------------------------------------------------- */
9 /* --- INCLUDES ------------------------------------------------------------- */
10 /* -------------------------------------------------------------------------- */
11 
12 #ifndef _SOT_LOADER_HH_
13 #define _SOT_LOADER_HH_
14 
15 // System includes
16 #include <cassert>
17 
18 // STL includes
19 #include <map>
20 
21 // Pinocchio includes
22 #include <pinocchio/fwd.hpp>
23 
24 // Boost includes
25 #include <boost/program_options.hpp>
26 #include <boost/foreach.hpp>
27 #include <boost/format.hpp>
28 #include <boost/thread/thread.hpp>
29 
30 // ROS includes
31 #include "ros/ros.h"
32 #include "std_srvs/Empty.h"
33 #include <sensor_msgs/JointState.h>
34 #include <tf2_ros/transform_broadcaster.h>
35 
36 // Sot Framework includes
37 #include <sot/core/debug.hh>
38 #include <sot/core/abstract-sot-external-interface.hh>
39 
40 // Dynamic-graph-bridge includes.
42 
43 namespace po = boost::program_options;
44 namespace dgs = dynamicgraph::sot;
45 
46 class SotLoader : public SotLoaderBasic {
47  protected:
49  std::map<std::string, dgs::SensorValues> sensorsIn_;
51  std::map<std::string, dgs::ControlValues> controlValues_;
52 
54  std::vector<double> angleEncoder_;
56  std::vector<double> angleControl_;
58  std::vector<double> forces_;
60  std::vector<double> torques_;
62  std::vector<double> baseAtt_;
64  std::vector<double> accelerometer_;
66  std::vector<double> gyrometer_;
67 
69  std::string robot_desc_string_;
70 
72  boost::thread thread_;
73 
74  // \brief Start control loop
75  virtual void startControlLoop();
76 
77  // Robot Pose Publisher
78  tf2_ros::TransformBroadcaster freeFlyerPublisher_;
79  geometry_msgs::TransformStamped freeFlyerPose_;
80 
81  public:
82  SotLoader();
83  ~SotLoader();
84 
85  // \brief Create a thread for ROS and start the control loop.
86  void initializeRosNode(int argc, char *argv[]);
87 
88  // \brief Compute one iteration of control.
89  // Basically calls fillSensors, the SoT and the readControl.
90  void oneIteration();
91 
92  // \brief Fill the sensors value for the SoT.
93  void fillSensors(std::map<std::string, dgs::SensorValues> &sensorsIn);
94 
95  // \brief Read the control computed by the SoT framework.
96  void readControl(std::map<std::string, dgs::ControlValues> &controlValues);
97 
98  // \brief Prepare the SoT framework.
99  void setup();
100 };
101 
102 #endif /* SOT_LOADER_HH_ */
SotLoader::baseAtt_
std::vector< double > baseAtt_
Attitude of the robot computed by extended Kalman filter.
Definition: sot_loader.hh:62
SotLoader::angleEncoder_
std::vector< double > angleEncoder_
Angular values read by encoders.
Definition: sot_loader.hh:54
sot_loader_basic.hh
SotLoader::controlValues_
std::map< std::string, dgs::ControlValues > controlValues_
Map of control values.
Definition: sot_loader.hh:51
SotLoader::setup
void setup()
SotLoader::SotLoader
SotLoader()
SotLoaderBasic
Definition: sot_loader_basic.hh:41
SotLoader::sensorsIn_
std::map< std::string, dgs::SensorValues > sensorsIn_
Map of sensor readings.
Definition: sot_loader.hh:49
SotLoader::initializeRosNode
void initializeRosNode(int argc, char *argv[])
SotLoader::angleControl_
std::vector< double > angleControl_
Angular values sent to motors.
Definition: sot_loader.hh:56
SotLoader::accelerometer_
std::vector< double > accelerometer_
Accelerations read by Accelerometers.
Definition: sot_loader.hh:64
SotLoader::~SotLoader
~SotLoader()
SotLoader::freeFlyerPublisher_
tf2_ros::TransformBroadcaster freeFlyerPublisher_
Definition: sot_loader.hh:78
SotLoader::robot_desc_string_
std::string robot_desc_string_
URDF string description of the robot.
Definition: sot_loader.hh:69
SotLoader::readControl
void readControl(std::map< std::string, dgs::ControlValues > &controlValues)
SotLoader::thread_
boost::thread thread_
The thread running dynamic graph.
Definition: sot_loader.hh:72
SotLoader::freeFlyerPose_
geometry_msgs::TransformStamped freeFlyerPose_
Definition: sot_loader.hh:79
SotLoader::gyrometer_
std::vector< double > gyrometer_
Angular velocity read by gyrometers.
Definition: sot_loader.hh:66
SotLoader::forces_
std::vector< double > forces_
Forces read by force sensors.
Definition: sot_loader.hh:58
SotLoader::fillSensors
void fillSensors(std::map< std::string, dgs::SensorValues > &sensorsIn)
SotLoader::torques_
std::vector< double > torques_
Torques.
Definition: sot_loader.hh:60
SotLoader::startControlLoop
virtual void startControlLoop()
SotLoader::oneIteration
void oneIteration()
SotLoader
Definition: sot_loader.hh:46