[RST-2128] fuse rl ignition sensor #6

svwilliams · 2019-06-18T01:10:43Z

Created an ignition/origin sensor compatible with the unicycle 2D motion model, and roughly equivalent to how the robot_localization package handles state initialization.

Sends initial priors on all the state variables. The values are read from the parameter server.
In response to a set_pose message or set_pose service call, resets the optimizer. Then sets the pose variables to the provided pose values and the rest of the state to the values from the parameter server.

svwilliams · 2019-06-18T01:11:34Z

include/fuse_rl/unicycle_2d/ignition.h

+  /**
+   * @brief Triggers the publication of a new prior transaction at the supplied pose
+   */
+  bool serviceCallback(robot_localization::SetPose::Request& req, robot_localization::SetPose::Request&);


I feel a little strange about using the robot_localization package here. Anyone have any better ideas?

Message types in ROS don't care about the actual name/type of the message, so long as the contents are identical. We've taken advantage of this internally. With that in mind, I think I'd opt for the following:

Create the identical service type in fuse_rl, and switch this callback to it, but add a deprecation notice when it's called.

Create a new service type that also has a return code (why I neglected that in the first place, I can't say).

Add a callback for the new service, and make this one wrap to it.

But that doesn't need to be for this PR. Feel free to leave this as-is, and throw that task my way.

Like so (bool success, string reason)? Or would you prefer enum-like error codes? Or different names? Do we have a standard naming convention for service call return values?

svwilliams · 2019-06-18T01:14:18Z

src/unicycle_2d/ignition.cpp

+    *position,
+    fuse_core::Vector2d(position->x(), position->y()),
+    position_cov);
+  auto orientation_constraint = fuse_constraints::AbsoluteOrientation2DStampedConstraint::make_shared(


We are cheating here a little bit by creating separate position and orientation constraints. This prevents us from including the position-orientation correlation terms from the PoseWithCovarianceStamped message. I could switch this to an AbsolutePose2DStampedConstraint if we feel like that is important.

I say leave it this way. I don't think I've ever seen anyone set the off-diagonal terms in a SetPose service call, or from the parameters.

ayrton04 · 2019-06-18T09:05:06Z

include/fuse_rl/unicycle_2d/ignition.h

+  /**
+   * @brief Triggers the publication of a new prior transaction at the supplied pose
+   */
+  bool serviceCallback(robot_localization::SetPose::Request& req, robot_localization::SetPose::Request&);


Message types in ROS don't care about the actual name/type of the message, so long as the contents are identical. We've taken advantage of this internally. With that in mind, I think I'd opt for the following:

Create the identical service type in fuse_rl, and switch this callback to it, but add a deprecation notice when it's called.

Create a new service type that also has a return code (why I neglected that in the first place, I can't say).

Add a callback for the new service, and make this one wrap to it.

But that doesn't need to be for this PR. Feel free to leave this as-is, and throw that task my way.

ayrton04 · 2019-06-18T09:14:38Z

src/unicycle_2d/ignition.cpp

+    *position,
+    fuse_core::Vector2d(position->x(), position->y()),
+    position_cov);
+  auto orientation_constraint = fuse_constraints::AbsoluteOrientation2DStampedConstraint::make_shared(


I say leave it this way. I don't think I've ever seen anyone set the off-diagonal terms in a SetPose service call, or from the parameters.

efernandez · 2019-06-20T09:45:22Z

src/unicycle_2d/ignition.cpp

+                                         pose.pose.pose.orientation.y,
+                                         pose.pose.pose.orientation.z);
+  auto linear_velocity = fuse_variables::VelocityLinear2DStamped::make_shared(stamp, device_id_);
+  linear_velocity->x() = params_.initial_state[0];


I think the indices are wrong. This should be:

linear_velocity->x() = params_.initial_state[3]; linear_velocity->y() = params_.initial_state[4]; auto angular_velocity = fuse_variables::VelocityAngular2DStamped::make_shared(stamp, device_id_); angular_velocity->yaw() = params_.initial_state[5]; auto linear_acceleration = fuse_variables::AccelerationLinear2DStamped::make_shared(stamp, device_id_); linear_acceleration->x() = params_.initial_state[6]; linear_acceleration->y() = params_.initial_state[7]; // Create the covariances for each variable // The pose covariances are extracted from the provided PoseWithCovarianceStamped message. // The remaining covariances are provided as parameters to the parameter server. auto linear_velocity_cov = fuse_core::Matrix2d(); linear_velocity_cov << params_.initial_sigma[3] * params_.initial_sigma[3], 0.0, 0.0, params_.initial_sigma[4] * params_.initial_sigma[4]; auto angular_velocity_cov = fuse_core::Matrix1d(); angular_velocity_cov << params_.initial_sigma[5] * params_.initial_sigma[5]; auto linear_acceleration_cov = fuse_core::Matrix2d(); linear_acceleration_cov << params_.initial_sigma[6] * params_.initial_sigma[6], 0.0, 0.0, params_.initial_sigma[7] * params_.initial_sigma[7];

efernandez · 2019-06-20T09:56:31Z

include/fuse_rl/parameters/unicycle_2d_ignition_params.h

+        if (!std::all_of(sigma_vector.begin(), sigma_vector.end(), is_sigma_valid))
+        {
+          throw std::invalid_argument("The supplied initial_sigma parameter must contain valid floating point values. "
+                                      "NaN, Inf, and values <=0 are not acceptable.");


values <=0 -> values <= 0

efernandez · 2019-06-20T13:48:33Z

src/unicycle_2d/ignition.cpp

+  // Connect to the reset service
+  if (!params_.reset_service.empty())
+  {
+    reset_client_ = node_handle_.serviceClient<std_srvs::Empty>(params_.reset_service);


This requires the config to specify the node name. Unfortunately, private_node_handle_ also requires that.
I wonder if there's a better solution for this. 🤔

I feel like I need to use the private node handle to advertise topics/services within the plugins. Otherwise the default names will step on each other if you attempt to run the same plugin multiple times (e.g. multiple scan match sensors for robots with multiple lasers). This, of course, puts the node name in the topic and makes remapping topics almost mandatory. Tom's preference was to supply the topic names via parameter instead. Not that it solves anything; the full node name is still required. But it allows you to manage the entire config from within the yaml file, instead of half in the yaml file and half in the launch. I suppose with this method you can choose between using the default topic names and remapping them via the launch file, or providing the desired topic name via parameters.

For this specific line, the optimizer could advertise the "reset" service in the global namespace. It just seemed like "reset" was so generic that it should be advertised in the local namespace. "/smoother/reset" instead of "/reset". But since this service is advertised at the optimizer level and can be easily remapped to whatever you want, I don't have a strong preference on what the default should be.

@ayrton04 Do you have any thoughts on any of this?

So yes, the reason I prefer parameters is precisely because plugins are a pain. If you have two plugins, and both subscribe to some topic, and you want to just change one of them to listen to another topic, you're hosed, because remapping will force them both to the new topic. For that reason, I prefer exposing parameters to specify topics. You get the best of both worlds: you can use the parameter, but you can still remap if you want to, as Stephen pointed out.

I think the best option is to advertise locally, subscribe globally. We have to assume that there will be many plugins, so advertising locally make sense. But for subscibers/clients, you can't know the other namespaces in the system, so you have to assume global and remap/use parameters.

Thanks for your replies 👍

efernandez · 2019-06-20T20:26:05Z

Although everything LGTM here, I'm having trouble to run this in my setup.

As soon as I start the fixed-lag smoother I get NaN in the state. I have an odom -> map configuration and I even tried removing the initial zero-pose set in the ignition sensor initialization, so only when I set pose I actually gets things moving forward.

I wonder if you're experiencing the same with your setup 🤔

efernandez · 2019-06-20T20:29:58Z

BTW In my tests I rebased the branch of this PR on top of devel. I saw there's a commit you don't have here. Just saying, because we might not be testing the same thing.

svwilliams · 2019-06-21T20:17:21Z

Rebased on top of #8 for testing purposes

efernandez · 2019-06-24T22:22:00Z

src/unicycle_2d/ignition.cpp

@@ -88,17 +88,21 @@ void Ignition::onInit()
  // Connect to the reset service
  if (!params_.reset_service.empty())
  {
-    reset_client_ = node_handle_.serviceClient<std_srvs::Empty>(params_.reset_service);
+    reset_client_ = node_handle_.serviceClient<std_srvs::Empty>(ros::names::resolve(params_.reset_service));


Could you do the same for the server side in fuse, please?

efernandez

I've been testing this for while now and it's working fine.

I must say that when I set pose I eventually get a rank deficient covariance matrix, which crashes the program. It doesn't happen all the time, but it's quite frequently, approx. 30% of the time. However, I think this could be squashed and merged as is and then deal with that other issue. I guess you should be able to easily reproduce it by setting a pose over and over again until it crashes.

ayrton04

LGTM

svwilliams requested a review from ayrton04 June 18, 2019 01:10

svwilliams commented Jun 18, 2019

View reviewed changes

ayrton04 previously approved these changes Jun 18, 2019

View reviewed changes

svwilliams dismissed ayrton04’s stale review via 0657b7f June 18, 2019 23:54

efernandez reviewed Jun 20, 2019

View reviewed changes

svwilliams requested a review from ayrton04 June 20, 2019 18:29

ayrton04 previously approved these changes Jun 21, 2019

View reviewed changes

svwilliams dismissed ayrton04’s stale review via 543b5d5 June 21, 2019 20:16

svwilliams force-pushed the RST-2128-fuse_rl-ignition-sensor branch from 90d91e7 to 543b5d5 Compare June 21, 2019 20:16

This was referenced Jun 21, 2019

Add allocate_shared and allocate_aligned_shared locusrobotics/fuse#62

Closed

Fix append partial measurement merging indices #9

Closed

efernandez reviewed Jun 24, 2019

View reviewed changes

svwilliams requested a review from ayrton04 June 25, 2019 14:50

svwilliams added 11 commits June 25, 2019 14:15

Initial code for the unicycle 2d ignition sensor. Not tested yet.

65d663f

Added validity checks for the pose request

67887d6

Created set_pose srv definitions for fuse_rl

ba4c919

PR feedback

58f2494

PR feedback

ea8d98d

Added some simple unit tests to the Ignition sensor

4b29b84

Added rostest test depend

c61a2b6

Small tweak to the SetPose service definition

55c955c

Added missing header

9c49903

Added flag to enable/disable the transaction on startup

7e6dcdb

Better handling of topic names and namespaces

60b4a60

svwilliams force-pushed the RST-2128-fuse_rl-ignition-sensor branch from bf8f38f to 6539471 Compare June 25, 2019 21:16

svwilliams force-pushed the RST-2128-fuse_rl-ignition-sensor branch from 6539471 to 60b4a60 Compare June 25, 2019 21:19

PR feedback

9dafa74

efernandez approved these changes Jun 26, 2019

View reviewed changes

ayrton04 approved these changes Jun 26, 2019

View reviewed changes

svwilliams merged commit 5c6d563 into devel Jun 26, 2019

svwilliams deleted the RST-2128-fuse_rl-ignition-sensor branch June 26, 2019 13:06

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[RST-2128] fuse rl ignition sensor #6

[RST-2128] fuse rl ignition sensor #6

svwilliams commented Jun 18, 2019

svwilliams Jun 18, 2019

ayrton04 Jun 18, 2019

svwilliams Jun 18, 2019

svwilliams Jun 18, 2019

ayrton04 Jun 18, 2019

ayrton04 Jun 18, 2019

ayrton04 Jun 18, 2019

efernandez Jun 20, 2019

efernandez Jun 20, 2019

efernandez Jun 20, 2019

svwilliams Jun 20, 2019

ayrton04 Jun 20, 2019

efernandez Jun 20, 2019

efernandez commented Jun 20, 2019

efernandez commented Jun 20, 2019

svwilliams commented Jun 21, 2019

efernandez Jun 24, 2019

svwilliams Jun 24, 2019

efernandez left a comment

ayrton04 left a comment

[RST-2128] fuse rl ignition sensor #6

[RST-2128] fuse rl ignition sensor #6

Conversation

svwilliams commented Jun 18, 2019

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

efernandez commented Jun 20, 2019

efernandez commented Jun 20, 2019

svwilliams commented Jun 21, 2019

Choose a reason for hiding this comment

Choose a reason for hiding this comment

efernandez left a comment

Choose a reason for hiding this comment

ayrton04 left a comment

Choose a reason for hiding this comment