generic_updater.cpp
Go to the documentation of this file.
1 
29 #include <boost/foreach.hpp>
30 #include <iostream>
31 #include <vector>
32 
33 namespace generic_updater
34 {
35  template<class CommandType>
36  GenericUpdater<CommandType>::GenericUpdater(std::vector<UpdateConfig> update_configs_vector,
38  : nh_tilde("~"), which_data_to_request(0), update_state(update_state), update_configs_vector(
39  update_configs_vector)
40  {
41  mutex = boost::shared_ptr<boost::mutex>(new boost::mutex());
42 
43  BOOST_FOREACH(UpdateConfig config, update_configs_vector)
44  {
45  if (config.when_to_update == -2.0)
46  {
47  initialization_configs_vector.push_back(config);
48  }
49  else if (config.when_to_update != -1.0)
50  {
51  double tmp_dur = config.when_to_update;
52  ros::Duration duration(tmp_dur);
53 
54  timers.push_back(
56  duration,
57  boost::bind(&GenericUpdater::timer_callback, this, _1, config.what_to_update)));
58  }
59  else
60  {
61  important_update_configs_vector.push_back(config);
62  }
63  }
64  ROS_DEBUG_STREAM("Init config size" << initialization_configs_vector.size());
65  ROS_DEBUG_STREAM("Important config size" << important_update_configs_vector.size());
66 
67  // If there isn't any command defined as initializingcommand (-2), state switches to operation
68  if (initialization_configs_vector.size() == 0)
69  {
70  ROS_INFO_STREAM("No init command. Switching to operation");
72  }
73  }
74 
75  template<class CommandType>
77  {
79  {
80  boost::mutex::scoped_lock l(*mutex);
81  unimportant_data_queue.push(data_type);
82 
83  ROS_DEBUG_STREAM("Timer: data type = " << data_type << " | queue size: " << unimportant_data_queue.size());
84  }
85  }
86 
87  // Only to ensure that the template class is compiled for the types we are interested in
88  template
90 
91  template
93 
94  template
96 
97  template
99 } // end namespace generic_updater
100 
101 /* For the emacs weenies in the crowd.
102  Local Variables:
103  c-basic-offset: 2
104  End:
105 */
std::queue< int32u, std::list< int32u > > unimportant_data_queue
operation_mode::device_update_state::DeviceUpdateState update_state
boost::shared_ptr< boost::mutex > mutex
std::vector< ros::Timer > timers
void timer_callback(const ros::TimerEvent &event, int32u data_type)
unsigned int int32u
std::vector< UpdateConfig > initialization_configs_vector
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
#define ROS_DEBUG_STREAM(args)
std::vector< UpdateConfig > important_update_configs_vector
Contains all the important data types.
#define ROS_INFO_STREAM(args)
GenericUpdater(std::vector< UpdateConfig > update_configs_vector, operation_mode::device_update_state::DeviceUpdateState update_state)


sr_robot_lib
Author(s): Ugo Cupcic, Toni Oliver
autogenerated on Tue Oct 13 2020 04:01:57