add Queue handling in solver_trajectory_subscriber

[Sergim96]

Hi @cmastalli this PR brings the queue concept to crocoddyl_msgs. As you know, this feature is needed for our MPC and bringing it to crocoddyl_msgs lets us:

1. use a dynamic and measured communication delay
2. unittest its behavior.

The behaviour of the queue is well-documented and should be self-explanatory, I believe it is extensively unittested so we are confident it is working correctly, I had to write the unittest in c++ as ros.time from python is inconsistent with the c++ one, making the unittest unreliable.

in addition it fixes a minor bug in the updateBodyInertialParameters of the reduced model.

Let me known your thoughts!

[cmastalli]

The PR is quite complete and well-developed. However, there are a few aspects to improve:

1. Allocate date for merged objects,
2. Support unit tests for ROS2.

This latter point is the most relevant in my opinon. Also, because we need to kill our ROS1 CI. Can you also handle this in this PR?

[cmastalli]

Let's allocate data for merged objects as well.

[cmastalli]

Should we remove this?

[Sergim96]

yes sorry! I forgot to remove that

[cmastalli]

Could we add some unit tests for all these cases?

[Sergim96]

they already are see here

crocoddyl_msgs/unittest/test_whole_body_state.py

Lines 406 to 523 in 523aa2d

	def test_update_reduced_model(self):
	qref = pinocchio.randomConfiguration(self.MODEL)
	reduced_model = pinocchio.buildReducedModel(
	self.MODEL,
	[self.MODEL.getJointId(name) for name in self.LOCKED_JOINTS],
	qref,
	)
	pub = WholeBodyStateRosPublisher(
	reduced_model, "whole_body_state_update_reduced_model"
	)
	sub = WholeBodyStateRosSubscriber(
	reduced_model, "whole_body_state_update_reduced_model"
	)
	time.sleep(1)
	# update inertia parameters
	if pinocchio.__version__ >= "2.7.1":
	frame_names = [
	f.name
	for f in reduced_model.frames
	if f.name != "universe"
	and (
	f.type == pinocchio.BODY
	or f.type == pinocchio.JOINT
	or f.type == pinocchio.FIXED_JOINT
	)
	]
	else:
	frame_names = [
	f.name for f in reduced_model.frames if f.type == pinocchio.JOINT
	]
	new_parameters = []
	for name in frame_names:
	psi = pinocchio.Inertia.Random().toDynamicParameters()
	new_parameters.append(psi)
	pub.update_body_inertial_parameters(name, psi)
	sub.update_body_inertial_parameters(name, psi)
	# publish whole-body state messages
	nv_root = getRootNv(self.MODEL)
	q = pinocchio.randomConfiguration(self.MODEL)
	v = np.random.rand(self.MODEL.nv)
	tau = np.random.rand(self.MODEL.nv - nv_root)
	q, v, tau = toReduced(self.MODEL, reduced_model, q, v, tau)
	while True:
	pub.publish(self.t, q, v, tau, self.p, self.pd, self.f, self.s)
	if sub.has_new_msg():
	break
	# get inertias
	for i, name in enumerate(frame_names):
	pub_parameters = pub.get_body_inertial_parameters(name)
	sub_parameters = sub.get_body_inertial_parameters(name)
	self.assertTrue(
	np.allclose(
	pub_parameters,
	new_parameters[i],
	atol=1e-9,
	),
	"Wrong publisher's inertial parameters in frame "
	+ name
	+ "\n"
	+ "desired:\n"
	+ str(new_parameters[i])
	+ "obtained:\n"
	+ str(pub_parameters),
	)
	self.assertTrue(
	np.allclose(
	sub_parameters,
	new_parameters[i],
	atol=1e-9,
	),
	"Wrong subscriber's inertial parameters in frame "
	+ name
	+ "\n"
	+ "desired:\n"
	+ str(new_parameters[i])
	+ "obtained:\n"
	+ str(sub_parameters),
	)
	# get whole-body state
	_t, _q, _v, _tau, _p, _pd, _f, _s = sub.get_state()
	qdiff = pinocchio.difference(reduced_model, q, _q)
	mask = ~np.isnan(qdiff)
	self.assertEqual(self.t, _t, "Wrong time interval")
	self.assertTrue(
	np.allclose(qdiff[mask], np.zeros(reduced_model.nv)[mask], atol=1e-9),
	"Wrong q",
	)
	self.assertTrue(np.allclose(v, _v, atol=1e-9), "Wrong v")
	self.assertTrue(np.allclose(tau, _tau, atol=1e-9), "Wrong tau")
	for name in self.p:
	M, [_t, _R] = self.p[name], _p[name]
	F, _F = self.f[name], _f[name]
	S, _S = self.s[name], _s[name]
	self.assertTrue(
	np.allclose(M.translation, _t, atol=1e-9),
	"Wrong contact translation at " + name,
	)
	self.assertTrue(
	np.allclose(M.rotation, _R, atol=1e-9),
	"Wrong contact rotation at " + name,
	)
	self.assertTrue(
	np.allclose(self.pd[name].vector, _pd[name], atol=1e-9),
	"Wrong contact velocity translation at " + name,
	)
	self.assertTrue(
	np.allclose(F[0], _F[0], atol=1e-9),
	"Wrong contact wrench translation at " + name,
	)
	self.assertTrue(F[1] == _F[1], "Wrong contact type at " + name)
	self.assertTrue(F[2] == _F[2], "Wrong contact status at " + name)
	self.assertTrue(
	np.allclose(S[0], _S[0], atol=1e-9),
	"Wrong contact surface translation at " + name,
	)
	self.assertEqual(
	S[1], _S[1], "Wrong contact friction coefficient at " + name
	)

the problem is that this test is not running in the CI due to the pinocchio version requirement

[cmastalli]

We are not running test_solver_queue on ROS2, which should be our first support.

[cmastalli]

Weird code. No name for this namespace.

[Sergim96]

what do you mean?

[cmastalli]

Your namespace doesn't have a name.

[cmastalli]

Do we need to use 1e-9 as a tolerance? I believe we can remove the tolerance for each test case.

[Sergim96]

Provably no, I will remove it

[cmastalli]

I have pushed some minor changes that I preferred not to comment on.

[cmastalli]

@Sergim96 -- please ping me when you're done with my comments.

[Sergim96]

Hi @cmastalli I don't know what happened but the test is getting stuck in some kind of endless loop now, are you able to run it?

[cmastalli]

Hey @Sergim96! I am swamped in this period. Could you be more specific? Is this happening on your PC?