Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/catkin/dg_control/dynamic_graph_manager/src/device.cpp	Lines:	104	126	82.5 %
Date:	2020-04-15 11:50:02	Branches:	125	280	44.6 %


/**
 * @file device.cpp
 * @author Maximilien Naveau (maximilien.naveau@gmail.com)
 * @license License BSD-3-Clause
 * @copyright Copyright (c) 2019, New York University and Max Planck Gesellschaft.
 * @date 2019-05-22
 */

#include <iostream>

#include "yaml_cpp_catkin/yaml_eigen.h"

#include <dynamic-graph/factory.h>
#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/linear-algebra.h>
#include <dynamic-graph/debug.h>

#include <dynamic_graph_manager/device.hh>

using namespace std;
using namespace dynamic_graph;

/**
 * @brief str2int used in the switch case, it allows to convert in compile time
 * strings to int
 * @param str is the string to convert
 * @param h is the index of the string to start with
 * @return an integer that correspond to the string
 */
constexpr unsigned int str2int(const char* str, int h = 0)
{
  return !str[h] ?
        5381 : (str2int(str, h+1) * 33) ^ static_cast<unsigned int>(str[h]);
}

/**
 * @brief dynamic_graph::Device::CLASS_NAME must be the name as the actual
 * device class.
 */
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Device, "Device");

Device::Device(const std::string& input_name):
  // Call the mother class constructor
  Entity(input_name)
{
  /*******************************************
  * Initilize the command upon construction. *
  ********************************************/
  // I am not sure why bu this as to take place in the constructor

  // Handle commands and signals called in a synchronous way
  periodic_call_before_.addSpecificCommands(*this, commandMap, "before.");
  periodic_call_after_.addSpecificCommands(*this, commandMap, "after.");

  // Add a initialize command to parametrize the device from a yaml file
  addCommand (
        "initialize",
        dynamicgraph::command::makeCommandVoid1(
          *this,
          &Device::initialize_from_file,
          dynamicgraph::command::docCommandVoid1(
            "Initialize the device from a YAML file.",
            "string (valid path to the yaml configuration file)")));

  addCommand (
        "executeGraph",
        dynamicgraph::command::makeCommandVoid0(
          *this,
          &Device::execute_graph_deprecated,
          dynamicgraph::command::docCommandVoid0(
            "Deprecated Execute the current control graph using the currentsensor signals.")));

  addCommand (
        "execute_graph",
        dynamicgraph::command::makeCommandVoid0(
          *this,
          &Device::execute_graph,
          dynamicgraph::command::docCommandVoid0(
            "Execute the current control graph using the currentsensor signals.")));
}

void Device::initialize_from_file(const std::string& yaml_file)
{
  YAML::Node params = YAML::LoadFile(yaml_file);
  initialize(params["device"]);
}

void Device::initialize(const YAML::Node& params)
{
  params_ = params;
  /**********************************************
   * create maps for signals and vector<double> *
   **********************************************/
  initialize_maps(params_);

  /******************************
   * registering sensor signals *
   ******************************/
  for (DeviceOutSignalMap::iterator sig_it = sensors_out_.begin();
       sig_it != sensors_out_.end(); ++sig_it)
  {
    signalRegistration(*(sig_it->second));
  }

  /*******************************
   * registering control signals *
   *******************************/
  for (DeviceInSignalMap::iterator sig_it = motor_controls_in_.begin();
       sig_it != motor_controls_in_.end(); ++sig_it)
  {
    signalRegistration(*(sig_it->second));
  }
}

Device::~Device()
{
  // destruct the sensor signals
  for(DeviceOutSignalMap::iterator it = sensors_out_.begin() ;
      it != sensors_out_.end() ; ++it)
  {
    if(it->second != nullptr)
    {
      delete it->second;
      it->second = nullptr;
    }
  }
  // destruct the motor controls signals
  for(DeviceInSignalMap::iterator it = motor_controls_in_.begin() ;
      it != motor_controls_in_.end() ; ++it)
  {
    if(it->second != nullptr)
    {
      delete it->second;
      it->second = nullptr;
    }
  }

  this->entityDeregistration();
}

void Device::initialize_maps(const YAML::Node& sensors_and_controls)
{
  /**************************************************************
   * Parsing the YAML node and fill in the Eigen::VectosXd maps *
   **************************************************************/
  parse_yaml_node(sensors_and_controls, sensors_map_, motor_controls_map_);

  std::string hardware_name ("");

  /*******************************
   * We iterate over the sensors *
   *******************************/
  sensors_out_.clear();
  for (VectorDGMap::const_iterator sensor_it = sensors_map_.begin();
       sensor_it != sensors_map_.end(); ++sensor_it)
  {
    {
      hardware_name = sensor_it->first;
      ostringstream sig_name;
      sig_name << "Device(" << this->name << ")::"
               << "output(vector" << sensor_it->second.size() << "d)::"
               << hardware_name ;
      sensors_out_[hardware_name] = new OutSignal(sig_name.str());
      sensors_out_[hardware_name]->setConstant(sensors_map_[hardware_name]);
    }
  }

  /********************************
   * We iterate over the controls *
   ********************************/
  for (VectorDGMap::const_iterator control_it = motor_controls_map_.begin();
       control_it != motor_controls_map_.end(); ++control_it)
  {
    {
      hardware_name = control_it->first;
      ostringstream sig_name;
      sig_name << "Device(" << this->name << ")::"
               << "input(vector" << control_it->second.size() << "d)::"
               << hardware_name ;
      motor_controls_in_[hardware_name] =
          new InSignal(nullptr, sig_name.str());
      motor_controls_in_[hardware_name]->setConstant(
            motor_controls_map_[hardware_name]);
    }
  }
}

void Device::set_sensors_from_map(const VectorDGMap& sensors)
{
  // check that all map has the same size
  assert(sensors.size() == sensors_map_.size() &&
         sensors_map_.size() == sensors_out_.size() &&
         "Device::set_sensors_from_map: All maps has to be the same size");
  // we do a copy of the map while checking for sanity
  for(VectorDGMap::const_iterator ext_sensor_it = sensors.begin();
      ext_sensor_it != sensors.end(); ++ext_sensor_it)
  {
    assert(sensors_map_.count(ext_sensor_it->first) &&
           "Device::set_sensors_from_map: All field in the input sensors map"
            " exists in the internal copy");
    assert(static_cast<unsigned>(ext_sensor_it->second.size()) ==
           sensors_map_[ext_sensor_it->first].size() &&
           "Device::set_sensors_from_map: the vectors have the same size in the"
            " maps");
    sensors_map_[ext_sensor_it->first] = ext_sensor_it->second;
    sensors_out_[ext_sensor_it->first]->setConstant(ext_sensor_it->second);
  }
}

void Device::execute_graph()
{
  /*******************************************
   * Get the time of the last sensor reading *
   *******************************************/
  assert(sensors_out_.size() != 0 && "There exist some sensors.");
  // Here the time is the maximum time of all sensors.
  int local_time = sensors_out_.begin()->second->getTime();
  for(DeviceOutSignalMap::const_iterator sig_out_it = sensors_out_.begin() ;
      sig_out_it != sensors_out_.end() ; ++sig_out_it)
  {
    int sig_time = sig_out_it->second->getTime();
    if(sig_time > local_time)
    {
      local_time = sig_time;
    }
  }

  /******************************************************************
   * Run Synchronous commands and evaluate signals outside the main *
   * connected component of the graph.                              *
   ******************************************************************/
  try
  {
    periodic_call_before_.run(local_time+1);
  }
  catch (std::exception& e)
  {
    std::cerr
        << "exception caught while running periodical commands (before): "
        << e.what () << std::endl;
  }
  catch (const char* str)
  {
    std::cerr
        << "exception caught while running periodical commands (before): "
        << str << std::endl;
  }
  catch (...)
  {
    std::cerr
        << "unknown exception caught while"
        << " running periodical commands (before)" << std::endl;
  }

  /***********************************************************************
   * Run the graph by accessing the values of the signals inside the map *
   ***********************************************************************/
  for(DeviceInSignalMap::const_iterator sig_in_it = motor_controls_in_.begin() ;
      sig_in_it != motor_controls_in_.end() ; ++sig_in_it)
  {
    sig_in_it->second->recompute(local_time+1);
  }

  /******************************************************************
   * Run Synchronous commands and evaluate signals outside the main *
   * connected component of the graph.                              *
   ******************************************************************/
  try
  {
    // TODO: "time" or "time + 1"
    periodic_call_after_.run(local_time+1);
  }
  catch (std::exception& e)
  {
    std::cerr
        << "exception caught while running periodical commands (after): "
        << e.what () << std::endl;
  }
  catch (const char* str)
  {
    std::cerr
        << "exception caught while running periodical commands (after): "
        << str << std::endl;
  }
  catch (...)
  {
    std::cerr
        << "unknown exception caught while"
        << " running periodical commands (after)" << std::endl;
  }
}

void Device::get_controls_to_map(VectorDGMap& motor_controls)
{
  // check that all map has the same size
  assert(motor_controls.size() == motor_controls_map_.size() &&
         motor_controls_map_.size() == motor_controls_in_.size() &&
         "Device::get_controls_to_map: All maps has to be the same size");
  // we do a copy of the map while checking for sanity
  for(VectorDGMap::iterator ext_control_it = motor_controls.begin();
      ext_control_it != motor_controls.end(); ++ext_control_it)
  {
    assert(motor_controls_map_.count(ext_control_it->first) &&
           "Device::get_controls_to_map: All field in the input sensors map "
            "exists in the internal copy");
    assert(static_cast<unsigned>(ext_control_it->second.size()) ==
           motor_controls_map_[ext_control_it->first].size() &&
           "Device::get_controls_to_map: the vectors have the same size in the "
           "maps");

   motor_controls_map_[ext_control_it->first] =
        motor_controls_in_[ext_control_it->first]->accessCopy();
   ext_control_it->second = motor_controls_map_[ext_control_it->first];
  }
}


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			/**
2			* @file device.cpp
3			* @author Maximilien Naveau (maximilien.naveau@gmail.com)
4			* @license License BSD-3-Clause
5			* @copyright Copyright (c) 2019, New York University and Max Planck Gesellschaft.
6			* @date 2019-05-22
7			*/
8
9			#include <iostream>
10
11			#include "yaml_cpp_catkin/yaml_eigen.h"
12
13			#include <dynamic-graph/factory.h>
14			#include <dynamic-graph/all-commands.h>
15			#include <dynamic-graph/linear-algebra.h>
16			#include <dynamic-graph/debug.h>
17
18			#include <dynamic_graph_manager/device.hh>
19
20			using namespace std;
21			using namespace dynamic_graph;
22
23			/**
24			* @brief str2int used in the switch case, it allows to convert in compile time
25			* strings to int
26			* @param str is the string to convert
27			* @param h is the index of the string to start with
28			* @return an integer that correspond to the string
29			*/
30			constexpr unsigned int str2int(const char* str, int h = 0)
31			{
32			return !str[h] ?
33			5381 : (str2int(str, h+1) * 33) ^ static_cast<unsigned int>(str[h]);
34			}
35
36			/**
37			* @brief dynamic_graph::Device::CLASS_NAME must be the name as the actual
38			* device class.
39			*/
40	✓✗✓✗ ✓✗✗✗	4	DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Device, "Device");
41
42		20	Device::Device(const std::string& input_name):
43			// Call the mother class constructor
44	✓✗✓✗	20	Entity(input_name)
45			{
46			/*******************************************
47			* Initilize the command upon construction. *
48			********************************************/
49			// I am not sure why bu this as to take place in the constructor
50
51			// Handle commands and signals called in a synchronous way
52	✓✗✓✗	20	periodic_call_before_.addSpecificCommands(*this, commandMap, "before.");
53	✓✗✓✗	20	periodic_call_after_.addSpecificCommands(*this, commandMap, "after.");
54
55			// Add a initialize command to parametrize the device from a yaml file
56	✓✗	60	addCommand (
57			"initialize",
58	✓✗	20	dynamicgraph::command::makeCommandVoid1(
59			*this,
60			&Device::initialize_from_file,
61	✓✗✓✗ ✓✗	40	dynamicgraph::command::docCommandVoid1(
62			"Initialize the device from a YAML file.",
63	✓✗	20	"string (valid path to the yaml configuration file)")));
64
65	✓✗	60	addCommand (
66			"executeGraph",
67	✓✗	20	dynamicgraph::command::makeCommandVoid0(
68			*this,
69			&Device::execute_graph_deprecated,
70	✓✗✓✗	40	dynamicgraph::command::docCommandVoid0(
71	✓✗	20	"Deprecated Execute the current control graph using the currentsensor signals.")));
72
73	✓✗	60	addCommand (
74			"execute_graph",
75	✓✗	20	dynamicgraph::command::makeCommandVoid0(
76			*this,
77			&Device::execute_graph,
78	✓✗✓✗	40	dynamicgraph::command::docCommandVoid0(
79	✓✗	20	"Execute the current control graph using the currentsensor signals.")));
80		20	}
81
82			void Device::initialize_from_file(const std::string& yaml_file)
83			{
84			YAML::Node params = YAML::LoadFile(yaml_file);
85			initialize(params["device"]);
86			}
87
88		18	void Device::initialize(const YAML::Node& params)
89			{
90		18	params_ = params;
91			/**********************************************
92			* create maps for signals and vector<double> *
93			**********************************************/
94		18	initialize_maps(params_);
95
96			/******************************
97			* registering sensor signals *
98			******************************/
99	✓✓	222	for (DeviceOutSignalMap::iterator sig_it = sensors_out_.begin();
100		148	sig_it != sensors_out_.end(); ++sig_it)
101			{
102	✓✗✓✗	56	signalRegistration(*(sig_it->second));
103			}
104
105			/*******************************
106			* registering control signals *
107			*******************************/
108	✓✓	138	for (DeviceInSignalMap::iterator sig_it = motor_controls_in_.begin();
109		92	sig_it != motor_controls_in_.end(); ++sig_it)
110			{
111	✓✗✓✗	28	signalRegistration(*(sig_it->second));
112			}
113		18	}
114
115		53	Device::~Device()
116			{
117			// destruct the sensor signals
118	✓✓	264	for(DeviceOutSignalMap::iterator it = sensors_out_.begin() ;
119		176	it != sensors_out_.end() ; ++it)
120			{
121	✓✗	68	if(it->second != nullptr)
122			{
123	✓✗	68	delete it->second;
124		68	it->second = nullptr;
125			}
126			}
127			// destruct the motor controls signals
128	✓✓	162	for(DeviceInSignalMap::iterator it = motor_controls_in_.begin() ;
129		108	it != motor_controls_in_.end() ; ++it)
130			{
131	✓✗	34	if(it->second != nullptr)
132			{
133	✓✗	34	delete it->second;
134		34	it->second = nullptr;
135			}
136			}
137
138		20	this->entityDeregistration();
139	✗✓	33	}
140
141		21	void Device::initialize_maps(const YAML::Node& sensors_and_controls)
142			{
143			/**************************************************************
144			* Parsing the YAML node and fill in the Eigen::VectosXd maps *
145			**************************************************************/
146	✓✗	21	parse_yaml_node(sensors_and_controls, sensors_map_, motor_controls_map_);
147
148	✓✗	42	std::string hardware_name ("");
149
150			/*******************************
151			* We iterate over the sensors *
152			*******************************/
153		21	sensors_out_.clear();
154	✓✓	267	for (VectorDGMap::const_iterator sensor_it = sensors_map_.begin();
155		178	sensor_it != sensors_map_.end(); ++sensor_it)
156			{
157			{
158	✓✗	68	hardware_name = sensor_it->first;
159	✓✗	136	ostringstream sig_name;
160	✓✗✓✗ ✓✗	68	sig_name << "Device(" << this->name << ")::"
161	✓✗✓✗ ✓✗✓✗	136	<< "output(vector" << sensor_it->second.size() << "d)::"
162	✓✗	68	<< hardware_name ;
163	✓✗✓✗ ✓✗✓✗	68	sensors_out_[hardware_name] = new OutSignal(sig_name.str());
164	✓✗✓✗ ✓✗	68	sensors_out_[hardware_name]->setConstant(sensors_map_[hardware_name]);
165			}
166			}
167
168			/********************************
169			* We iterate over the controls *
170			********************************/
171	✓✓	165	for (VectorDGMap::const_iterator control_it = motor_controls_map_.begin();
172		110	control_it != motor_controls_map_.end(); ++control_it)
173			{
174			{
175	✓✗	34	hardware_name = control_it->first;
176	✓✗	68	ostringstream sig_name;
177	✓✗✓✗ ✓✗	34	sig_name << "Device(" << this->name << ")::"
178	✓✗✓✗ ✓✗✓✗	68	<< "input(vector" << control_it->second.size() << "d)::"
179	✓✗	34	<< hardware_name ;
180	✓✗	34	motor_controls_in_[hardware_name] =
181	✓✗✓✗ ✓✗	68	new InSignal(nullptr, sig_name.str());
182	✓✗	34	motor_controls_in_[hardware_name]->setConstant(
183	✓✗✓✗	34	motor_controls_map_[hardware_name]);
184			}
185			}
186		21	}
187
188		2	void Device::set_sensors_from_map(const VectorDGMap& sensors)
189			{
190			// check that all map has the same size
191	✗✓	4	assert(sensors.size() == sensors_map_.size() &&
192			sensors_map_.size() == sensors_out_.size() &&
193	✓✗	2	"Device::set_sensors_from_map: All maps has to be the same size");
194			// we do a copy of the map while checking for sanity
195	✓✓	30	for(VectorDGMap::const_iterator ext_sensor_it = sensors.begin();
196		20	ext_sensor_it != sensors.end(); ++ext_sensor_it)
197			{
198	✓✗	8	assert(sensors_map_.count(ext_sensor_it->first) &&
199			"Device::set_sensors_from_map: All field in the input sensors map"
200	✗✓	8	" exists in the internal copy");
201	✓✗✓✗ ✓✗	8	assert(static_cast<unsigned>(ext_sensor_it->second.size()) ==
202			sensors_map_[ext_sensor_it->first].size() &&
203			"Device::set_sensors_from_map: the vectors have the same size in the"
204	✗✓	8	" maps");
205	✓✗✓✗	8	sensors_map_[ext_sensor_it->first] = ext_sensor_it->second;
206	✓✗✓✗	8	sensors_out_[ext_sensor_it->first]->setConstant(ext_sensor_it->second);
207			}
208		2	}
209
210		2	void Device::execute_graph()
211			{
212			/*******************************************
213			* Get the time of the last sensor reading *
214			*******************************************/
215	✗✓	2	assert(sensors_out_.size() != 0 && "There exist some sensors.");
216			// Here the time is the maximum time of all sensors.
217	✓✗	2	int local_time = sensors_out_.begin()->second->getTime();
218	✓✓	30	for(DeviceOutSignalMap::const_iterator sig_out_it = sensors_out_.begin() ;
219		20	sig_out_it != sensors_out_.end() ; ++sig_out_it)
220			{
221	✓✗	8	int sig_time = sig_out_it->second->getTime();
222	✗✓	8	if(sig_time > local_time)
223			{
224			local_time = sig_time;
225			}
226			}
227
228			/******************************************************************
229			* Run Synchronous commands and evaluate signals outside the main *
230			* connected component of the graph. *
231			******************************************************************/
232			try
233			{
234	✓✗✗✗ ✗	2	periodic_call_before_.run(local_time+1);
235			}
236			catch (std::exception& e)
237			{
238			std::cerr
239			<< "exception caught while running periodical commands (before): "
240			<< e.what () << std::endl;
241			}
242			catch (const char* str)
243			{
244			std::cerr
245			<< "exception caught while running periodical commands (before): "
246			<< str << std::endl;
247			}
248			catch (...)
249			{
250			std::cerr
251			<< "unknown exception caught while"
252			<< " running periodical commands (before)" << std::endl;
253			}
254
255			/***********************************************************************
256			* Run the graph by accessing the values of the signals inside the map *
257			***********************************************************************/
258	✓✓	18	for(DeviceInSignalMap::const_iterator sig_in_it = motor_controls_in_.begin() ;
259		12	sig_in_it != motor_controls_in_.end() ; ++sig_in_it)
260			{
261	✓✗	4	sig_in_it->second->recompute(local_time+1);
262			}
263
264			/******************************************************************
265			* Run Synchronous commands and evaluate signals outside the main *
266			* connected component of the graph. *
267			******************************************************************/
268			try
269			{
270			// TODO: "time" or "time + 1"
271	✓✗✗✗ ✗	2	periodic_call_after_.run(local_time+1);
272			}
273			catch (std::exception& e)
274			{
275			std::cerr
276			<< "exception caught while running periodical commands (after): "
277			<< e.what () << std::endl;
278			}
279			catch (const char* str)
280			{
281			std::cerr
282			<< "exception caught while running periodical commands (after): "
283			<< str << std::endl;
284			}
285			catch (...)
286			{
287			std::cerr
288			<< "unknown exception caught while"
289			<< " running periodical commands (after)" << std::endl;
290			}
291		2	}
292
293		2	void Device::get_controls_to_map(VectorDGMap& motor_controls)
294			{
295			// check that all map has the same size
296	✗✓	4	assert(motor_controls.size() == motor_controls_map_.size() &&
297			motor_controls_map_.size() == motor_controls_in_.size() &&
298	✓✗	2	"Device::get_controls_to_map: All maps has to be the same size");
299			// we do a copy of the map while checking for sanity
300	✓✓	18	for(VectorDGMap::iterator ext_control_it = motor_controls.begin();
301		12	ext_control_it != motor_controls.end(); ++ext_control_it)
302			{
303	✓✗	4	assert(motor_controls_map_.count(ext_control_it->first) &&
304			"Device::get_controls_to_map: All field in the input sensors map "
305	✗✓	4	"exists in the internal copy");
306	✓✗✓✗ ✓✗	4	assert(static_cast<unsigned>(ext_control_it->second.size()) ==
307			motor_controls_map_[ext_control_it->first].size() &&
308			"Device::get_controls_to_map: the vectors have the same size in the "
309	✗✓	4	"maps");
310
311	✓✗	8	motor_controls_map_[ext_control_it->first] =
312	✓✗✓✗ ✓✗	12	motor_controls_in_[ext_control_it->first]->accessCopy();
313	✓✗✓✗	4	ext_control_it->second = motor_controls_map_[ext_control_it->first];
314			}
315	✓✗✓✗	14	}