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:
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_ */
Definition: sot_loader_basic.hh:41
Definition: sot_loader.hh:46
boost::thread thread_
The thread running dynamic graph.
Definition: sot_loader.hh:72
std::vector< double > angleEncoder_
Angular values read by encoders.
Definition: sot_loader.hh:54
void readControl(std::map< std::string, dgs::ControlValues > &controlValues)
std::map< std::string, dgs::ControlValues > controlValues_
Map of control values.
Definition: sot_loader.hh:51
void setup()
void fillSensors(std::map< std::string, dgs::SensorValues > &sensorsIn)
tf2_ros::TransformBroadcaster freeFlyerPublisher_
Definition: sot_loader.hh:78
std::map< std::string, dgs::SensorValues > sensorsIn_
Map of sensor readings.
Definition: sot_loader.hh:49
std::vector< double > baseAtt_
Attitude of the robot computed by extended Kalman filter.
Definition: sot_loader.hh:62
std::string robot_desc_string_
URDF string description of the robot.
Definition: sot_loader.hh:69
std::vector< double > accelerometer_
Accelerations read by Accelerometers.
Definition: sot_loader.hh:64
std::vector< double > forces_
Forces read by force sensors.
Definition: sot_loader.hh:58
std::vector< double > gyrometer_
Angular velocity read by gyrometers.
Definition: sot_loader.hh:66
std::vector< double > torques_
Torques.
Definition: sot_loader.hh:60
void oneIteration()
geometry_msgs::TransformStamped freeFlyerPose_
Definition: sot_loader.hh:79
std::vector< double > angleControl_
Angular values sent to motors.
Definition: sot_loader.hh:56
void initializeRosNode(int argc, char *argv[])
virtual void startControlLoop()