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
39namespace po = boost::program_options;
40namespace dgs = dynamicgraph::sot;
41
43 protected:
44 // Dynamic graph is stopped.
46
48 dgs::AbstractSotExternalInterface* sotController_;
49
50 po::variables_map vm_;
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
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.
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_ */
Definition: sot_loader_basic.hh:42
XmlRpc::XmlRpcValue stateVectorMap_
Map between SoT state vector and some joint_state_links.
Definition: sot_loader_basic.hh:57
bool dynamic_graph_stopped_
Definition: sot_loader_basic.hh:45
std::string dynamicLibraryName_
Definition: sot_loader_basic.hh:51
ros::ServiceServer service_stop_
Advertises stop_dynamic_graph services.
Definition: sot_loader_basic.hh:69
std::vector< int > parallel_joints_to_state_vector_t
List of parallel joints from the state vector.
Definition: sot_loader_basic.hh:60
parallel_joints_to_state_vector_t::size_type nbOfParallelJoints_
Definition: sot_loader_basic.hh:79
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:66
~SotLoaderBasic()
Definition: sot_loader_basic.hh:83
dgs::AbstractSotExternalInterface * sotController_
the sot-hrp2 controller
Definition: sot_loader_basic.hh:48
void * sotRobotControllerLibrary_
Handle on the SoT library.
Definition: sot_loader_basic.hh:54
int parseOptions(int argc, char *argv[])
void setDynamicLibraryName(std::string &afilename)
int nbOfJoints_
Definition: sot_loader_basic.hh:78
ros::Publisher joint_pub_
Definition: sot_loader_basic.hh:72
bool start_dg(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response)
int readSotVectorStateParam()
bool isDynamicGraphStopped()
Definition: sot_loader_basic.hh:112
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:61
sensor_msgs::JointState joint_state_
Definition: sot_loader_basic.hh:75
void Initialization()
Load the SoT device corresponding to the robot.
po::variables_map vm_
Definition: sot_loader_basic.hh:50
std::vector< double > coefficient_parallel_joints_
Coefficient between parallel joints and the state vector.
Definition: sot_loader_basic.hh:64