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 
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.
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_ */
Definition: sot_loader_basic.hh:41
XmlRpc::XmlRpcValue stateVectorMap_
Map between SoT state vector and some joint_state_links.
Definition: sot_loader_basic.hh:56
bool dynamic_graph_stopped_
Definition: sot_loader_basic.hh:44
std::string dynamicLibraryName_
Definition: sot_loader_basic.hh:50
ros::ServiceServer service_stop_
Advertises stop_dynamic_graph services.
Definition: sot_loader_basic.hh:68
std::vector< int > parallel_joints_to_state_vector_t
List of parallel joints from the state vector.
Definition: sot_loader_basic.hh:59
parallel_joints_to_state_vector_t::size_type nbOfParallelJoints_
Definition: sot_loader_basic.hh:78
void CleanUp()
Unload the library which handles the robot device.
virtual void initializeRosNode(int argc, char *argv[])
int initPublication()
ros::ServiceServer service_start_
Advertises start_dynamic_graph services.
Definition: sot_loader_basic.hh:65
~SotLoaderBasic()
Definition: sot_loader_basic.hh:82
dgs::AbstractSotExternalInterface * sotController_
the sot-hrp2 controller
Definition: sot_loader_basic.hh:47
void * sotRobotControllerLibrary_
Handle on the SoT library.
Definition: sot_loader_basic.hh:53
int parseOptions(int argc, char *argv[])
void setDynamicLibraryName(std::string &afilename)
int nbOfJoints_
Definition: sot_loader_basic.hh:77
ros::Publisher joint_pub_
Definition: sot_loader_basic.hh:71
bool start_dg(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response)
int readSotVectorStateParam()
bool isDynamicGraphStopped()
Definition: sot_loader_basic.hh:109
bool stop_dg(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response)
parallel_joints_to_state_vector_t parallel_joints_to_state_vector_
Definition: sot_loader_basic.hh:60
sensor_msgs::JointState joint_state_
Definition: sot_loader_basic.hh:74
void Initialization()
Load the SoT device corresponding to the robot.
po::variables_map vm_
Definition: sot_loader_basic.hh:49
std::vector< double > coefficient_parallel_joints_
Coefficient between parallel joints and the state vector.
Definition: sot_loader_basic.hh:63