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