Velocity smoother tests

[stonier]

No description provided.

[stonier]

@clalancette this is finally starting to pay dividends, i.e. finding bugs!

For reference...

You might want to make sure we're careful not to default-construct any rclcpp::Time objects. When they get compared with objects later generated via this->get_clock()->now(), it throws:

terminate called after throwing an instance of 'std::runtime_error'
  what():  can't subtract times with different time sources [1 != 2]

I've also started plugging in defaults for the parameters. Should anyone want to run the nodes directly, it crashes if these do not have a default value at the type error check (e.g. 'not set' != double).

[stonier]

Current Status

• Test wiring works
• Matplotlib's default Tinker backend doesn't
• Suggested 'Agg' backend doesn't either (no window appears, but no error either)
• Getting spammed with the following. Probably something to do with the feedback (odom or otherwise) or the feedback being broken

[velocity_smoother-2] [WARN] [velocity_smoother]: Velocity Smoother : using robot velocity feedback end commands instead of last command: 0.001420, -0.045000, 0.000000

• Resulting plot. Obviously broken - something to do with the preceding bullet most likely
  

[clalancette]

You might want to make sure we're careful not to default-construct any rclcpp::Time objects.

Good call.

I've also started plugging in defaults for the parameters. Should anyone want to run the nodes directly, it crashes if these do not have a default value at the type error check (e.g. 'not set' != double).

For what it is worth, that was the ROS 1 behavior; these were marked as "mandatory" parameters. I kind of agree with the ROS 1 behavior here, as this is something that should definitely be tuned per-robot. It does make it a little more annoying to run it on the command-line, but you can always do so via:

ros2 run velocity_smoother velocity_smoother_node --ros-args -p speed_lim_v:=0.8 -p speed_lim_w:=5.4 -p accel_lim_v:=0.3 accel_lim_w:=3.5

[clalancette]

Looking at this a bit further, I found a few things:

1. There is a bug in the timerCB(), where if we have achieved our target, we'll constantly send 0 to the smoothed output (as opposed to the last command). I've fixed this in 5e73b1f.
2. There is a potential bug in timerCB() where we weren't initializing the last_cmd_vel_{linear_x, angular_z}_. I've fixed this in 8891a43.
3. Even after those fixes, I'm still seeing the plot above. I noticed that the test is setup incorrectly, however. The robot_feedback parameter is set to "COMMAND", which means that it is looking for command velocities as the feedback input (on robot_cmd_vel). But nothing is publishing on robot_cmd_vel in this test. If you look closely at the ROS 1 version of the test, you can see that the smooth_cmd_vel output is being remapped to cmd_vel/output, while the robot_cmd_vel input is also being remapped to cmd_vel/output. That means that the smoothed output is also being used as the input for the next iteration. As far as I can tell, in the ROS 1 version, the odometry is completely unused. There are 2 ways to fix this; either do the remapping as the ROS 1 version of the test does, or switch robot_feedback to ODOMETRY (1), which allows us to proceed further. I've pushed this in 8a4e68d, though I'm not entirely sure what we want to use as a default.
4. After all of the above, running the test and getting the plot shows me:

This at least gets rid of the more obvious errors, but doesn't look "smoothed" at all to me. So I think there is probably still something else wrong here, but I'm not sure what.

[clalancette]

All right, and it turns out that just using the odometry is no good. The problem there is that the odometry is "lying" during the test; it says the robot is going faster and faster, even though the cmd_vel we are outputting isn't commanding that. So I instead changed the test to work like the ROS 1 version, where the "smooth_cmd_vel" is fed back into the "robot_cmd_vel", and that seems to make it work. At least, I can tune down the linear acceleration and maximum speed, and see those have an effect on the graph. @stonier take a look at what I've done in ffcbcc8, you may or may not agree with it.

[stonier]

Oh, bugger. Looks like I was walking down the same paths you did (rewiring the feedback channel appropriately), pushed a commit thinking the merge conflict was just something leftover from what I'd done at work.

I'm getting smoothing, but there is still some logic breakdown early in the profile. The code seems to be brittle - lots of if-else decisions that just try to make it work. I'll revisit this again ASAP, though the next few days will be difficult to find the time. If you do want to keep plugging away, would you like to push on the cmd_vel_mux?

[stonier]

Alright...working! Well, just one translational test that works for all variants of feedback. Certainly doesn't cover the permutations of things that can happen, nor does it cover angular rotational smoothing. But is probably enough to get started.

NB: Not a big fan of this package, but until it can be proven that it doesn't do the 'job' for kobuki, a refactor can wait.

[stonier]

Re the aforementioned parameter discussion. I'm fine with either requiring parameters to be set on startup, or providing defaults (just noticed that the original package required these as well, missed that the first time around). The only thing that should be avoided is conflating the error messages for parameter not set and parameter is of the wrong type.

When you see the program crash out with speed_lim_w must be specified as a double it leads you to think there was a genuine error in configuring the node, when in fact it was None is not type double because the node author requires a parameter to be set.

I left them with default for now simply because I'm too lazy to revert and test :)