Skip to content

Commit

Permalink
feat(motion_velocity_smoother)!: added force acceleration (#1329)
Browse files Browse the repository at this point in the history
* add feature for force acceleration

Signed-off-by: Go Sakayori <[email protected]>

* Add log info when force acceleration is activated/deactivated

Signed-off-by: Go Sakayori <[email protected]>

* fix enagage velocity/acceleration

Signed-off-by: Go Sakayori <[email protected]>

* Update planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp

* Update planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp

---------

Signed-off-by: Go Sakayori <[email protected]>
Signed-off-by: Go Sakayori <[email protected]>
Co-authored-by: Shumpei Wakabayashi <[email protected]>
  • Loading branch information
go-sakayori and shmpwk committed Aug 6, 2024
1 parent 0e539d1 commit e3b68d5
Show file tree
Hide file tree
Showing 5 changed files with 106 additions and 0 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,7 @@
#include "autoware_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 @@ -65,6 +66,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 @@ -97,6 +99,8 @@ class VelocitySmootherNode : public rclcpp::Node
sub_external_velocity_limit_{this, "~/input/external_velocity_limit_mps"};
autoware::universe_utils::InterProcessPollingSubscriber<OperationModeState> sub_operation_mode_{
this, "~/input/operation_mode_state"};
rclcpp::Service<SetBool>::SharedPtr srv_force_acceleration_;
rclcpp::Service<SetBool>::SharedPtr srv_slow_driving_;

Odometry::ConstSharedPtr current_odometry_ptr_; // current odometry
AccelWithCovarianceStamped::ConstSharedPtr current_acceleration_ptr_;
Expand Down Expand Up @@ -134,6 +138,15 @@ class VelocitySmootherNode : public rclcpp::Node
NORMAL = 3,
};

struct ForceAccelerationParam
{
double max_acceleration;
double max_jerk;
double max_lateral_acceleration;
double engage_velocity;
double engage_acceleration;
};

struct Param
{
bool enable_lateral_acc_limit;
Expand All @@ -159,6 +172,8 @@ class VelocitySmootherNode : public rclcpp::Node
AlgorithmType algorithm_type; // Option : JerkFiltered, Linf, L2

bool plan_from_ego_speed_on_manual_mode = true;

ForceAccelerationParam force_acceleration_param;
} node_param_{};

struct ExternalVelocityLimit
Expand Down Expand Up @@ -244,6 +259,11 @@ class VelocitySmootherNode : public rclcpp::Node

// parameter handling
void initCommonParam();
void onForceAcceleration(
const std::shared_ptr<SetBool::Request> request, std::shared_ptr<SetBool::Response> response);
bool force_acceleration_mode_;
void onSlowDriving(
const std::shared_ptr<SetBool::Request> request, std::shared_ptr<SetBool::Response> response);

// debug
autoware::universe_utils::StopWatch<std::chrono::milliseconds> stop_watch_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -82,6 +82,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/autoware_velocity_smoother/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,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_debug_msgs</depend>
Expand Down
67 changes: 67 additions & 0 deletions planning/autoware_velocity_smoother/src/node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@ VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_opti
: Node("velocity_smoother", node_options)
{
using std::placeholders::_1;
using std::placeholders::_2;

// set common params
const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo();
Expand Down Expand Up @@ -66,6 +67,13 @@ VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_opti
sub_current_trajectory_ = create_subscription<Trajectory>(
"~/input/trajectory", 1, std::bind(&VelocitySmootherNode::onCurrentTrajectory, this, _1));

srv_force_acceleration_ = create_service<SetBool>(
"~/adjust_common_param",
std::bind(&MotionVelocitySmootherNode::onForceAcceleration, this, _1, _2));
srv_slow_driving_ = create_service<SetBool>(
"~/slow_driving", std::bind(&MotionVelocitySmootherNode::onSlowDriving, this, _1, _2));
force_acceleration_mode_ = false;

// parameter update
set_param_res_ =
this->add_on_set_parameters_callback(std::bind(&VelocitySmootherNode::onParameter, this, _1));
Expand Down Expand Up @@ -184,6 +192,14 @@ rcl_interfaces::msg::SetParametersResult VelocitySmootherNode::onParameter(
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("force_acceleration.max_acc", p.force_acceleration_param.max_acceleration);
update_param("force_acceleration.max_jerk", p.force_acceleration_param.max_jerk);
update_param(
"force_acceleration.max_lateral_acc", p.force_acceleration_param.max_lateral_acceleration);
update_param("force_acceleration.engage_velocity", p.force_acceleration_param.engage_velocity);
update_param(
"force_acceleration.engage_acceleration", p.force_acceleration_param.engage_acceleration);
}

{
Expand Down Expand Up @@ -303,6 +319,16 @@ void VelocitySmootherNode::initCommonParam()

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

p.force_acceleration_param.max_acceleration =
declare_parameter<double>("force_acceleration.max_acc");
p.force_acceleration_param.max_jerk = declare_parameter<double>("force_acceleration.max_jerk");
p.force_acceleration_param.max_lateral_acceleration =
declare_parameter<double>("force_acceleration.max_lateral_acc");
p.force_acceleration_param.engage_velocity =
declare_parameter<double>("force_acceleration.engage_velocity");
p.force_acceleration_param.engage_acceleration =
declare_parameter<double>("force_acceleration.engage_acceleration");
}

void VelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory) const
Expand Down Expand Up @@ -1116,6 +1142,47 @@ TrajectoryPoint VelocitySmootherNode::calcProjectedTrajectoryPointFromEgo(
return calcProjectedTrajectoryPoint(trajectory, current_odometry_ptr_->pose.pose);
}

void MotionVelocitySmootherNode::onForceAcceleration(
const std::shared_ptr<SetBool::Request> request, std::shared_ptr<SetBool::Response> response)
{
std::string message = "default";

if (request->data && !force_acceleration_mode_) {
RCLCPP_INFO(get_logger(), "Force acceleration is activated");
smoother_->setMaxAccel(get_parameter("force_acceleration.max_acc").as_double());
smoother_->setMaxJerk(get_parameter("force_acceleration.max_jerk").as_double());
smoother_->setMaxLatAccel(get_parameter("force_acceleration.max_lateral_acc").as_double());
node_param_.engage_velocity = get_parameter("force_acceleration.engage_velocity").as_double();
node_param_.engage_acceleration =
get_parameter("force_acceleration.engage_acceleration").as_double();

force_acceleration_mode_ = true;
message = "Trigger force acceleration";
} else if (!request->data && force_acceleration_mode_) {
RCLCPP_INFO(get_logger(), "Force acceleration is deactivated");
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());

node_param_.engage_velocity = get_parameter("engage_velocity").as_double();
node_param_.engage_acceleration = get_parameter("engage_acceleration").as_double();

force_acceleration_mode_ = false;
message = "Trigger normal acceleration";
}

response->success = true;
}

void MotionVelocitySmootherNode::onSlowDriving(
const std::shared_ptr<SetBool::Request> request, std::shared_ptr<SetBool::Response> response)
{
const std::string message = request->data ? "Slow driving" : "Default";

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

} // namespace autoware::velocity_smoother

#include "rclcpp_components/register_node_macro.hpp"
Expand Down
15 changes: 15 additions & 0 deletions planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -97,6 +97,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

0 comments on commit e3b68d5

Please sign in to comment.