Optimize integrators, Rviz plugin and add usage examples #701

Job	Run time
Define common variables	4s
Build CI workspace container	1m 4s
Lint	43s
Build	2m 1s
Build docs	1m 38s
Clang tidy	7m 33s
Catkin install	2m 48s
Test	3m 28s
Valgrind memcheck	11m 46s
Sanitize undefined behavior	8m 22s
Coverage	0s
Sanitize addressability and leaks	7m 9s
	46m 36s

Provide feedback