Added pose_publish_rate which controls the rate of /fiducial_pose

[ivan140]

Hi

The rate for publishing the fiducial_pose is hard_coded to 20 Hz. This limits the possible speeds when controlling by directly acquiring the fiducials position. This feature makes it possible to adjust the publishing speed through a rosparam.

[JanezCim]

Thanks for the contribution!

I've tested this out in gazebo and it works:

roslaunch magni_gazebo fiducial_world.launch

with the paramater on 30Hz

rostopic hz /fiducial_pose
subscribed to [/fiducial_pose]
WARNING: may be using simulated time
average rate: 29.851
    min: 0.030s max: 0.038s std dev: 0.00190s window: 17

so in theory it works, but i dont know what that would mean in real world if the camera framerate was only, say 20Hz, odom 30Hz and you had this parameter on say 50Hz. This could pose some problems since you cant generate new pose info with 50Hz. But anyway I think this is better then having the rate hardcoded at 20Hz.

What do you think @jim-v @rohbotics ?

[rohbotics]

I think this is reasonable, thanks for the contribution @ivan140

[ivan140]

@JanezCim I made some measurements for you on my Raspi 4 with the default cam. When looking into the numbers, I'd say it makes sense to set the rate to at least the fps of the camera. Also, it seems not to cause any problems having a higher pose publishing rate, than the detector.

rate 20, CPU ~5%	topic	rate	min_delta	max_delta	std_dev	window
/cv_camera/image_raw/compressed	30.0	0.01152	0.0606	0.004668	1446
/fiducial_transforms	25.48	0.006356	0.1234	0.01392	1446
/fiducial_pose	18.77	0.02711	0.1096	0.01284	1227

rate 40, CPU ~6%	topic	rate	min_delta	max_delta	std_dev	window
/cv_camera/image_raw/compressed	29.86	2.17e-05	0.4886	0.01702	1259
/fiducial_transforms	26.88	1.55e-05	0.3566	0.01971	1259
/fiducial_pose	26.62	1.478e-05	0.4819	0.02254	1133

rate 100, CPU ~6%	topic	rate	min_delta	max_delta	std_dev	window
/cv_camera/image_raw/compressed	30.01	0.009938	0.07629	0.005509	1236
/fiducial_transforms	25.85	9.441e-05	0.136	0.01295	1236
/fiducial_pose	25.85	9.561e-05	0.1318	0.01353	1063

[JanezCim]

Okay seems ok to me then, thank you @ivan140