Time stamp on simulated lidar

[olofmat]

In src/svea_core/src/svea/simulators/sim_lidar.py rospy.Time.now() is only called once in initialization. For me this lead to issues with tf transforms with the /scan topic (as all messages had the same, and wrong, timestamp).

I added self._scan_msg.header.stamp = rospy.Time.now() to the publish_scan function which made it work for me. Not sure if that is a good fix or not.

@kaarmu and @frankjjiang is that a good fix or will that affect other parts of the script?

[frankjjiang]

@olofmat nice find! Could you open a PR with this change?

Also, could you update the time here:

svea/src/svea_core/src/svea/simulators/sim_lidar.py

Line 209 in 06c1145

self._scan_msg.ranges = self.ranges

instead of publish_scan?

[rcywongaa]

Somewhat related, it might also be a good idea to publish the sim time from sim_SVEA.py to facilitate tf lookups

e.g.

                self.clock_pub.publish(self.current_state.pose_msg.header.stamp)

after https://github.com/KTH-SML/svea/blob/main/src/svea_core/src/svea/simulators/sim_SVEA.py#L228

or to have the tf messages use current time in
https://github.com/KTH-SML/svea/blob/main/src/svea_core/src/svea/simulators/sim_SVEA.py#L233

[frankjjiang]

Somewhat related, it might also be a good idea to publish the sim time from sim_SVEA.py to facilitate tf lookups

e.g.

                self.clock_pub.publish(self.current_state.pose_msg.header.stamp)

after https://github.com/KTH-SML/svea/blob/main/src/svea_core/src/svea/simulators/sim_SVEA.py#L228

or to have the tf messages use current time in https://github.com/KTH-SML/svea/blob/main/src/svea_core/src/svea/simulators/sim_SVEA.py#L233

Both options sound good to me. Could you and @olofmat do a joint PR for this?

[rcywongaa]

I realized that my first approach isn't compatible with the simulated lidar fix.

                self.clock_pub.publish(self.current_state.pose_msg.header.stamp)

since sim_lidar would be using rospy.Time.now() while sim_SVEA will be using self.current_state.pose_msg.header.stamp which means the times will not be synchronized. (sim_lidar will not get the time published by sim_SVEA since the whole sim stack is not launched with /use_sim_time)

The only way I to get both of these to work together is to use the second approach where both the laser time and the transform time uses rospy.Time.now()