Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(motion_velocity_smoother): add force acceleration function #7180

Closed

Conversation

go-sakayori
Copy link
Contributor

Description

A function to adjust the parameters for the motion velocity smoother is added. The purpose is to realize quick acceleration when needed.

As it is a basic function, std_srvs::srv::SetBool service is implemented to adjust few parameters.
Once there is a request with "true", the parameters would be updated. If the request is "false" or there is no request the default values would be used for the velocity smoothing.

Related links

autowarefoundation/autoware_launch#1010

Tests performed

TIER IV interal evaluator + Psim

Notes for reviewers

Interface changes

ROS Service Changes

Service Name Type Description
/planning/scenario_planning/motion_velocity_smoother/adjust_common_param std_srvs::srv::SetBool Changes the parameter for maximum acceleration, maximum jerk, and maximum lateral acceleration
/planning/scenario_planning/motion_velocity_smoother/slow_driving std_srvs::srv::SetBool Receive a request

Effects on system behavior

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

  • The PR follows the pull request guidelines.
  • The PR has been properly tested.
  • The PR has been reviewed by the code owners.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

  • There are no open discussions or they are tracked via tickets.
  • The PR is ready for merge.

After all checkboxes are checked, anyone who has write access can merge the PR.

go-sakayori and others added 6 commits May 29, 2024 12:24
Signed-off-by: Go Sakayori <gsakayori@gmail.com>
Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>
Signed-off-by: Go Sakayori <gsakayori@gmail.com>
Signed-off-by: Go Sakayori <gsakayori@gmail.com>
Signed-off-by: Go Sakayori <gsakayori@gmail.com>
Signed-off-by: Go Sakayori <gsakayori@gmail.com>
@github-actions github-actions bot added the component:planning Route planning, decision-making, and navigation. (auto-assigned) label May 30, 2024
@go-sakayori go-sakayori marked this pull request as ready for review May 30, 2024 13:15
void MotionVelocitySmootherNode::onSlow(
const std::shared_ptr<SetBool::Request> request, std::shared_ptr<SetBool::Response> response)
{
bool success = true;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

No need to define the variable success.

{
bool success = true;

std::string message = request->data ? "Slow driving" : "Default";
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Add const

Comment on lines +96 to +97
rclcpp::Service<SetBool>::SharedPtr srv_adjust_common_param;
rclcpp::Service<SetBool>::SharedPtr srv_slow_driving;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Add _ at the last of the member variables' names.

@@ -72,6 +73,13 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions
"~/input/operation_mode_state", 1,
[this](const OperationModeState::ConstSharedPtr msg) { operation_mode_ = *msg; });

srv_adjust_common_param = create_service<SetBool>(
"~/adjust_common_param", std::bind(&MotionVelocitySmootherNode::onAdjustParam, this, _1, _2));
adjustParam = false;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is this variable necessary?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thought it might not be necessary to set the force acceleration parameter every time when the request is continuously true. The same applies when false is continuous.
What do you think about it?

@TakaHoribe
Copy link
Contributor

@go-sakayori
This advice is not just for this PR but in general: when communicating between systems (like between nodes in this case), it’s important to clearly define the scope and meaning of the interface. Here are a few ways to improve this PR:

  1. The type currently used (std_srvs::srv::SetBool) doesn’t convey meaningful information, making it hard to understand its function. This is why std_msgs is deprecated in ROS2. It’s better to create specific types for specific purposes.
  2. The service name (adjust_common_param) is unclear. Terms like "common" and "adjust" are vague (and there is no README to explain). Since this command updates acceleration behavior constraints, a clearer name like enable_aggressive_mode or use_strong_acceleration_params would be better, especially for an OSS project. (slow_driving is a good example of a clear name).
  3. Using a Bool for this operation limits future extensibility. If there might be a need to change acceleration behavior in steps in the future, consider introducing modes or an interface that allows direct control of acceleration and deceleration. However, extensibility should be balanced with the scope of the interface.

I leave it to you to decide how much to address these points in this PR (at least I want to have a README though), but please keep them in mind. Autoware.ai had many interfaces with unclear and vague names, making maintenance very difficult.

@go-sakayori
Copy link
Contributor Author

It would be closed. Would make a PR once the functions are arranged in proper architecture.

@go-sakayori go-sakayori deleted the feat/force-acceleration branch December 19, 2024 07:33
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
component:planning Route planning, decision-making, and navigation. (auto-assigned)
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants