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.

Sign up for GitHub

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
wants to merge 6 commits into from
Closed

feat(motion_velocity_smoother): add force acceleration function #7180

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
34b3bf6
added service for force acceleration
go-sakayori May 29, 2024
b8a1eb1
add parameter to modify max lateral acceleration
go-sakayori May 30, 2024
8e6f973
Merge branch 'awf-latest' into feat/force-acceleration
go-sakayori May 30, 2024
ca551be
neatly formated
go-sakayori May 30, 2024
85830a4
Merge branch 'awf-latest' into feat/force-acceleration
go-sakayori May 30, 2024
bfff4bf
fix
go-sakayori May 30, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 14 additions & 0 deletions ...tion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,7 @@
#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "std_srvs/srv/set_bool.hpp"
#include "tier4_debug_msgs/msg/float32_stamped.hpp" // temporary
#include "tier4_planning_msgs/msg/stop_speed_exceeded.hpp" // temporary
#include "tier4_planning_msgs/msg/velocity_limit.hpp" // temporary
Expand All @@ -63,6 +64,7 @@ using geometry_msgs::msg::AccelWithCovarianceStamped;
using geometry_msgs::msg::Pose;
using geometry_msgs::msg::PoseStamped;
using nav_msgs::msg::Odometry;
using std_srvs::srv::SetBool;
using tier4_debug_msgs::msg::Float32Stamped; // temporary
using tier4_planning_msgs::msg::StopSpeedExceeded; // temporary
using tier4_planning_msgs::msg::VelocityLimit; // temporary
Expand Down Expand Up @@ -91,6 +93,8 @@ class MotionVelocitySmootherNode : public rclcpp::Node
rclcpp::Subscription<Trajectory>::SharedPtr sub_current_trajectory_;
rclcpp::Subscription<VelocityLimit>::SharedPtr sub_external_velocity_limit_;
rclcpp::Subscription<OperationModeState>::SharedPtr sub_operation_mode_;
rclcpp::Service<SetBool>::SharedPtr srv_adjust_common_param;
rclcpp::Service<SetBool>::SharedPtr srv_slow_driving;
Comment on lines +96 to +97
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.


Odometry::ConstSharedPtr current_odometry_ptr_; // current odometry
AccelWithCovarianceStamped::ConstSharedPtr current_acceleration_ptr_;
Expand Down Expand Up @@ -153,6 +157,10 @@ class MotionVelocitySmootherNode : public rclcpp::Node
AlgorithmType algorithm_type; // Option : JerkFiltered, Linf, L2

bool plan_from_ego_speed_on_manual_mode = true;

double adjusted_max_acceleration; // enable strong acceleration
double adjusted_max_jerk; // enable strong acceleration
double adjusted_max_lateral_acceleration; // enable strong acceleration
} node_param_{};

struct ExternalVelocityLimit
Expand All @@ -161,6 +169,7 @@ class MotionVelocitySmootherNode : public rclcpp::Node
double dist{0.0}; // distance to set external velocity limit
std::string sender{""};
};

ExternalVelocityLimit
external_velocity_limit_; // velocity and distance constraint of external velocity limit

Expand Down Expand Up @@ -242,6 +251,11 @@ class MotionVelocitySmootherNode : public rclcpp::Node

// parameter handling
void initCommonParam();
void onAdjustParam(
const std::shared_ptr<SetBool::Request> request, std::shared_ptr<SetBool::Response> response);
bool adjustParam;
void onSlow(
const std::shared_ptr<SetBool::Request> request, std::shared_ptr<SetBool::Response> response);

// debug
tier4_autoware_utils::StopWatch<std::chrono::milliseconds> stop_watch_;
Expand Down
3 changes: 3 additions & 0 deletions ...ning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -79,6 +79,9 @@ class SmootherBase
double getMinJerk() const;

void setWheelBase(const double wheel_base);
void setMaxAccel(const double max_accel);
void setMaxJerk(const double max_jerk);
void setMaxLatAccel(const double max_accel);

void setParam(const BaseParam & param);
BaseParam getBaseParam() const;
Expand Down
1 change: 1 addition & 0 deletions planning/motion_velocity_smoother/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@
<depend>nav_msgs</depend>
<depend>osqp_interface</depend>
<depend>rclcpp</depend>
<depend>std_srvs</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>tier4_autoware_utils</depend>
Expand Down
52 changes: 52 additions & 0 deletions planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2021 Tier IV, Inc.

Check notice on line 1 in planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Overall Code Complexity 

The mean cyclomatic complexity decreases from 4.03 to 4.03, threshold = 4. This file has many conditional statements (e.g. if, for, while) across its implementation, leading to lower code health. Avoid adding more conditionals.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -38,6 +38,7 @@
: Node("motion_velocity_smoother", node_options)
{
using std::placeholders::_1;
using std::placeholders::_2;

// set common params
const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo();
Expand Down Expand Up @@ -72,6 +73,13 @@
"~/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?


srv_slow_driving = create_service<SetBool>(
"~/slow_driving", std::bind(&MotionVelocitySmootherNode::onSlow, this, _1, _2));

// parameter update
set_param_res_ = this->add_on_set_parameters_callback(
std::bind(&MotionVelocitySmootherNode::onParameter, this, _1));
Expand Down Expand Up @@ -189,6 +197,10 @@
update_param("ego_nearest_dist_threshold", p.ego_nearest_dist_threshold);
update_param("ego_nearest_yaw_threshold", p.ego_nearest_yaw_threshold);
update_param_bool("plan_from_ego_speed_on_manual_mode", p.plan_from_ego_speed_on_manual_mode);

update_param("adjusted_max_acceleration", p.adjusted_max_acceleration);
update_param("adjusted_max_jerk", p.adjusted_max_jerk);
update_param("adjusted_max_lateral_acceleration", p.adjusted_max_lateral_acceleration);

Check warning on line 203 in planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method 

MotionVelocitySmootherNode::onParameter already has high cyclomatic complexity, and now it increases in Lines of Code from 121 to 124. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.
}

{
Expand Down Expand Up @@ -308,6 +320,11 @@

p.plan_from_ego_speed_on_manual_mode =
declare_parameter<bool>("plan_from_ego_speed_on_manual_mode");

p.adjusted_max_acceleration = declare_parameter<double>("adjusted_max_acceleration");
p.adjusted_max_jerk = declare_parameter<double>("adjusted_max_jerk");
p.adjusted_max_lateral_acceleration =
declare_parameter<double>("adjusted_max_lateral_acceleration");
}

void MotionVelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory) const
Expand Down Expand Up @@ -1100,6 +1117,41 @@
return calcProjectedTrajectoryPoint(trajectory, current_odometry_ptr_->pose.pose);
}

void MotionVelocitySmootherNode::onAdjustParam(
const std::shared_ptr<SetBool::Request> request, std::shared_ptr<SetBool::Response> response)
{
bool success = true;

if (request->data && !adjustParam) {
smoother_->setMaxAccel(get_parameter("adjusted_max_acceleration").as_double());
smoother_->setMaxJerk(get_parameter("adjusted_max_jerk").as_double());
smoother_->setMaxLatAccel(get_parameter("adjusted_max_lateral_acceleration").as_double());

adjustParam = true;
} else if (!request->data && adjustParam) {
smoother_->setMaxAccel(get_parameter("normal.max_acc").as_double());
smoother_->setMaxJerk(get_parameter("normal.max_jerk").as_double());
smoother_->setMaxLatAccel(get_parameter("max_lateral_accel").as_double());

adjustParam = false;
}
std::string message = success ? "Operation completed successfully" : "Operation failed";

response->success = success;
response->message = message;
}

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.


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


response->success = success;
response->message = message;
}

} // namespace motion_velocity_smoother

#include "rclcpp_components/register_node_macro.hpp"
Expand Down
15 changes: 15 additions & 0 deletions planning/motion_velocity_smoother/src/smoother/smoother_base.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -95,6 +95,21 @@ void SmootherBase::setWheelBase(const double wheel_base)
base_param_.wheel_base = wheel_base;
}

void SmootherBase::setMaxAccel(const double max_accel)
{
base_param_.max_accel = max_accel;
}

void SmootherBase::setMaxJerk(const double max_jerk)
{
base_param_.max_jerk = max_jerk;
}

void SmootherBase::setMaxLatAccel(const double max_accel)
{
base_param_.max_lateral_accel = max_accel;
}

void SmootherBase::setParam(const BaseParam & param)
{
base_param_ = param;
Expand Down
Loading