From daf6a37fb62919701bf1fc87562eb2dea5bb45b2 Mon Sep 17 00:00:00 2001
From: Olivier Stasse <ostasse@laas.fr>
Date: Wed, 13 Jul 2016 09:02:12 +0200
Subject: [PATCH] Add sot_loader_basic class to decouple starting/stopping with
filling sensor, control computation and sending control.
---
CMakeLists.txt | 7 +-
include/dynamic_graph_bridge/sot_loader.hh | 75 ++----
.../dynamic_graph_bridge/sot_loader_basic.hh | 123 +++++++++
src/sot_loader.cpp | 195 +-------------
src/sot_loader_basic.cpp | 237 ++++++++++++++++++
5 files changed, 381 insertions(+), 256 deletions(-)
create mode 100644 include/dynamic_graph_bridge/sot_loader_basic.hh
create mode 100644 src/sot_loader_basic.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index aff7f76..6a43e0e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -37,6 +37,7 @@ set(${PROJECT_NAME}_HEADERS
include/dynamic_graph_bridge/ros_init.hh
include/dynamic_graph_bridge/ros_interpreter.hh
include/dynamic_graph_bridge/sot_loader.hh
+ include/dynamic_graph_bridge/sot_loader_basic.hh
)
include(cmake/base.cmake)
include(cmake/ros.cmake)
@@ -151,15 +152,15 @@ pkg_config_use_dependency(interpreter dynamic_graph_bridge_msgs)
#install(TARGETS interpreter DESTINATION bin)
# Stand alone embedded intepreter with a robot controller.
-add_executable(geometric_simu src/geometric_simu.cpp src/sot_loader.cpp)
+add_executable(geometric_simu src/geometric_simu.cpp src/sot_loader.cpp src/sot_loader_basic.cpp)
pkg_config_use_dependency(geometric_simu roscpp)
target_link_libraries(geometric_simu ros_bridge ${Boost_LIBRARIES} ${CMAKE_DL_LIBS})
# Sot loader library
-add_library(sot_loader src/sot_loader.cpp)
+add_library(sot_loader src/sot_loader.cpp src/sot_loader_basic.cpp)
pkg_config_use_dependency(sot_loader dynamic-graph)
pkg_config_use_dependency(sot_loader sot-core)
-target_link_libraries(sot_loader ${Boost_LIBRARIES})
+target_link_libraries(sot_loader ${Boost_LIBRARIES} roscpp)
install(TARGETS sot_loader DESTINATION lib)
diff --git a/include/dynamic_graph_bridge/sot_loader.hh b/include/dynamic_graph_bridge/sot_loader.hh
index 2c1faf1..c05a95a 100644
--- a/include/dynamic_graph_bridge/sot_loader.hh
+++ b/include/dynamic_graph_bridge/sot_loader.hh
@@ -1,25 +1,28 @@
/*
- * Copyright 2011,
+ * Copyright 2016,
* Olivier Stasse,
*
* CNRS
*
- * This file is part of sot-core.
- * sot-core is free software: you can redistribute it and/or
+ * This file is part of dynamic-graph-bridge.
+ * dynamic-graph-bridge is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
- * sot-core is distributed in the hope that it will be
+ * dynamic-graph-bridge is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
- * with sot-core. If not, see <http://www.gnu.org/licenses/>.
+ * with dynamic-graph-bridge. If not, see <http://www.gnu.org/licenses/>.
*/
/* -------------------------------------------------------------------------- */
/* --- INCLUDES ------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
+#ifndef _SOT_LOADER_HH_
+#define _SOT_LOADER_HH_
+
// System includes
#include <iostream>
#include <cassert>
@@ -41,22 +44,18 @@
#include <sot/core/debug.hh>
#include <sot/core/abstract-sot-external-interface.hh>
+// Dynamic-graph-bridge includes.
+#include <dynamic_graph_bridge/sot_loader_basic.hh>
+
namespace po = boost::program_options;
namespace dgs = dynamicgraph::sot;
-class SotLoader {
+class SotLoader: public SotLoaderBasic
+{
protected:
- // Dynamic graph is stopped.
- bool dynamic_graph_stopped_;
-
- /// \brief the sot-hrp2 controller
- dgs::AbstractSotExternalInterface * sotController_;
-
- po::variables_map vm_;
- std::string dynamicLibraryName_;
/// Map of sensor readings
std::map <std::string,dgs::SensorValues> sensorsIn_;
@@ -81,42 +80,15 @@ protected:
/// URDF string description of the robot.
std::string robot_desc_string_;
- /// \brief Map between SoT state vector and some joint_state_links
- XmlRpc::XmlRpcValue stateVectorMap_;
-
- /// \brief List of parallel joints from the state vector.
- typedef std::vector<int> parallel_joints_to_state_vector_t;
- parallel_joints_to_state_vector_t parallel_joints_to_state_vector_;
-
- /// \brief Coefficient between parallel joints and the state vector.
- std::vector<double> coefficient_parallel_joints_;
-
- // Joint state publication.
- ros::Publisher joint_pub_;
-
- // Joint state to be published.
- sensor_msgs::JointState joint_state_;
// \brief Start control loop
virtual void startControlLoop();
- // Number of DOFs according to KDL.
- int nbOfJoints_;
- parallel_joints_to_state_vector_t::size_type nbOfParallelJoints_;
-
public:
SotLoader();
~SotLoader() {};
- // \brief Read user input to extract the path of the SoT dynamic library.
- int parseOptions(int argc, char *argv[]);
-
- // \brief Load the SoT
- void Initialization();
-
- // \brief Create a thread for ROS.
- void initializeRosNode(int argc, char *argv[]);
// \brief Compute one iteration of control.
// Basically calls fillSensors, the SoT and the readControl.
@@ -133,26 +105,7 @@ public:
// \brief Prepare the SoT framework.
void setup();
- // \brief Callback function when starting dynamic graph.
- bool start_dg(std_srvs::Empty::Request& request,
- std_srvs::Empty::Response& response);
-
- // \brief Callback function when stopping dynamic graph.
- bool stop_dg(std_srvs::Empty::Request& request,
- std_srvs::Empty::Response& response);
-
- // \brief Read the state vector description based upon the robot links.
- int readSotVectorStateParam();
-
- // \brief Init publication of joint states.
- int initPublication();
-
- // \brief Get Status of dg.
- bool isDynamicGraphStopped()
- { return dynamic_graph_stopped_; }
-
- // \brief Specify the name of the dynamic library.
- void setDynamicLibraryName(std::string &afilename);
};
+#endif /* SOT_LOADER_HH_ */
diff --git a/include/dynamic_graph_bridge/sot_loader_basic.hh b/include/dynamic_graph_bridge/sot_loader_basic.hh
new file mode 100644
index 0000000..10b13b7
--- /dev/null
+++ b/include/dynamic_graph_bridge/sot_loader_basic.hh
@@ -0,0 +1,123 @@
+/*
+ * Copyright 2016,
+ * Olivier Stasse,
+ *
+ * CNRS
+ *
+ * This file is part of dynamic_graph_bridge.
+ * dynamic_graph_bridge is free software: you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public License
+ * as published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ * dynamic_graph_bridge is distributed in the hope that it will be
+ * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details. You should
+ * have received a copy of the GNU Lesser General Public License along
+ * with sot-core. If not, see <http://www.gnu.org/licenses/>.
+ */
+/* -------------------------------------------------------------------------- */
+/* --- INCLUDES ------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+
+#ifndef _SOT_LOADER_BASIC_HH_
+#define _SOT_LOADER_BASIC_HH_
+
+// System includes
+#include <iostream>
+#include <cassert>
+
+// STL includes
+#include <map>
+
+// Boost includes
+#include <boost/program_options.hpp>
+#include <boost/foreach.hpp>
+#include <boost/format.hpp>
+
+// ROS includes
+#include "ros/ros.h"
+#include "std_srvs/Empty.h"
+#include <sensor_msgs/JointState.h>
+
+// Sot Framework includes
+#include <sot/core/debug.hh>
+#include <sot/core/abstract-sot-external-interface.hh>
+
+namespace po = boost::program_options;
+namespace dgs = dynamicgraph::sot;
+
+
+class SotLoaderBasic {
+protected:
+
+ // Dynamic graph is stopped.
+ bool dynamic_graph_stopped_;
+
+ /// \brief the sot-hrp2 controller
+ dgs::AbstractSotExternalInterface * sotController_;
+
+ po::variables_map vm_;
+ std::string dynamicLibraryName_;
+
+
+ /// \brief Map between SoT state vector and some joint_state_links
+ XmlRpc::XmlRpcValue stateVectorMap_;
+
+ /// \brief List of parallel joints from the state vector.
+ typedef std::vector<int> parallel_joints_to_state_vector_t;
+ parallel_joints_to_state_vector_t parallel_joints_to_state_vector_;
+
+ /// \brief Coefficient between parallel joints and the state vector.
+ std::vector<double> coefficient_parallel_joints_;
+
+ // Joint state publication.
+ ros::Publisher joint_pub_;
+
+ // Joint state to be published.
+ sensor_msgs::JointState joint_state_;
+
+ // Number of DOFs according to KDL.
+ int nbOfJoints_;
+ parallel_joints_to_state_vector_t::size_type nbOfParallelJoints_;
+
+
+public:
+ SotLoaderBasic();
+ ~SotLoaderBasic() {};
+
+ // \brief Read user input to extract the path of the SoT dynamic library.
+ int parseOptions(int argc, char *argv[]);
+
+ // \brief Load the SoT
+ void Initialization();
+
+ // \brief Create a thread for ROS.
+ void initializeRosNode(int argc, char *argv[]);
+
+
+ // \brief Callback function when starting dynamic graph.
+ bool start_dg(std_srvs::Empty::Request& request,
+ std_srvs::Empty::Response& response);
+
+ // \brief Callback function when stopping dynamic graph.
+ bool stop_dg(std_srvs::Empty::Request& request,
+ std_srvs::Empty::Response& response);
+
+ // \brief Read the state vector description based upon the robot links.
+ int readSotVectorStateParam();
+
+ // \brief Init publication of joint states.
+ int initPublication();
+
+ // \brief Get Status of dg.
+ bool isDynamicGraphStopped()
+ { return dynamic_graph_stopped_; }
+
+ // \brief Specify the name of the dynamic library.
+ void setDynamicLibraryName(std::string &afilename);
+
+
+};
+
+#endif /* _SOT_LOADER_BASIC_HH_ */
diff --git a/src/sot_loader.cpp b/src/sot_loader.cpp
index 575f804..8c3a4a0 100644
--- a/src/sot_loader.cpp
+++ b/src/sot_loader.cpp
@@ -30,28 +30,12 @@
#include <boost/thread/condition.hpp>
boost::condition_variable cond;
-boost::mutex mut;
using namespace std;
using namespace dynamicgraph::sot;
namespace po = boost::program_options;
-void createRosSpin(SotLoader *aSotLoader)
-{
- ROS_INFO("createRosSpin started\n");
- ros::NodeHandle n;
-
- ros::ServiceServer service = n.advertiseService("start_dynamic_graph",
- &SotLoader::start_dg,
- aSotLoader);
-
- ros::ServiceServer service2 = n.advertiseService("stop_dynamic_graph",
- &SotLoader::stop_dg,
- aSotLoader);
-
- ros::waitForShutdown();
-}
void workThreadLoader(SotLoader *aSotLoader)
{
@@ -70,7 +54,6 @@ void workThreadLoader(SotLoader *aSotLoader)
}
SotLoader::SotLoader():
- dynamic_graph_stopped_(true),
sensorsIn_ (),
controlValues_ (),
angleEncoder_ (),
@@ -85,171 +68,12 @@ SotLoader::SotLoader():
initPublication();
}
-int SotLoader::initPublication()
-{
- ros::NodeHandle & n = dynamicgraph::rosInit(false);
-
-
- // Prepare message to be published
- joint_pub_ = n.advertise<sensor_msgs::JointState>("joint_states", 1);
-
- return 0;
-}
-
void SotLoader::startControlLoop()
{
boost::thread thr(workThreadLoader, this);
}
-void SotLoader::initializeRosNode(int , char *[])
-{
- ROS_INFO("Ready to start dynamic graph.");
- boost::unique_lock<boost::mutex> lock(mut);
- boost::thread thr2(createRosSpin,this);
-
- startControlLoop();
-}
-
-
-void SotLoader::setDynamicLibraryName(std::string &afilename)
-{
- dynamicLibraryName_ = afilename;
-}
-
-int SotLoader::readSotVectorStateParam()
-{
- std::map<std::string,int> from_state_name_to_state_vector;
- std::map<std::string,std::string> from_parallel_name_to_state_vector_name;
- ros::NodeHandle n;
-
- if (!ros::param::has("/sot/state_vector_map"))
- {
- std::cerr<< " Read Sot Vector State Param " << std::endl;
- return 1;
- }
-
- n.getParam("/sot/state_vector_map", stateVectorMap_);
- ROS_ASSERT(stateVectorMap_.getType() == XmlRpc::XmlRpcValue::TypeArray);
- nbOfJoints_ = stateVectorMap_.size();
-
- if (ros::param::has("/sot/joint_state_parallel"))
- {
- XmlRpc::XmlRpcValue joint_state_parallel;
- n.getParam("/sot/joint_state_parallel", joint_state_parallel);
- ROS_ASSERT(joint_state_parallel.getType() == XmlRpc::XmlRpcValue::TypeStruct);
- std::cout << "Type of joint_state_parallel:" << joint_state_parallel.getType() << std::endl;
-
- for(XmlRpc::XmlRpcValue::iterator it = joint_state_parallel.begin();
- it!= joint_state_parallel.end(); it++)
- {
- XmlRpc::XmlRpcValue local_value=it->second;
- std::string final_expression, map_expression = static_cast<string>(local_value);
- double final_coefficient = 1.0;
- // deal with sign
- if (map_expression[0]=='-')
- {
- final_expression = map_expression.substr(1,map_expression.size()-1);
- final_coefficient = -1.0;
- }
- else
- final_expression = map_expression;
-
- std::cout <<it->first.c_str() << ": " << final_coefficient << std::endl;
- from_parallel_name_to_state_vector_name[it->first.c_str()] = final_expression;
- coefficient_parallel_joints_.push_back(final_coefficient);
-
- }
- nbOfParallelJoints_ = from_parallel_name_to_state_vector_name.size();
- }
-
- // Prepare joint_state according to robot description.
- joint_state_.name.resize(nbOfJoints_+nbOfParallelJoints_);
- joint_state_.position.resize(nbOfJoints_+nbOfParallelJoints_);
-
- // Fill in the name of the joints from the state vector.
- // and build local map from state name to state vector
- for (int32_t i = 0; i < stateVectorMap_.size(); ++i)
- {
- joint_state_.name[i]= static_cast<string>(stateVectorMap_[i]);
-
- from_state_name_to_state_vector[joint_state_.name[i]] = i;
- }
-
- // Fill in the name of the joints from the parallel joint vector.
- // and build map from parallel name to state vector
- int i=0;
- parallel_joints_to_state_vector_.resize(nbOfParallelJoints_);
- for (std::map<std::string,std::string>::iterator it = from_parallel_name_to_state_vector_name.begin();
- it != from_parallel_name_to_state_vector_name.end();
- it++,i++)
- {
- joint_state_.name[i+nbOfJoints_]=it->first.c_str();
- parallel_joints_to_state_vector_[i] = from_state_name_to_state_vector[it->second];
- }
-
- angleEncoder_.resize(nbOfJoints_);
-
- return 0;
-}
-
-
-int SotLoader::parseOptions(int argc, char *argv[])
-{
- po::options_description desc("Allowed options");
- desc.add_options()
- ("help", "produce help message")
- ("input-file", po::value<string>(), "library to load")
- ;
-
-
- po::store(po::parse_command_line(argc, argv, desc), vm_);
- po::notify(vm_);
-
- if (vm_.count("help")) {
- cout << desc << "\n";
- return -1;
- }
- if (!vm_.count("input-file")) {
- cout << "No filename specified\n";
- return -1;
- }
- else
- dynamicLibraryName_= vm_["input-file"].as< string >();
-
- Initialization();
- return 0;
-}
-void SotLoader::Initialization()
-{
-
- // Load the SotRobotBipedController library.
- void * SotRobotControllerLibrary = dlopen( dynamicLibraryName_.c_str(),
- RTLD_LAZY | RTLD_GLOBAL );
- if (!SotRobotControllerLibrary) {
- std::cerr << "Cannot load library: " << dlerror() << '\n';
- return ;
- }
-
- // reset errors
- dlerror();
-
- // Load the symbols.
- createSotExternalInterface_t * createSot =
- reinterpret_cast<createSotExternalInterface_t *>
- (reinterpret_cast<long>
- (dlsym(SotRobotControllerLibrary,
- "createSotExternalInterface")));
- const char* dlsym_error = dlerror();
- if (dlsym_error) {
- std::cerr << "Cannot load symbol create: " << dlsym_error << '\n';
- return ;
- }
-
- // Create robot-controller
- sotController_ = createSot();
- cout <<"Went out from Initialization." << endl;
-}
void
SotLoader::fillSensors(map<string,dgs::SensorValues> & sensorsIn)
@@ -259,8 +83,7 @@ SotLoader::fillSensors(map<string,dgs::SensorValues> & sensorsIn)
sensorsIn["joints"].setName("angle");
for(unsigned int i=0;i<angleControl_.size();i++)
angleEncoder_[i] = angleControl_[i];
- sensorsIn["joints"].setValues(angleEncoder_);
-
+ sensorsIn["joints"].setValues(angleEncoder_);
}
void
@@ -298,6 +121,8 @@ SotLoader::readControl(map<string,dgs::ControlValues> &controlValues)
void SotLoader::setup()
{
+ angleEncoder_.resize(nbOfJoints_);
+
fillSensors(sensorsIn_);
sotController_->setupSetSensors(sensorsIn_);
sotController_->getControl(controlValues_);
@@ -318,17 +143,3 @@ void SotLoader::oneIteration()
}
-bool SotLoader::start_dg(std_srvs::Empty::Request& ,
- std_srvs::Empty::Response& )
-{
- dynamic_graph_stopped_=false;
- return true;
-}
-
-bool SotLoader::stop_dg(std_srvs::Empty::Request& ,
- std_srvs::Empty::Response& )
-{
- dynamic_graph_stopped_ = true;
- return true;
-}
-
diff --git a/src/sot_loader_basic.cpp b/src/sot_loader_basic.cpp
new file mode 100644
index 0000000..e9c3f19
--- /dev/null
+++ b/src/sot_loader_basic.cpp
@@ -0,0 +1,237 @@
+/*
+ * Copyright 2016,
+ * Olivier Stasse,
+ *
+ * CNRS
+ *
+ * This file is part of dynamic_graph_bridge.
+ * dynamic_graph_bridge is free software: you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public License
+ * as published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ * dynamic_graph_bridge is distributed in the hope that it will be
+ * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details. You should
+ * have received a copy of the GNU Lesser General Public License along
+ * with dynamic_graph_bridge. If not, see <http://www.gnu.org/licenses/>.
+ */
+/* -------------------------------------------------------------------------- */
+/* --- INCLUDES ------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+
+#include <dynamic_graph_bridge/sot_loader.hh>
+#include "dynamic_graph_bridge/ros_init.hh"
+
+// POSIX.1-2001
+#include <dlfcn.h>
+
+#include <boost/thread/thread.hpp>
+#include <boost/thread/condition.hpp>
+
+boost::mutex mut;
+
+using namespace std;
+using namespace dynamicgraph::sot;
+namespace po = boost::program_options;
+
+void createRosSpin(SotLoaderBasic *aSotLoaderBasic)
+{
+ ROS_INFO("createRosSpin started\n");
+ ros::NodeHandle n;
+
+ ros::ServiceServer service = n.advertiseService("start_dynamic_graph",
+ &SotLoaderBasic::start_dg,
+ aSotLoaderBasic);
+
+ ros::ServiceServer service2 = n.advertiseService("stop_dynamic_graph",
+ &SotLoaderBasic::stop_dg,
+ aSotLoaderBasic);
+
+
+ ros::waitForShutdown();
+}
+
+
+SotLoaderBasic::SotLoaderBasic():
+ dynamic_graph_stopped_(true)
+{
+ readSotVectorStateParam();
+ initPublication();
+}
+
+int SotLoaderBasic::initPublication()
+{
+ ros::NodeHandle & n = dynamicgraph::rosInit(false);
+
+
+ // Prepare message to be published
+ joint_pub_ = n.advertise<sensor_msgs::JointState>("joint_states", 1);
+
+ return 0;
+}
+
+
+void SotLoaderBasic::initializeRosNode(int , char *[])
+{
+ ROS_INFO("Ready to start dynamic graph.");
+ boost::unique_lock<boost::mutex> lock(mut);
+ boost::thread thr2(createRosSpin,this);
+
+}
+
+
+void SotLoaderBasic::setDynamicLibraryName(std::string &afilename)
+{
+ dynamicLibraryName_ = afilename;
+}
+
+int SotLoaderBasic::readSotVectorStateParam()
+{
+ std::map<std::string,int> from_state_name_to_state_vector;
+ std::map<std::string,std::string> from_parallel_name_to_state_vector_name;
+ ros::NodeHandle n;
+
+ if (!ros::param::has("/sot/state_vector_map"))
+ {
+ std::cerr<< " Read Sot Vector State Param " << std::endl;
+ return 1;
+ }
+
+ n.getParam("/sot/state_vector_map", stateVectorMap_);
+ ROS_ASSERT(stateVectorMap_.getType() == XmlRpc::XmlRpcValue::TypeArray);
+ nbOfJoints_ = stateVectorMap_.size();
+
+ if (ros::param::has("/sot/joint_state_parallel"))
+ {
+ XmlRpc::XmlRpcValue joint_state_parallel;
+ n.getParam("/sot/joint_state_parallel", joint_state_parallel);
+ ROS_ASSERT(joint_state_parallel.getType() == XmlRpc::XmlRpcValue::TypeStruct);
+ std::cout << "Type of joint_state_parallel:" << joint_state_parallel.getType() << std::endl;
+
+ for(XmlRpc::XmlRpcValue::iterator it = joint_state_parallel.begin();
+ it!= joint_state_parallel.end(); it++)
+ {
+ XmlRpc::XmlRpcValue local_value=it->second;
+ std::string final_expression, map_expression = static_cast<string>(local_value);
+ double final_coefficient = 1.0;
+ // deal with sign
+ if (map_expression[0]=='-')
+ {
+ final_expression = map_expression.substr(1,map_expression.size()-1);
+ final_coefficient = -1.0;
+ }
+ else
+ final_expression = map_expression;
+
+ std::cout <<it->first.c_str() << ": " << final_coefficient << std::endl;
+ from_parallel_name_to_state_vector_name[it->first.c_str()] = final_expression;
+ coefficient_parallel_joints_.push_back(final_coefficient);
+
+ }
+ nbOfParallelJoints_ = from_parallel_name_to_state_vector_name.size();
+ }
+
+ // Prepare joint_state according to robot description.
+ joint_state_.name.resize(nbOfJoints_+nbOfParallelJoints_);
+ joint_state_.position.resize(nbOfJoints_+nbOfParallelJoints_);
+
+ // Fill in the name of the joints from the state vector.
+ // and build local map from state name to state vector
+ for (int32_t i = 0; i < stateVectorMap_.size(); ++i)
+ {
+ joint_state_.name[i]= static_cast<string>(stateVectorMap_[i]);
+
+ from_state_name_to_state_vector[joint_state_.name[i]] = i;
+ }
+
+ // Fill in the name of the joints from the parallel joint vector.
+ // and build map from parallel name to state vector
+ int i=0;
+ parallel_joints_to_state_vector_.resize(nbOfParallelJoints_);
+ for (std::map<std::string,std::string>::iterator it = from_parallel_name_to_state_vector_name.begin();
+ it != from_parallel_name_to_state_vector_name.end();
+ it++,i++)
+ {
+ joint_state_.name[i+nbOfJoints_]=it->first.c_str();
+ parallel_joints_to_state_vector_[i] = from_state_name_to_state_vector[it->second];
+ }
+
+ return 0;
+}
+
+
+int SotLoaderBasic::parseOptions(int argc, char *argv[])
+{
+ po::options_description desc("Allowed options");
+ desc.add_options()
+ ("help", "produce help message")
+ ("input-file", po::value<string>(), "library to load")
+ ;
+
+
+ po::store(po::parse_command_line(argc, argv, desc), vm_);
+ po::notify(vm_);
+
+ if (vm_.count("help")) {
+ cout << desc << "\n";
+ return -1;
+ }
+ if (!vm_.count("input-file")) {
+ cout << "No filename specified\n";
+ return -1;
+ }
+ else
+ dynamicLibraryName_= vm_["input-file"].as< string >();
+
+ Initialization();
+ return 0;
+}
+
+void SotLoaderBasic::Initialization()
+{
+
+ // Load the SotRobotBipedController library.
+ void * SotRobotControllerLibrary = dlopen( dynamicLibraryName_.c_str(),
+ RTLD_LAZY | RTLD_GLOBAL );
+ if (!SotRobotControllerLibrary) {
+ std::cerr << "Cannot load library: " << dlerror() << '\n';
+ return ;
+ }
+
+ // reset errors
+ dlerror();
+
+ // Load the symbols.
+ createSotExternalInterface_t * createSot =
+ reinterpret_cast<createSotExternalInterface_t *>
+ (reinterpret_cast<long>
+ (dlsym(SotRobotControllerLibrary,
+ "createSotExternalInterface")));
+ const char* dlsym_error = dlerror();
+ if (dlsym_error) {
+ std::cerr << "Cannot load symbol create: " << dlsym_error << '\n';
+ return ;
+ }
+
+ // Create robot-controller
+ sotController_ = createSot();
+ cout <<"Went out from Initialization." << endl;
+}
+
+
+
+bool SotLoaderBasic::start_dg(std_srvs::Empty::Request& ,
+ std_srvs::Empty::Response& )
+{
+ dynamic_graph_stopped_=false;
+ return true;
+}
+
+bool SotLoaderBasic::stop_dg(std_srvs::Empty::Request& ,
+ std_srvs::Empty::Response& )
+{
+ dynamic_graph_stopped_ = true;
+ return true;
+}
+
--
GitLab