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 motion_velocity_smoother's virtual wall #1024

Merged
merged 1 commit into from
Nov 20, 2023
Merged

feat(motion_velocity_smoother): add motion_velocity_smoother's virtual wall #1024

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
1 change: 1 addition & 0 deletions planning/external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,7 @@ VelocityLimit getHardestLimit(
// find hardest max velocity
if (max_velocity < hardest_max_velocity) {
hardest_limit.stamp = limit.second.stamp;
hardest_limit.sender = limit.first;
hardest_limit.max_velocity = max_velocity;
hardest_max_velocity = max_velocity;
}
Expand Down
5 changes: 5 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 @@ -42,6 +42,7 @@
#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
#include "visualization_msgs/msg/marker_array.hpp"

#include <iostream>
#include <memory>
Expand All @@ -63,6 +64,7 @@ using nav_msgs::msg::Odometry;
using tier4_debug_msgs::msg::Float32Stamped; // temporary
using tier4_planning_msgs::msg::StopSpeedExceeded; // temporary
using tier4_planning_msgs::msg::VelocityLimit; // temporary
using visualization_msgs::msg::MarkerArray;

struct Motion
{
Expand All @@ -80,6 +82,7 @@ class MotionVelocitySmootherNode : public rclcpp::Node

private:
rclcpp::Publisher<Trajectory>::SharedPtr pub_trajectory_;
rclcpp::Publisher<MarkerArray>::SharedPtr pub_virtual_wall_;
rclcpp::Publisher<StopSpeedExceeded>::SharedPtr pub_over_stop_velocity_;
rclcpp::Subscription<Odometry>::SharedPtr sub_current_odometry_;
rclcpp::Subscription<AccelWithCovarianceStamped>::SharedPtr sub_current_acceleration_;
Expand All @@ -95,6 +98,7 @@ class MotionVelocitySmootherNode : public rclcpp::Node
double max_velocity_with_deceleration_; // maximum velocity with deceleration
// for external velocity limit
double wheelbase_; // wheelbase
double base_link2front_; // base_link to front

TrajectoryPoints prev_output_; // previously published trajectory

Expand Down Expand Up @@ -150,6 +154,7 @@ class MotionVelocitySmootherNode : public rclcpp::Node
{
double velocity{0.0}; // current external_velocity_limit
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
14 changes: 14 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
Expand Up @@ -14,6 +14,7 @@

#include "motion_velocity_smoother/motion_velocity_smoother_node.hpp"

#include "motion_utils/marker/marker_helper.hpp"
#include "motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp"
#include "motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp"
#include "motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp"
Expand Down Expand Up @@ -41,6 +42,7 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions
// set common params
const auto vehicle_info = VehicleInfoUtil(*this).getVehicleInfo();
wheelbase_ = vehicle_info.wheel_base_m;
base_link2front_ = vehicle_info.max_longitudinal_offset_m;
initCommonParam();
over_stop_velocity_warn_thr_ = declare_parameter<double>("over_stop_velocity_warn_thr");

Expand All @@ -49,6 +51,7 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions

// publishers, subscribers
pub_trajectory_ = create_publisher<Trajectory>("~/output/trajectory", 1);
pub_virtual_wall_ = create_publisher<MarkerArray>("~/virtual_wall", 1);
pub_velocity_limit_ = create_publisher<VelocityLimit>(
"~/output/current_velocity_limit_mps", rclcpp::QoS{1}.transient_local());
pub_dist_to_stopline_ = create_publisher<Float32Stamped>("~/distance_to_stopline", 1);
Expand Down Expand Up @@ -323,6 +326,9 @@ void MotionVelocitySmootherNode::calcExternalVelocityLimit()
return;
}

// sender
external_velocity_limit_.sender = external_velocity_limit_ptr_->sender;

// on the first time, apply directly
if (prev_output_.empty() || !current_closest_point_from_prev_output_) {
external_velocity_limit_.velocity = external_velocity_limit_ptr_->max_velocity;
Expand Down Expand Up @@ -879,6 +885,14 @@ void MotionVelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & t
trajectory_utils::applyMaximumVelocityLimit(
*inserted_index, traj.size(), external_velocity_limit_.velocity, traj);

// create virtual wall
if (std::abs(external_velocity_limit_.velocity) < 1e-3) {
const auto virtual_wall_marker = motion_utils::createStopVirtualWallMarker(
traj.at(*inserted_index).pose, external_velocity_limit_.sender, this->now(), 0,
base_link2front_);
pub_virtual_wall_->publish(virtual_wall_marker);
}

RCLCPP_DEBUG(
get_logger(), "externalVelocityLimit : limit_vel = %.3f", external_velocity_limit_.velocity);
}
Expand Down
Loading