sot_loader_basic.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_BASIC_HH_
13 #define _SOT_LOADER_BASIC_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 
29 // ROS includes
30 #include <sensor_msgs/JointState.h>
31 
32 #include "ros/ros.h"
33 #include "std_srvs/Empty.h"
34 
35 // Sot Framework includes
36 #include <sot/core/abstract-sot-external-interface.hh>
37 #include <sot/core/debug.hh>
38 
39 namespace po = boost::program_options;
40 namespace dgs = dynamicgraph::sot;
41 
43  protected:
44  // Dynamic graph is stopped.
46 
48  dgs::AbstractSotExternalInterface* sotController_;
49 
50  po::variables_map vm_;
51  std::string dynamicLibraryName_;
52 
55 
57  XmlRpc::XmlRpcValue stateVectorMap_;
58 
60  typedef std::vector<int> parallel_joints_to_state_vector_t;
62 
64  std::vector<double> coefficient_parallel_joints_;
66  ros::ServiceServer service_start_;
67 
69  ros::ServiceServer service_stop_;
70 
71  // Joint state publication.
72  ros::Publisher joint_pub_;
73 
74  // Joint state to be published.
75  sensor_msgs::JointState joint_state_;
76 
77  // Number of DOFs according to KDL.
79  parallel_joints_to_state_vector_t::size_type nbOfParallelJoints_;
80 
81  public:
84 
85  // \brief Read user input to extract the path of the SoT dynamic library.
86  int parseOptions(int argc, char* argv[]);
87 
89  void Initialization();
90 
92  void CleanUp();
93 
94  // \brief Create a thread for ROS.
95  virtual void initializeRosNode(int argc, char* argv[]);
96 
97  // \brief Callback function when starting dynamic graph.
98  bool start_dg(std_srvs::Empty::Request& request,
99  std_srvs::Empty::Response& response);
100 
101  // \brief Callback function when stopping dynamic graph.
102  bool stop_dg(std_srvs::Empty::Request& request,
103  std_srvs::Empty::Response& response);
104 
105  // \brief Read the state vector description based upon the robot links.
107 
108  // \brief Init publication of joint states.
109  int initPublication();
110 
111  // \brief Get Status of dg.
113 
114  // \brief Specify the name of the dynamic library.
115  void setDynamicLibraryName(std::string& afilename);
116 };
117 
118 #endif /* _SOT_LOADER_BASIC_HH_ */
SotLoaderBasic::nbOfParallelJoints_
parallel_joints_to_state_vector_t::size_type nbOfParallelJoints_
Definition: sot_loader_basic.hh:79
SotLoaderBasic::parallel_joints_to_state_vector_
parallel_joints_to_state_vector_t parallel_joints_to_state_vector_
Definition: sot_loader_basic.hh:61
SotLoaderBasic::SotLoaderBasic
SotLoaderBasic()
SotLoaderBasic::nbOfJoints_
int nbOfJoints_
Definition: sot_loader_basic.hh:78
SotLoaderBasic::sotRobotControllerLibrary_
void * sotRobotControllerLibrary_
Handle on the SoT library.
Definition: sot_loader_basic.hh:54
SotLoaderBasic::dynamicLibraryName_
std::string dynamicLibraryName_
Definition: sot_loader_basic.hh:51
SotLoaderBasic::start_dg
bool start_dg(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response)
SotLoaderBasic::parseOptions
int parseOptions(int argc, char *argv[])
SotLoaderBasic
Definition: sot_loader_basic.hh:42
SotLoaderBasic::initPublication
int initPublication()
SotLoaderBasic::coefficient_parallel_joints_
std::vector< double > coefficient_parallel_joints_
Coefficient between parallel joints and the state vector.
Definition: sot_loader_basic.hh:64
SotLoaderBasic::service_start_
ros::ServiceServer service_start_
Advertises start_dynamic_graph services.
Definition: sot_loader_basic.hh:66
SotLoaderBasic::readSotVectorStateParam
int readSotVectorStateParam()
SotLoaderBasic::joint_state_
sensor_msgs::JointState joint_state_
Definition: sot_loader_basic.hh:75
SotLoaderBasic::Initialization
void Initialization()
Load the SoT device corresponding to the robot.
SotLoaderBasic::isDynamicGraphStopped
bool isDynamicGraphStopped()
Definition: sot_loader_basic.hh:112
SotLoaderBasic::joint_pub_
ros::Publisher joint_pub_
Definition: sot_loader_basic.hh:72
SotLoaderBasic::dynamic_graph_stopped_
bool dynamic_graph_stopped_
Definition: sot_loader_basic.hh:45
SotLoaderBasic::stateVectorMap_
XmlRpc::XmlRpcValue stateVectorMap_
Map between SoT state vector and some joint_state_links.
Definition: sot_loader_basic.hh:57
SotLoaderBasic::initializeRosNode
virtual void initializeRosNode(int argc, char *argv[])
SotLoaderBasic::stop_dg
bool stop_dg(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response)
SotLoaderBasic::vm_
po::variables_map vm_
Definition: sot_loader_basic.hh:50
SotLoaderBasic::setDynamicLibraryName
void setDynamicLibraryName(std::string &afilename)
SotLoaderBasic::service_stop_
ros::ServiceServer service_stop_
Advertises stop_dynamic_graph services.
Definition: sot_loader_basic.hh:69
SotLoaderBasic::CleanUp
void CleanUp()
Unload the library which handles the robot device.
SotLoaderBasic::~SotLoaderBasic
~SotLoaderBasic()
Definition: sot_loader_basic.hh:83
SotLoaderBasic::sotController_
dgs::AbstractSotExternalInterface * sotController_
the sot-hrp2 controller
Definition: sot_loader_basic.hh:48
SotLoaderBasic::parallel_joints_to_state_vector_t
std::vector< int > parallel_joints_to_state_vector_t
List of parallel joints from the state vector.
Definition: sot_loader_basic.hh:60