-
Notifications
You must be signed in to change notification settings - Fork 27
/
sot_loader_basic.hh
132 lines (96 loc) · 3.85 KB
/
sot_loader_basic.hh
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
/*
* Copyright 2016,
* Olivier Stasse,
*
* CNRS
*
* This file is part of dynamic_graph_bridge.
* dynamic_graph_bridge is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
* dynamic_graph_bridge is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
* with sot-core. If not, see <http://www.gnu.org/licenses/>.
*/
/* -------------------------------------------------------------------------- */
/* --- INCLUDES ------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
#ifndef _SOT_LOADER_BASIC_HH_
#define _SOT_LOADER_BASIC_HH_
// System includes
#include <cassert>
// STL includes
#include <map>
// Boost includes
#include <boost/program_options.hpp>
#include <boost/foreach.hpp>
#include <boost/format.hpp>
// ROS includes
#include "ros/ros.h"
#include "std_srvs/Empty.h"
#include <sensor_msgs/JointState.h>
// Sot Framework includes
#include <sot/core/debug.hh>
#include <sot/core/abstract-sot-external-interface.hh>
namespace po = boost::program_options;
namespace dgs = dynamicgraph::sot;
class SotLoaderBasic {
protected:
// Dynamic graph is stopped.
bool dynamic_graph_stopped_;
/// \brief the sot-hrp2 controller
dgs::AbstractSotExternalInterface * sotController_;
po::variables_map vm_;
std::string dynamicLibraryName_;
/// \brief Handle on the SoT library.
void * sotRobotControllerLibrary_;
/// \brief Map between SoT state vector and some joint_state_links
XmlRpc::XmlRpcValue stateVectorMap_;
/// \brief List of parallel joints from the state vector.
typedef std::vector<int> parallel_joints_to_state_vector_t;
parallel_joints_to_state_vector_t parallel_joints_to_state_vector_;
/// \brief Coefficient between parallel joints and the state vector.
std::vector<double> coefficient_parallel_joints_;
/// Advertises start_dynamic_graph services
ros::ServiceServer service_start_;
/// Advertises stop_dynamic_graph services
ros::ServiceServer service_stop_;
// Joint state publication.
ros::Publisher joint_pub_;
// Joint state to be published.
sensor_msgs::JointState joint_state_;
// Number of DOFs according to KDL.
int nbOfJoints_;
parallel_joints_to_state_vector_t::size_type nbOfParallelJoints_;
public:
SotLoaderBasic();
~SotLoaderBasic() {};
// \brief Read user input to extract the path of the SoT dynamic library.
int parseOptions(int argc, char *argv[]);
/// \brief Load the SoT device corresponding to the robot.
void Initialization();
/// \brief Unload the library which handles the robot device.
void CleanUp();
// \brief Create a thread for ROS.
virtual void initializeRosNode(int argc, char *argv[]);
// \brief Callback function when starting dynamic graph.
bool start_dg(std_srvs::Empty::Request& request,
std_srvs::Empty::Response& response);
// \brief Callback function when stopping dynamic graph.
bool stop_dg(std_srvs::Empty::Request& request,
std_srvs::Empty::Response& response);
// \brief Read the state vector description based upon the robot links.
int readSotVectorStateParam();
// \brief Init publication of joint states.
int initPublication();
// \brief Get Status of dg.
bool isDynamicGraphStopped()
{ return dynamic_graph_stopped_; }
// \brief Specify the name of the dynamic library.
void setDynamicLibraryName(std::string &afilename);
};
#endif /* _SOT_LOADER_BASIC_HH_ */