20 #include <std_msgs/Float64.h>
21 #include <std_msgs/String.h>
29 #include <string_view>
31 #include <unordered_map>
38 template <
typename RegistreeInfo,
typename TimeKeyType>
45 using ScheduleMap = std::multimap<TimeKeyType, RegistreeInfo>;
88 std::cout << ri <<
" has registered." << std::endl;
93 Scheduler(ros::NodeHandle &n,
int minRegisters,
bool autostart,
bool tcp_no_delay)
98 ros::TransportHints().tcpNoDelay(tcp_no_delay));
100 ros::TransportHints().tcpNoDelay(tcp_no_delay));
103 m_now = std::make_pair(0, 0);
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);
122 return new Scheduler(n, minRegisters, autostart, tcp_no_delay);
130 std::cout << std::endl
131 <<
"Type s to start" << std::endl;
147 std::cout <<
"scheduling started automatically" << std::endl;
160 auto upper_time_limit =
m_schedule->begin()->first;
164 if (incrementalIncrease)
166 auto d = (upper_time_limit.second -
m_now.second) / 1000;
167 while (upper_time_limit.second >
m_now.second)
173 if (
m_now < upper_time_limit)
178 m_now = upper_time_limit;
198 std::cout << (
m_pause ?
"pause at " :
"resume at ");
213 m_lastTimeSet.second = std::make_pair(ros::WallTime::now().sec, ros::WallTime::now().nsec);
215 std::cout <<
"limiting simulation speed is now " << (
m_limitSimulationSpeed ?
"activated" :
"deactivated")
228 m_lastTimeSet.second = std::make_pair(ros::WallTime::now().sec, ros::WallTime::now().nsec);
230 std_msgs::Float64 simulationTimeOutput;
231 simulationTimeOutput.data =
static_cast<double>(
m_now.first) + (
static_cast<double>(
m_now.second)) * 1e-9;
241 return minuend.first * 1e9 + minuend.second - subtrahend.first * 1e9 - subtrahend.second;
249 std::pair<uint32_t, uint32_t> newWallTime =
250 std::make_pair(ros::WallTime::now().sec, ros::WallTime::now().nsec);
252 (
static_cast<double>(
m_now.first) + (
static_cast<double>(
m_now.second)) * 1e-9 -
254 (
static_cast<double>(newWallTime.first) + (
static_cast<double>(newWallTime.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;
267 std::string clientName = msg->data;
269 RegistreeInfo
id = std::stoul(clientName, &pos, 10);
270 m_clientNames.insert(std::make_pair(
id, clientName.substr(pos + 1)));
278 std::cout <<
"\nscheduling info at ";
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;
294 std::cout <<
"scheduling started" << std::endl;
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