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
44namespace po = boost::program_options;
45namespace dgs = dynamicgraph::sot;
46
47class 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.
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()