ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
adore_scheduler.h
Go to the documentation of this file.
1 /********************************************************************************
2  * Copyright (C) 2017-2023 German Aerospace Center (DLR).
3  * Eclipse ADORe, Automated Driving Open Research https://eclipse.org/adore
4  *
5  * This program and the accompanying materials are made available under the
6  * terms of the Eclipse Public License 2.0 which is available at
7  * http://www.eclipse.org/legal/epl-2.0.
8  *
9  * SPDX-License-Identifier: EPL-2.0
10  *
11  * Contributors:
12  * Matthias Nichting - initial API and implementation
13  * Thomas Lobig - initial API and implementation
14  ********************************************************************************/
15 
19 #include <ros/ros.h>
20 #include <std_msgs/Float64.h>
21 #include <std_msgs/String.h>
22 
23 #include <chrono>
24 #include <functional>
25 #include <iostream>
26 #include <map>
27 #include <memory>
28 #include <string>
29 #include <string_view>
30 #include <thread>
31 #include <unordered_map>
32 #include <utility>
33 #include <vector>
34 
36 {
37 
38  template <typename RegistreeInfo, typename TimeKeyType>
43  class Scheduler
44  {
45  using ScheduleMap = std::multimap<TimeKeyType, RegistreeInfo>;
46 
47  private:
48  ros::Subscriber m_notificationReader;
49  ros::Subscriber m_clientNameReader;
50  ros::Publisher m_simulationTimeWriter;
51  ros::Publisher m_clockTimeWriter;
54  bool m_pause; // true if scheduling is paused
55  int m_minRegisters; // number of registers that is needed to automatically start the scheduling
56  bool m_autostart; // true if scheduling should start without keyboard input
57  bool m_started; // ture if scheduling has started
58  int m_printIntervalS; // interval of console output in seconds (measured in simulation time)
59  bool m_limitSimulationSpeed; // true if simulation speed is limited
60  std::pair<uint32_t, uint32_t> m_lastWallTime;
61  std::pair<uint32_t, uint32_t> m_lastRosTime;
63  TimeKeyType m_now;
64  std::pair<TimeKeyType, std::pair<uint32_t, uint32_t>> m_lastTimeSet;
65  std::unordered_map<RegistreeInfo, std::string> m_clientNames;
66 
75  void updateSchedule(RegistreeInfo ri, TimeKeyType tk)
76  {
77  for (auto it = m_schedule->begin(); it != m_schedule->end(); ++it)
78  {
79  if (it->second == ri)
80  {
81  auto nh = m_schedule->extract(it);
82  nh.key() = tk;
83  m_schedule->insert(std::move(nh));
84  return;
85  }
86  }
87  m_schedule->insert(std::make_pair(tk, ri));
88  std::cout << ri << " has registered." << std::endl;
89  write();
91  }
92 
93  Scheduler(ros::NodeHandle &n, int minRegisters, bool autostart, bool tcp_no_delay)
94  {
95  m_schedule = new ScheduleMap();
98  ros::TransportHints().tcpNoDelay(tcp_no_delay));
100  ros::TransportHints().tcpNoDelay(tcp_no_delay));
102  m_clockTimeWriter = n.advertise<rosgraph_msgs::Clock>(adore_if_ros_scheduling::TOPIC_NAME_CLOCK_TIME, 1);
103  m_now = std::make_pair(0, 0);
104  m_lastTimeSet.first = std::make_pair(0, 0);
105  m_lastTimeSet.second = std::make_pair(0, 0);
106  m_pause = false;
107  m_autostart = autostart;
108  m_started = false;
109  m_minRegisters = minRegisters;
110  m_lastWallTime = std::make_pair(ros::WallTime::now().sec, ros::WallTime::now().nsec);
111  m_lastRosTime = std::make_pair(ros::Time::now().sec, ros::Time::now().nsec);
112  m_limitSimulationSpeed = false;
113  m_printIntervalS = 10;
114  }
115 
116  public:
120  static Scheduler *getInstance(ros::NodeHandle &n, int minRegisters, bool autostart, bool tcp_no_delay)
121  {
122  return new Scheduler(n, minRegisters, autostart, tcp_no_delay);
123  }
124 
128  void init()
129  {
130  std::cout << std::endl
131  << "Type s to start" << std::endl;
132  }
133 
137  void updateClientUpperTimeLimit(const adore_if_ros_scheduling_msg::SchedulerNotification::ConstPtr &msg)
138  {
140  updateSchedule(clientNotification.getID(), clientNotification.getUpperTimeLimitPair());
141  if (!m_pause && m_started)
142  {
143  setNewTime();
144  }
145  else if (!m_pause && m_minRegisters < 1 && m_autostart)
146  {
147  std::cout << "scheduling started automatically" << std::endl;
148  m_started = true;
149  setNewTime();
150  }
151  }
152 
156  inline void setNewTime(bool incrementalIncrease = false)
157  {
158  if (!m_schedule->empty())
159  {
160  auto upper_time_limit = m_schedule->begin()->first;
164  if (incrementalIncrease)
165  {
166  auto d = (upper_time_limit.second - m_now.second) / 1000;
167  while (upper_time_limit.second > m_now.second)
168  {
169  m_now.second += d;
170  write();
171  }
172  }
173  if (m_now < upper_time_limit)
174  {
175  bool print = false;
176  if (upper_time_limit.first > m_now.first && upper_time_limit.first % m_printIntervalS == 0)
177  print = true;
178  m_now = upper_time_limit;
179  if (print)
180  printTime();
181  write();
182  }
183  }
184  else
185  {
186  m_now.first = 0;
187  m_now.second = 0;
188  write();
189  }
190  }
191 
195  void togglePause()
196  {
197  m_pause = !m_pause;
198  std::cout << (m_pause ? "pause at " : "resume at ");
199  printTime();
200  if (!m_pause)
201  setNewTime();
202  }
203 
208  {
211  {
212  m_lastTimeSet.first = m_now;
213  m_lastTimeSet.second = std::make_pair(ros::WallTime::now().sec, ros::WallTime::now().nsec);
214  }
215  std::cout << "limiting simulation speed is now " << (m_limitSimulationSpeed ? "activated" : "deactivated")
216  << std::endl;
217  }
218 
222  inline void write()
223  {
225  {
226  std::this_thread::sleep_for(std::chrono::nanoseconds(getTimeDiff(m_lastTimeSet.first, m_now)));
227  m_lastTimeSet.first = m_now;
228  m_lastTimeSet.second = std::make_pair(ros::WallTime::now().sec, ros::WallTime::now().nsec);
229  }
230  std_msgs::Float64 simulationTimeOutput;
231  simulationTimeOutput.data = static_cast<double>(m_now.first) + (static_cast<double>(m_now.second)) * 1e-9;
232  m_simulationTimeWriter.publish(simulationTimeOutput);
234  }
235 
239  uint32_t getTimeDiff(TimeKeyType subtrahend, TimeKeyType minuend)
240  {
241  return minuend.first * 1e9 + minuend.second - subtrahend.first * 1e9 - subtrahend.second;
242  }
243 
247  void printTime()
248  {
249  std::pair<uint32_t, uint32_t> newWallTime =
250  std::make_pair(ros::WallTime::now().sec, ros::WallTime::now().nsec);
251  double speedfactor =
252  (static_cast<double>(m_now.first) + (static_cast<double>(m_now.second)) * 1e-9 -
253  static_cast<double>(m_lastRosTime.first) - (static_cast<double>(m_lastRosTime.second)) * 1e-9) /
254  (static_cast<double>(newWallTime.first) + (static_cast<double>(newWallTime.second)) * 1e-9 -
255  static_cast<double>(m_lastWallTime.first) - (static_cast<double>(m_lastWallTime.second)) * 1e-9);
256  std::cout << "time = " << m_now.first << "." << std::setfill('0') << std::setw(9) << m_now.second
257  << "; rel. simulation speed = " << speedfactor << std::endl;
258  m_lastWallTime = newWallTime;
260  }
261 
265  void saveClientName(const std_msgs::String::ConstPtr &msg)
266  {
267  std::string clientName = msg->data;
268  size_t pos;
269  RegistreeInfo id = std::stoul(clientName, &pos, 10);
270  m_clientNames.insert(std::make_pair(id, clientName.substr(pos + 1)));
271  }
272 
276  void printInfo()
277  {
278  std::cout << "\nscheduling info at ";
279  printTime();
280  for (auto i = m_schedule->begin(); i != m_schedule->end(); ++i)
281  {
282  auto clientname_it = m_clientNames.find(i->second);
283  std::string clientname = clientname_it != m_clientNames.end() ? " : " + clientname_it->second : "";
284  std::cout << " max time for id " << i->second << " is " << i->first.first << "." << std::setfill('0')
285  << std::setw(9) << i->first.second << clientname << std::endl;
286  }
287  }
288 
292  void start()
293  {
294  std::cout << "scheduling started" << std::endl;
295  m_started = true;
296  setNewTime(true);
297  }
298  };
299 } // namespace adore_if_ros_scheduling
Definition: clocktimeconversion.h:27
Definition: schedulernotificationconversion.h:25
Scheduler is a class which provides functionality for stepped simulation.
Definition: adore_scheduler.h:44
bool m_pause
Definition: adore_scheduler.h:54
std::pair< TimeKeyType, std::pair< uint32_t, uint32_t > > m_lastTimeSet
Definition: adore_scheduler.h:64
void updateSchedule(RegistreeInfo ri, TimeKeyType tk)
Definition: adore_scheduler.h:75
void printTime()
Definition: adore_scheduler.h:247
ros::Subscriber m_notificationReader
Definition: adore_scheduler.h:48
ros::Publisher m_simulationTimeWriter
Definition: adore_scheduler.h:50
bool m_autostart
Definition: adore_scheduler.h:56
SchedulerNotificationConversion m_schedulerNotificationConversion
Definition: adore_scheduler.h:52
void start()
Definition: adore_scheduler.h:292
TimeKeyType m_now
Definition: adore_scheduler.h:63
ClockTimeConversion m_clockTimeConversion
Definition: adore_scheduler.h:53
bool m_limitSimulationSpeed
Definition: adore_scheduler.h:59
Scheduler(ros::NodeHandle &n, int minRegisters, bool autostart, bool tcp_no_delay)
Definition: adore_scheduler.h:93
void printInfo()
Definition: adore_scheduler.h:276
void init()
Definition: adore_scheduler.h:128
std::multimap< TimeKeyType, RegistreeInfo > ScheduleMap
Definition: adore_scheduler.h:45
int m_printIntervalS
Definition: adore_scheduler.h:58
std::unordered_map< RegistreeInfo, std::string > m_clientNames
Definition: adore_scheduler.h:65
void saveClientName(const std_msgs::String::ConstPtr &msg)
Definition: adore_scheduler.h:265
ros::Subscriber m_clientNameReader
Definition: adore_scheduler.h:49
void limitSimulationSpeed()
Definition: adore_scheduler.h:207
static Scheduler * getInstance(ros::NodeHandle &n, int minRegisters, bool autostart, bool tcp_no_delay)
Definition: adore_scheduler.h:120
int m_minRegisters
Definition: adore_scheduler.h:55
void updateClientUpperTimeLimit(const adore_if_ros_scheduling_msg::SchedulerNotification::ConstPtr &msg)
Definition: adore_scheduler.h:137
void setNewTime(bool incrementalIncrease=false)
Definition: adore_scheduler.h:156
std::pair< uint32_t, uint32_t > m_lastRosTime
Definition: adore_scheduler.h:61
void togglePause()
Definition: adore_scheduler.h:195
std::pair< uint32_t, uint32_t > m_lastWallTime
Definition: adore_scheduler.h:60
bool m_started
Definition: adore_scheduler.h:57
uint32_t getTimeDiff(TimeKeyType subtrahend, TimeKeyType minuend)
Definition: adore_scheduler.h:239
void write()
Definition: adore_scheduler.h:222
ScheduleMap * m_schedule
Definition: adore_scheduler.h:62
ros::Publisher m_clockTimeWriter
Definition: adore_scheduler.h:51
Definition: schedulernotification.h:26
std::pair< uint32_t, uint32_t > getUpperTimeLimitPair() const
Definition: schedulernotification.h:44
unsigned int getID() const
Definition: schedulernotification.h:64
Definition: adore_if_ros_scheduling_constants.h:19
const char TOPIC_NAME_CLIENT_NAME[]
Definition: adore_if_ros_scheduling_constants.h:22
const char TOPIC_NAME_SIMULATION_TIME[]
Definition: adore_if_ros_scheduling_constants.h:20
const char TOPIC_NAME_CLOCK_TIME[]
Definition: adore_if_ros_scheduling_constants.h:21
const char TOPIC_NAME_SCHEDULER_NOTIFICATION[]
Definition: adore_if_ros_scheduling_constants.h:23