Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/sot_loader_basic.cpp	Lines:	0	107	0.0 %
Date:	2022-09-08 07:44:08	Branches:	0	230	0.0 %


/*
 * Copyright 2016,
 * Olivier Stasse,
 *
 * CNRS
 *
 */
/* -------------------------------------------------------------------------- */
/* --- INCLUDES ------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */

#include <dynamic-graph/pool.h>

#include <dynamic_graph_bridge/sot_loader.hh>

#include "dynamic_graph_bridge/ros_init.hh"
#include "dynamic_graph_bridge/ros_parameter.hh"

// POSIX.1-2001
#include <dlfcn.h>

using namespace std;
using namespace dynamicgraph::sot;
namespace po = boost::program_options;

SotLoaderBasic::SotLoaderBasic()
    : dynamic_graph_stopped_(true), sotRobotControllerLibrary_(0) {
  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.");
  ros::NodeHandle n;
  service_start_ = n.advertiseService("start_dynamic_graph",
                                      &SotLoaderBasic::start_dg, this);

  service_stop_ =
      n.advertiseService("stop_dynamic_graph", &SotLoaderBasic::stop_dg, this);

  dynamicgraph::parameter_server_read_robot_description();
}

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();
  nbOfParallelJoints_ = 0;

  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>();

  return 0;
}

void SotLoaderBasic::Initialization() {
  // Load the SotRobotBipedController library.
  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 SotLoaderBasic::CleanUp() {
  dynamicgraph::PoolStorage::destroy();
  // We do not destroy the FactoryStorage singleton because the module will not
  // be reloaded at next initialization (because Python C API cannot safely
  // unload a module...).
  // SignalCaster singleton could probably be destroyed.

  // Load the symbols.
  destroySotExternalInterface_t* destroySot =
      reinterpret_cast<destroySotExternalInterface_t*>(reinterpret_cast<long>(
          dlsym(sotRobotControllerLibrary_, "destroySotExternalInterface")));
  const char* dlsym_error = dlerror();
  if (dlsym_error) {
    std::cerr << "Cannot load symbol destroy: " << dlsym_error << '\n';
    return;
  }

  destroySot(sotController_);
  sotController_ = NULL;

  /// Uncount the number of access to this library.
  dlclose(sotRobotControllerLibrary_);
}

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;
}


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			/*
2			* Copyright 2016,
3			* Olivier Stasse,
4			*
5			* CNRS
6			*
7			*/
8			/* -------------------------------------------------------------------------- */
9			/* --- INCLUDES ------------------------------------------------------------- */
10			/* -------------------------------------------------------------------------- */
11
12			#include <dynamic-graph/pool.h>
13
14			#include <dynamic_graph_bridge/sot_loader.hh>
15
16			#include "dynamic_graph_bridge/ros_init.hh"
17			#include "dynamic_graph_bridge/ros_parameter.hh"
18
19			// POSIX.1-2001
20			#include <dlfcn.h>
21
22			using namespace std;
23			using namespace dynamicgraph::sot;
24			namespace po = boost::program_options;
25
26			SotLoaderBasic::SotLoaderBasic()
27			: dynamic_graph_stopped_(true), sotRobotControllerLibrary_(0) {
28			readSotVectorStateParam();
29			initPublication();
30			}
31
32			int SotLoaderBasic::initPublication() {
33			ros::NodeHandle& n = dynamicgraph::rosInit(false);
34
35			// Prepare message to be published
36			joint_pub_ = n.advertise<sensor_msgs::JointState>("joint_states", 1);
37
38			return 0;
39			}
40
41			void SotLoaderBasic::initializeRosNode(int, char*[]) {
42			ROS_INFO("Ready to start dynamic graph.");
43			ros::NodeHandle n;
44			service_start_ = n.advertiseService("start_dynamic_graph",
45			&SotLoaderBasic::start_dg, this);
46
47			service_stop_ =
48			n.advertiseService("stop_dynamic_graph", &SotLoaderBasic::stop_dg, this);
49
50			dynamicgraph::parameter_server_read_robot_description();
51			}
52
53			void SotLoaderBasic::setDynamicLibraryName(std::string& afilename) {
54			dynamicLibraryName_ = afilename;
55			}
56
57			int SotLoaderBasic::readSotVectorStateParam() {
58			std::map<std::string, int> from_state_name_to_state_vector;
59			std::map<std::string, std::string> from_parallel_name_to_state_vector_name;
60			ros::NodeHandle n;
61
62			if (!ros::param::has("/sot/state_vector_map")) {
63			std::cerr << " Read Sot Vector State Param " << std::endl;
64			return 1;
65			}
66
67			n.getParam("/sot/state_vector_map", stateVectorMap_);
68			ROS_ASSERT(stateVectorMap_.getType() == XmlRpc::XmlRpcValue::TypeArray);
69			nbOfJoints_ = stateVectorMap_.size();
70			nbOfParallelJoints_ = 0;
71
72			if (ros::param::has("/sot/joint_state_parallel")) {
73			XmlRpc::XmlRpcValue joint_state_parallel;
74			n.getParam("/sot/joint_state_parallel", joint_state_parallel);
75			ROS_ASSERT(joint_state_parallel.getType() ==
76			XmlRpc::XmlRpcValue::TypeStruct);
77			std::cout << "Type of joint_state_parallel:"
78			<< joint_state_parallel.getType() << std::endl;
79
80			for (XmlRpc::XmlRpcValue::iterator it = joint_state_parallel.begin();
81			it != joint_state_parallel.end(); it++) {
82			XmlRpc::XmlRpcValue local_value = it->second;
83			std::string final_expression,
84			map_expression = static_cast<string>(local_value);
85			double final_coefficient = 1.0;
86			// deal with sign
87			if (map_expression[0] == '-') {
88			final_expression = map_expression.substr(1, map_expression.size() - 1);
89			final_coefficient = -1.0;
90			} else
91			final_expression = map_expression;
92
93			std::cout << it->first.c_str() << ": " << final_coefficient << std::endl;
94			from_parallel_name_to_state_vector_name[it->first.c_str()] =
95			final_expression;
96			coefficient_parallel_joints_.push_back(final_coefficient);
97			}
98			nbOfParallelJoints_ = from_parallel_name_to_state_vector_name.size();
99			}
100
101			// Prepare joint_state according to robot description.
102			joint_state_.name.resize(nbOfJoints_ + nbOfParallelJoints_);
103			joint_state_.position.resize(nbOfJoints_ + nbOfParallelJoints_);
104
105			// Fill in the name of the joints from the state vector.
106			// and build local map from state name to state vector
107			for (int32_t i = 0; i < stateVectorMap_.size(); ++i) {
108			joint_state_.name[i] = static_cast<string>(stateVectorMap_[i]);
109
110			from_state_name_to_state_vector[joint_state_.name[i]] = i;
111			}
112
113			// Fill in the name of the joints from the parallel joint vector.
114			// and build map from parallel name to state vector
115			int i = 0;
116			parallel_joints_to_state_vector_.resize(nbOfParallelJoints_);
117			for (std::map<std::string, std::string>::iterator it =
118			from_parallel_name_to_state_vector_name.begin();
119			it != from_parallel_name_to_state_vector_name.end(); it++, i++) {
120			joint_state_.name[i + nbOfJoints_] = it->first.c_str();
121			parallel_joints_to_state_vector_[i] =
122			from_state_name_to_state_vector[it->second];
123			}
124
125			return 0;
126			}
127
128			int SotLoaderBasic::parseOptions(int argc, char* argv[]) {
129			po::options_description desc("Allowed options");
130			desc.add_options()("help", "produce help message")(
131			"input-file", po::value<string>(), "library to load");
132
133			po::store(po::parse_command_line(argc, argv, desc), vm_);
134			po::notify(vm_);
135
136			if (vm_.count("help")) {
137			cout << desc << "\n";
138			return -1;
139			}
140			if (!vm_.count("input-file")) {
141			cout << "No filename specified\n";
142			return -1;
143			} else
144			dynamicLibraryName_ = vm_["input-file"].as<string>();
145
146			return 0;
147			}
148
149			void SotLoaderBasic::Initialization() {
150			// Load the SotRobotBipedController library.
151			sotRobotControllerLibrary_ =
152			dlopen(dynamicLibraryName_.c_str(), RTLD_LAZY \| RTLD_GLOBAL);
153			if (!sotRobotControllerLibrary_) {
154			std::cerr << "Cannot load library: " << dlerror() << '\n';
155			return;
156			}
157
158			// reset errors
159			dlerror();
160
161			// Load the symbols.
162			createSotExternalInterface_t* createSot =
163			reinterpret_cast<createSotExternalInterface_t*>(reinterpret_cast<long>(
164			dlsym(sotRobotControllerLibrary_, "createSotExternalInterface")));
165			const char* dlsym_error = dlerror();
166			if (dlsym_error) {
167			std::cerr << "Cannot load symbol create: " << dlsym_error << '\n';
168			return;
169			}
170
171			// Create robot-controller
172			sotController_ = createSot();
173			cout << "Went out from Initialization." << endl;
174			}
175
176			void SotLoaderBasic::CleanUp() {
177			dynamicgraph::PoolStorage::destroy();
178			// We do not destroy the FactoryStorage singleton because the module will not
179			// be reloaded at next initialization (because Python C API cannot safely
180			// unload a module...).
181			// SignalCaster singleton could probably be destroyed.
182
183			// Load the symbols.
184			destroySotExternalInterface_t* destroySot =
185			reinterpret_cast<destroySotExternalInterface_t*>(reinterpret_cast<long>(
186			dlsym(sotRobotControllerLibrary_, "destroySotExternalInterface")));
187			const char* dlsym_error = dlerror();
188			if (dlsym_error) {
189			std::cerr << "Cannot load symbol destroy: " << dlsym_error << '\n';
190			return;
191			}
192
193			destroySot(sotController_);
194			sotController_ = NULL;
195
196			/// Uncount the number of access to this library.
197			dlclose(sotRobotControllerLibrary_);
198			}
199
200			bool SotLoaderBasic::start_dg(std_srvs::Empty::Request&,
201			std_srvs::Empty::Response&) {
202			dynamic_graph_stopped_ = false;
203			return true;
204			}
205
206			bool SotLoaderBasic::stop_dg(std_srvs::Empty::Request&,
207			std_srvs::Empty::Response&) {
208			dynamic_graph_stopped_ = true;
209			return true;
210			}