Skip to content

Instantly share code, notes, and snippets.

@srathi-monarch
Last active August 1, 2023 06:56
Show Gist options
  • Save srathi-monarch/714de09152c93dd14dbde6f0d6ed2981 to your computer and use it in GitHub Desktop.
Save srathi-monarch/714de09152c93dd14dbde6f0d6ed2981 to your computer and use it in GitHub Desktop.
(Unnecessarily complicated?) fake clock publisher for ROS
#include <ros/ros.h>
#include <rosgraph_msgs/Clock.h>
template <typename T> auto to_ros(const T &t) {
auto dur = std::chrono::duration_cast<std::chrono::nanoseconds>(t);
return std::make_pair(int32_t(std::chrono::duration_cast<std::chrono::seconds>(dur).count()),
int32_t(dur.count() % 1000000000UL));
}
int main(int argc, char **argv) {
ros::init(argc, argv, "fake_clock");
ros::NodeHandle nh, pnh("~");
if (!nh.param("/use_sim_time", false)) {
ROS_FATAL("Param: \"/use_sim_time\" unset or set to false. Exiting");
return 1;
}
auto num = pnh.param("slowdown/num", 1);
auto den = pnh.param("slowdown/den", 1);
ROS_ASSERT(num >= 1 && den >= 1);
auto pub = nh.advertise<rosgraph_msgs::Clock>("/clock", 10);
using ns = std::chrono::nanoseconds;
ns t(0), carry(0);
const ns dt((long long)1e5), num_ns(num);
ROS_INFO("Starting clock with a slowdown of %i / %i.", num, den);
rosgraph_msgs::Clock ck;
const auto [sec, nsec] = to_ros(dt);
ros::WallRate r(ros::Duration(sec, nsec));
while (ros::ok()) {
auto x = dt * den;
t += x / num;
carry += x % num;
if (carry >= num_ns) {
t += ns{carry / num_ns};
carry %= num;
}
const auto [ts, tns] = to_ros(t);
ck.clock.sec = ts;
ck.clock.nsec = tns;
pub.publish(ck);
if (!r.sleep()) {
ROS_ERROR_THROTTLE(1.0, "Clock did not meet required rate.");
}
}
return 0;
}
<?xml version="1.0" ?>
<launch xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
xsi:noNamespaceSchemaLocation="https://gist.githubusercontent.com/nalt/dfa2abc9d2e3ae4feb82ca5608090387/raw/roslaunch.xsd">
<param name="/use_sim_time" value="true"/>
<node pkg="fake_clock" type="fake_clock" name="fake_clock">
<param name="slowdown/num" value="5"/>
<param name="slowdown/den" value="1"/>
</node>
</launch>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment