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/foreach.hpp>
26 #include <boost/format.hpp>
27 #include <boost/program_options.hpp>
28 #include <boost/thread/thread.hpp>
29 
30 // ROS includes
31 #include <sensor_msgs/JointState.h>
32 #include <tf2_ros/transform_broadcaster.h>
33 
34 #include "ros/ros.h"
35 #include "std_srvs/Empty.h"
36 
37 // Sot Framework includes
38 #include <sot/core/abstract-sot-external-interface.hh>
39 #include <sot/core/debug.hh>
40 
41 // Dynamic-graph-bridge includes.
43 
44 namespace po = boost::program_options;
45 namespace dgs = dynamicgraph::sot;
46 
47 class SotLoader : public SotLoaderBasic {
48  protected:
50  std::map<std::string, dgs::SensorValues> sensorsIn_;
52  std::map<std::string, dgs::ControlValues> controlValues_;
53 
55  std::vector<double> angleEncoder_;
57  std::vector<double> angleControl_;
59  std::vector<double> forces_;
61  std::vector<double> torques_;
63  std::vector<double> baseAtt_;
65  std::vector<double> accelerometer_;
67  std::vector<double> gyrometer_;
68 
70  std::string robot_desc_string_;
71 
73  boost::thread thread_;
74 
75  // \brief Start control loop
76  virtual void startControlLoop();
77 
78  // Robot Pose Publisher
79  tf2_ros::TransformBroadcaster freeFlyerPublisher_;
80  geometry_msgs::TransformStamped freeFlyerPose_;
81 
82  public:
85 
86  // \brief Create a thread for ROS and start the control loop.
87  void initializeRosNode(int argc, char *argv[]);
88 
89  // \brief Compute one iteration of control.
90  // Basically calls fillSensors, the SoT and the readControl.
91  void oneIteration();
92 
93  // \brief Fill the sensors value for the SoT.
94  void fillSensors(std::map<std::string, dgs::SensorValues> &sensorsIn);
95 
96  // \brief Read the control computed by the SoT framework.
97  void readControl(std::map<std::string, dgs::ControlValues> &controlValues);
98 
99  // \brief Prepare the SoT framework.
100  void setup();
101 };
102 
103 #endif /* SOT_LOADER_HH_ */
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 setup()
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
void oneIteration()
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()