Go to the documentation of this file.
12 #ifndef _SOT_LOADER_BASIC_HH_
13 #define _SOT_LOADER_BASIC_HH_
22 #include <pinocchio/fwd.hpp>
25 #include <boost/program_options.hpp>
26 #include <boost/foreach.hpp>
27 #include <boost/format.hpp>
31 #include "std_srvs/Empty.h"
32 #include <sensor_msgs/JointState.h>
35 #include <sot/core/debug.hh>
36 #include <sot/core/abstract-sot-external-interface.hh>
38 namespace po = boost::program_options;
39 namespace dgs = dynamicgraph::sot;
49 po::variables_map
vm_;
97 bool start_dg(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
100 bool stop_dg(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
parallel_joints_to_state_vector_t::size_type nbOfParallelJoints_
Definition: sot_loader_basic.hh:78
parallel_joints_to_state_vector_t parallel_joints_to_state_vector_
Definition: sot_loader_basic.hh:60
int nbOfJoints_
Definition: sot_loader_basic.hh:77
void * sotRobotControllerLibrary_
Handle on the SoT library.
Definition: sot_loader_basic.hh:53
std::string dynamicLibraryName_
Definition: sot_loader_basic.hh:50
bool start_dg(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response)
int parseOptions(int argc, char *argv[])
Definition: sot_loader_basic.hh:41
std::vector< double > coefficient_parallel_joints_
Coefficient between parallel joints and the state vector.
Definition: sot_loader_basic.hh:63
ros::ServiceServer service_start_
Advertises start_dynamic_graph services.
Definition: sot_loader_basic.hh:65
int readSotVectorStateParam()
sensor_msgs::JointState joint_state_
Definition: sot_loader_basic.hh:74
void Initialization()
Load the SoT device corresponding to the robot.
bool isDynamicGraphStopped()
Definition: sot_loader_basic.hh:109
ros::Publisher joint_pub_
Definition: sot_loader_basic.hh:71
bool dynamic_graph_stopped_
Definition: sot_loader_basic.hh:44
XmlRpc::XmlRpcValue stateVectorMap_
Map between SoT state vector and some joint_state_links.
Definition: sot_loader_basic.hh:56
virtual void initializeRosNode(int argc, char *argv[])
bool stop_dg(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response)
po::variables_map vm_
Definition: sot_loader_basic.hh:49
void setDynamicLibraryName(std::string &afilename)
ros::ServiceServer service_stop_
Advertises stop_dynamic_graph services.
Definition: sot_loader_basic.hh:68
void CleanUp()
Unload the library which handles the robot device.
~SotLoaderBasic()
Definition: sot_loader_basic.hh:82
dgs::AbstractSotExternalInterface * sotController_
the sot-hrp2 controller
Definition: sot_loader_basic.hh:47
std::vector< int > parallel_joints_to_state_vector_t
List of parallel joints from the state vector.
Definition: sot_loader_basic.hh:59