Duplicate timestamps in pose outputs

[willat343]

I've been testing the new ROS 2 pronto implementation with (a real) ANYmal and have been noticing that pronto is producing /pose_ext messages with duplicate timestamps (about 10 per timestamp). This occurs throughout a sequence (so not just during initialisation).The poses themselves are reasonable.

This is obviously a problem for evaluation - what was the pose of the robot at time X for comparing to the ground truth?

I checked my IMU and joint states input topics - there is no timestamp duplication there, so I confirm it occurs within pronto.

I use a very similar config file to the anymal_c_sim.yaml file provided. Besides frame and topic names, the only number i change is the timestep_dt to 0.0025 since the ANYmal IMU runs at 400Hz (although this is somewhat a guess).

[willat343]

In particular the msg->header.set__stamp(nh_->get_clock()->now()); line seems to be the culprit in the ros_frontend.hpp.

I would think that the sensor data timestamp should be preserved rather than grabbing the clock time.

I have use_sim_time true and am playing back data from a mcap with --clock. I've managed to find a workaround by setting the clock frequency very high (e.g. 1000 Hz)

[mauricefallon]

@lo-imperatore : can you please look into this issue?

I can see the calls to nh_->get_clock()->now() in the code. The code should not be doing this - it should retain the original timestamp as @willat343 said above.
https://github.com/ori-drs/pronto/blob/master/pronto_ros/include/pronto_ros/ros_frontend.hpp#L314

Perhaps @mcamurri can explain why he added these calls to the clock time?

[mcamurri]

The commit carries my name because I rebased the code from the Centro Piaggio's organization to ORI's, but it was actually authored by @Cionix90 here:
CentroEPiaggio/Pronto_Estimator@482d95d

In the original ROS1 code, the message was passed directly to the filter without changing its timestamp.

The computer clock time was used only for timing statistics:

pronto/pronto_ros/include/pronto_ros/ros_frontend.hpp

Lines 274 to 291 in e6aae29

	template <class MsgT>
	void ROSFrontEnd::callback(boost::shared_ptr<MsgT const> msg, const SensorId& sensor_id)
	{
	#if DEBUG_MODE
	ROS_INFO_STREAM("Callback for sensor " << sensor_id);
	#endif
	// this is a generic templated callback that does the same for every module:
	// if the module is initialized and the filter is ready
	// 1) take the measurement update and pass it to the filter if valid
	// 2) publish the filter state if the module wants to
	if(isFilterInitialized()) {
	// appropriate casting to the right type and call to the process message
	// function to get the update
	// Record start time
	#if DEBUG_MODE
	auto start = std::chrono::high_resolution_clock::now();
	#endif
	RBISUpdateInterface* update = static_cast<SensingModule<MsgT>*>(active_modules_[sensor_id])->processMessage(msg.get(), state_est_.get());

I agree the line should be removed.

My best guess for the reason why the line was introduced is that they were testing a custom sensors with invalid timestamps.

If that's the case, it should be the sensor driver's responsibility to compute a valid timestamp, not the ROS frontend's.

[lo-imperatore]

Hi @willat343,

Thank you for pointing out this issue. I believe it should be fixed now. Could you please pull the latest changes and let me know if everything works?

I also noticed an issue in the bias lock module, which I plan to address in the next commit.

If you find any other issues, feel free to contact me directly. I have already tested the pronto2 filters with some Grand Tour rosbags, so I’d be happy to help if you run into any integration problems with the anymal.

Sorry for the inconvenience. The project is still under active development, so there may still be some leftovers or rough edges. I am also planning to create a full example soon to make the deployment of Pronto easier.

[mauricefallon]

@lo-imperatore ,
I've reverted your recent changes (two commits) and put them on this branch instead:
https://github.com/ori-drs/pronto/tree/feat/recent-hard-commits-from-francesco

Instead, please:

• make a branch to address this issue (which is minimal and has no unintended changes)
• make a pull request - with an explanation of what the purpose of the PR is
• I (or @mcamurri) can review to changes.
  The is the branch development workflow used for shared projects like this.

Problems with your previous commit included:

• entirely unjustified changes to the QoS settings !!!!!!
• the code still seems to use the wall clock time. this is not what is intended here.
• various changes to the included files.

[willat343]

@lo-imperatore Nice to hear you have run on GrandTour! We have also run on all the GrandTour sequences and have a couple follow up questions:

• is there tuned configuration files for the anymal d that you would share, or is it similar to that of anymal c in sim (the IMU rate is different for example)
• I noticed that the filter diverges when the robot sits down (it generates a constant base velocity and flies away) - is this expected? If so i will be sure to evaluate only when the robot is standing

[lo-imperatore]

@willat343 I used the anymal c sim parameters, except for the imu rate. I would also suggest setting the leg-odometry mode to 4, but all the other values should be fine.

When there are no torques applied to the legs, the filter relies only on imu propagation. So yes, I would suggest evaluating it only when the robot is standing.