Skip to content

Commit

Permalink
feat(behavior_velocity_stop_line): replace autoware_motion_utils to a…
Browse files Browse the repository at this point in the history
…utoware_trajectory (#9299)

* feat(behavior_velocity_stop_line): replace autoware_motion_utils to autoware_trajectory

Signed-off-by: Y.Hisaki <[email protected]>

* update

Signed-off-by: Y.Hisaki <[email protected]>

---------

Signed-off-by: Y.Hisaki <[email protected]>
  • Loading branch information
yhisaki authored Nov 15, 2024
1 parent 3adfb07 commit 3b623fb
Show file tree
Hide file tree
Showing 10 changed files with 80 additions and 101 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,9 @@ class VelocityFactorInterface
const std::vector<PointType> & points, const Pose & curr_pose, const Pose & stop_pose,
const VelocityFactorStatus status, const std::string & detail = "");

void set(
const double & distance, const VelocityFactorStatus & status, const std::string & detail = "");

private:
VelocityFactorBehavior behavior_{VelocityFactor::UNKNOWN};
VelocityFactor velocity_factor_{};
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,15 @@ void VelocityFactorInterface::set(
velocity_factor_.detail = detail;
}

void VelocityFactorInterface::set(
const double & distance, const VelocityFactorStatus & status, const std::string & detail)
{
velocity_factor_.behavior = behavior_;
velocity_factor_.distance = static_cast<float>(distance);
velocity_factor_.status = status;
velocity_factor_.detail = detail;
}

template void VelocityFactorInterface::set<tier4_planning_msgs::msg::PathPointWithLaneId>(
const std::vector<tier4_planning_msgs::msg::PathPointWithLaneId> &, const Pose &, const Pose &,
const VelocityFactorStatus, const std::string &);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,7 @@ namespace autoware::trajectory::detail
*/
geometry_msgs::msg::Point to_point(const geometry_msgs::msg::Point & p);
geometry_msgs::msg::Point to_point(const geometry_msgs::msg::Pose & p);
geometry_msgs::msg::Point to_point(const Eigen::Ref<const Eigen::Vector2d> & p);
geometry_msgs::msg::Point to_point(const Eigen::Vector2d & p);
geometry_msgs::msg::Point to_point(const autoware_planning_msgs::msg::PathPoint & p);
geometry_msgs::msg::Point to_point(const tier4_planning_msgs::msg::PathPointWithLaneId & p);
geometry_msgs::msg::Point to_point(const lanelet::BasicPoint2d & p);
Expand Down
2 changes: 1 addition & 1 deletion common/autoware_trajectory/src/detail/util.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@ geometry_msgs::msg::Point to_point(const geometry_msgs::msg::Pose & p)
return p.position;
}

geometry_msgs::msg::Point to_point(const Eigen::Ref<const Eigen::Vector2d> & p)
geometry_msgs::msg::Point to_point(const Eigen::Vector2d & p)
{
geometry_msgs::msg::Point point;
point.x = p(0);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@
<depend>autoware_motion_utils</depend>
<depend>autoware_planning_msgs</depend>
<depend>autoware_route_handler</depend>
<depend>autoware_trajectory</depend>
<depend>eigen</depend>
<depend>geometry_msgs</depend>
<depend>pluginlib</depend>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,12 +12,12 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#include "autoware/behavior_velocity_planner_common/utilization/util.hpp"
#include "autoware/motion_utils/marker/virtual_wall_marker_creator.hpp"
#include "autoware/universe_utils/geometry/geometry.hpp"
#include "autoware/universe_utils/ros/marker_helper.hpp"
#include "scene.hpp"

#include <autoware/behavior_velocity_planner_common/utilization/util.hpp>
#include <autoware/motion_utils/marker/virtual_wall_marker_creator.hpp>
#include <autoware/universe_utils/geometry/geometry.hpp>
#include <autoware/universe_utils/ros/marker_helper.hpp>
#ifdef ROS_DISTRO_GALACTIC
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#else
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
#include "manager.hpp"

#include "autoware/universe_utils/ros/parameter.hpp"
#include "autoware_lanelet2_extension/utility/query.hpp"

#include <memory>
#include <set>
Expand Down Expand Up @@ -57,7 +58,7 @@ std::vector<StopLineWithLaneId> StopLineModuleManager::getStopLinesWithLaneIdOnP
}

for (const auto & stop_line : traffic_sign_reg_elem->refLines()) {
stop_lines_with_lane_id.push_back(std::make_pair(stop_line, lane_id));
stop_lines_with_lane_id.emplace_back(stop_line, lane_id);
}
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,11 +15,11 @@
#ifndef MANAGER_HPP_
#define MANAGER_HPP_

#include "autoware/behavior_velocity_planner_common/plugin_interface.hpp"
#include "autoware/behavior_velocity_planner_common/plugin_wrapper.hpp"
#include "autoware/behavior_velocity_planner_common/scene_module_interface.hpp"
#include "scene.hpp"

#include <autoware/behavior_velocity_planner_common/plugin_interface.hpp>
#include <autoware/behavior_velocity_planner_common/plugin_wrapper.hpp>
#include <autoware/behavior_velocity_planner_common/scene_module_interface.hpp>
#include <rclcpp/rclcpp.hpp>

#include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -14,96 +14,93 @@

#include "scene.hpp"

#include <autoware/behavior_velocity_planner_common/utilization/arc_lane_util.hpp>
#include <autoware/behavior_velocity_planner_common/utilization/util.hpp>
#include <autoware/motion_utils/trajectory/trajectory.hpp>
#include <autoware/universe_utils/system/time_keeper.hpp>
#include "autoware/behavior_velocity_planner_common/utilization/util.hpp"
#include "autoware/trajectory/path_point_with_lane_id.hpp"

#include <algorithm>
#include <vector>
#include <tier4_planning_msgs/msg/detail/path_with_lane_id__struct.hpp>

namespace autoware::behavior_velocity_planner
{
namespace bg = boost::geometry;

StopLineModule::StopLineModule(
const int64_t module_id, const size_t lane_id, const lanelet::ConstLineString3d & stop_line,
const PlannerParam & planner_param, const rclcpp::Logger logger,
const int64_t module_id, const size_t lane_id, lanelet::ConstLineString3d stop_line,
const PlannerParam & planner_param, const rclcpp::Logger & logger,
const rclcpp::Clock::SharedPtr clock)
: SceneModuleInterface(module_id, logger, clock),
lane_id_(lane_id),
stop_line_(stop_line),
stop_line_(std::move(stop_line)),
state_(State::APPROACH),
planner_param_(planner_param),
debug_data_()
{
velocity_factor_.init(PlanningBehavior::STOP_SIGN);
planner_param_ = planner_param;
}

bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason)
{
universe_utils::ScopedTimeTrack st(
std::string(__func__) + " (lane_id:=" + std::to_string(module_id_) + ")", *getTimeKeeper());
auto trajectory =
trajectory::Trajectory<tier4_planning_msgs::msg::PathPointWithLaneId>::Builder{}.build(
path->points);

if (!trajectory) {
return true;
}

debug_data_ = DebugData();
if (path->points.empty()) return true;
const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
debug_data_.base_link2front = base_link2front;
first_stop_path_point_distance_ = autoware::motion_utils::calcArcLength(path->points);
first_stop_path_point_distance_ = trajectory->length();
*stop_reason = planning_utils::initializeStopReason(StopReason::STOP_LINE);

const LineString2d stop_line = planning_utils::extendLine(
stop_line_[0], stop_line_[1], planner_data_->stop_line_extend_length);

time_keeper_->start_track("createTargetPoint");
// Calculate stop pose and insert index
const auto stop_point = arc_lane_utils::createTargetPoint(
*path, stop_line, planner_param_.stop_margin,
planner_data_->vehicle_info_.max_longitudinal_offset_m);
time_keeper_->end_track("createTargetPoint");
// Calculate intersection with stop line
const auto trajectory_stop_line_intersection =
trajectory->crossed(stop_line.front(), stop_line.back());

// If no collision found, do nothing
if (!stop_point) {
if (!trajectory_stop_line_intersection) {
RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 5000 /* ms */, "is no collision");
return true;
}

const auto stop_point_idx = stop_point->first;
auto stop_pose = stop_point->second;
const double stop_point_s =
*trajectory_stop_line_intersection -
(base_link2front + planner_param_.stop_margin); // consider vehicle length and stop margin

if (stop_point_s < 0.0) {
return true;
}

const auto stop_pose = trajectory->compute(stop_point_s);

/**
* @brief : calculate signed arc length consider stop margin from stop line
*
* |----------------------------|
* s---ego----------x--|--------g
*/
time_keeper_->start_track(
"calcSegmentIndexFromPointIndex & findEgoSegmentIndex & calcSignedArcLength");
const size_t stop_line_seg_idx = planning_utils::calcSegmentIndexFromPointIndex(
path->points, stop_pose.position, stop_point_idx);
const size_t current_seg_idx = findEgoSegmentIndex(path->points);
const double signed_arc_dist_to_stop_point = autoware::motion_utils::calcSignedArcLength(
path->points, planner_data_->current_odometry->pose.position, current_seg_idx,
stop_pose.position, stop_line_seg_idx);
time_keeper_->end_track(
"calcSegmentIndexFromPointIndex & findEgoSegmentIndex & calcSignedArcLength");
const double ego_on_trajectory_s =
trajectory->closest(planner_data_->current_odometry->pose.position);
const double signed_arc_dist_to_stop_point = stop_point_s - ego_on_trajectory_s;

switch (state_) {
case State::APPROACH: {
// Insert stop pose
planning_utils::insertStopPoint(stop_pose.position, stop_line_seg_idx, *path);
trajectory->longitudinal_velocity_mps.range(stop_point_s, trajectory->length()).set(0.0);

// Update first stop index
first_stop_path_point_distance_ = autoware::motion_utils::calcSignedArcLength(
path->points, 0, stop_pose.position, stop_line_seg_idx);
debug_data_.stop_pose = stop_pose;
// Update first stop path point distance
first_stop_path_point_distance_ = stop_point_s;
debug_data_.stop_pose = stop_pose.point.pose;

// Get stop point and stop factor
{
tier4_planning_msgs::msg::StopFactor stop_factor;
stop_factor.stop_pose = stop_pose;
stop_factor.stop_pose = stop_pose.point.pose;
stop_factor.stop_factor_points.push_back(getCenterOfStopLine(stop_line_));
planning_utils::appendStopReason(stop_factor, stop_reason);
velocity_factor_.set(
path->points, planner_data_->current_odometry->pose, stop_pose,
VelocityFactor::APPROACHING);
velocity_factor_.set(signed_arc_dist_to_stop_point, VelocityFactor::APPROACHING);
}

// Move to stopped state if stopped
Expand All @@ -125,34 +122,22 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop
}

case State::STOPPED: {
// Change state after vehicle departure
const auto stopped_pose = autoware::motion_utils::calcLongitudinalOffsetPose(
path->points, planner_data_->current_odometry->pose.position, 0.0);

if (!stopped_pose) {
break;
}

SegmentIndexWithPose ego_pos_on_path;
ego_pos_on_path.pose = stopped_pose.value();
ego_pos_on_path.index = findEgoSegmentIndex(path->points);

// Insert stop pose
planning_utils::insertStopPoint(ego_pos_on_path.pose.position, ego_pos_on_path.index, *path);

debug_data_.stop_pose = stop_pose;
trajectory->longitudinal_velocity_mps.range(ego_on_trajectory_s, trajectory->length())
.set(0.0);
const auto ego_pos_on_path = trajectory->compute(ego_on_trajectory_s).point.pose;
debug_data_.stop_pose = ego_pos_on_path;

// Get stop point and stop factor
{
tier4_planning_msgs::msg::StopFactor stop_factor;
stop_factor.stop_pose = ego_pos_on_path.pose;
stop_factor.stop_pose = ego_pos_on_path;
stop_factor.stop_factor_points.push_back(getCenterOfStopLine(stop_line_));
planning_utils::appendStopReason(stop_factor, stop_reason);
velocity_factor_.set(
path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::STOPPED);
velocity_factor_.set(signed_arc_dist_to_stop_point, VelocityFactor::STOPPED);
}

const auto elapsed_time = (clock_->now() - *stopped_time_).seconds();
const double elapsed_time = (clock_->now() - *stopped_time_).seconds();

if (planner_param_.stop_duration_sec < elapsed_time) {
RCLCPP_INFO(logger_, "STOPPED -> START");
Expand All @@ -175,6 +160,8 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop
}
}

path->points = trajectory->restore();

return true;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,23 +15,20 @@
#ifndef SCENE_HPP_
#define SCENE_HPP_

#include "autoware/behavior_velocity_planner_common/scene_module_interface.hpp"
#include "autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp"
#include "autoware/behavior_velocity_planner_common/utilization/util.hpp"

#include <memory>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>

#define EIGEN_MPL2_ONLY
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <autoware/behavior_velocity_planner_common/scene_module_interface.hpp>
#include <autoware/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp>
#include <autoware/behavior_velocity_planner_common/utilization/util.hpp>
#include <autoware_lanelet2_extension/utility/query.hpp>
#include <rclcpp/rclcpp.hpp>

#include <lanelet2_core/LaneletMap.h>
#include <lanelet2_routing/RoutingGraph.h>

namespace autoware::behavior_velocity_planner
{
Expand All @@ -42,24 +39,6 @@ class StopLineModule : public SceneModuleInterface
public:
enum class State { APPROACH, STOPPED, START };

struct SegmentIndexWithPose
{
size_t index;
geometry_msgs::msg::Pose pose;
};

struct SegmentIndexWithPoint2d
{
size_t index;
Point2d point;
};

struct SegmentIndexWithOffset
{
size_t index;
double offset;
};

struct DebugData
{
double base_link2front;
Expand All @@ -77,10 +56,9 @@ class StopLineModule : public SceneModuleInterface
bool show_stop_line_collision_check;
};

public:
StopLineModule(
const int64_t module_id, const size_t lane_id, const lanelet::ConstLineString3d & stop_line,
const PlannerParam & planner_param, const rclcpp::Logger logger,
const int64_t module_id, const size_t lane_id, lanelet::ConstLineString3d stop_line,
const PlannerParam & planner_param, const rclcpp::Logger & logger,
const rclcpp::Clock::SharedPtr clock);

bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override;
Expand Down

0 comments on commit 3b623fb

Please sign in to comment.