Add 6dof support for velocity smoother #5243
Conversation
Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>
|
Still adding unit tests |
Codecov ReportAttention: Patch coverage is
... and 5 files with indirect coverage changes 🚀 New features to boost your workflow:
|
There was a problem hiding this comment.
There's alot of test gaps, but beyond that LGTM!
The tests for this node are remarkably simple due to its pointedness: https://github.com/ros-navigation/navigation2/blob/main/nav2_velocity_smoother/test/test_velocity_smoother.cpp
I think you just need to add a couple of test cases doing 6DOF and you should be good to go.
| @@ -119,18 +119,23 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State & state) | |||
| node->get_parameter("deadband_velocity", deadband_velocities_); | |||
| node->get_parameter("velocity_timeout", velocity_timeout_dbl); | |||
| velocity_timeout_ = rclcpp::Duration::from_seconds(velocity_timeout_dbl); | |||
| size_t size = max_velocities_.size(); | |||
There was a problem hiding this comment.
I think the for (unsigned int i = 0; i != 3; i++) { above needs to be updated based on the size for the full 6DOF
| { | ||
| RCLCPP_ERROR( | ||
| get_logger(), | ||
| "Invalid setting of kinematic and/or deadband limits!" | ||
| " All limits must be size of 3 representing (x, y, theta)."); | ||
| " All limits must be size of 3 or 6"); |
There was a problem hiding this comment.
| " All limits must be size of 3 or 6"); | |
| " All limits must be size of 3 (x, y, theta) or 6 (x, y, z, r, p, y)"); |
| @@ -333,15 +338,36 @@ void VelocitySmoother::smootherTimer() | |||
| } | |||
|
|
|||
| // Apply absolute velocity restrictions to the command | |||
| command_->twist.linear.x = std::clamp( | |||
| if(!is_6dof_) { | |||
| command_->twist.linear.x = std::clamp( | |||
| command_->twist.linear.x, min_velocities_[0], | |||
There was a problem hiding this comment.
I think you have some formatting issues. The line extension after the std::clamp should have an intend. Ditto for this entire block
There was a problem hiding this comment.
After going through the full thing, it looks like from this point to the end there's alot of lines that should have been indented that weren't from the original code and your added code. Please fix that :-)
| RCLCPP_WARN(get_logger(), "Invalid size of parameter %s. Must be size 3", | ||
| param_name.c_str()); | ||
| size_t size = is_6dof_ ? 6 : 3; | ||
| if (parameter.as_double_array().size() != 3 && parameter.as_double_array().size() != size) { |
There was a problem hiding this comment.
I don't understand the first check for 3. Shouldn't the second be enough? It shouldn't be reconfigurable to a different size. It should always be 3 or always be 6, I think
|
Thanks Steve for the review, I will aim to complete this over the weekend |
Basic Info
Description of contribution in a few bullet points
Description of documentation updates required from your changes
ros-navigation/docs.nav2.org#706
Description of how this change was tested
Future work that may be required in bullet points
For Maintainers: