diff --git a/common/traffic_light_recognition_marker_publisher/CMakeLists.txt b/common/traffic_light_recognition_marker_publisher/CMakeLists.txt new file mode 100644 index 0000000000000..d028ab21475f7 --- /dev/null +++ b/common/traffic_light_recognition_marker_publisher/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.14) +project(traffic_light_recognition_marker_publisher) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/traffic_light_recognition_marker_publisher.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "TrafficLightRecognitionMarkerPublisher" + EXECUTABLE ${PROJECT_NAME}_node +) + +ament_auto_package(INSTALL_TO_SHARE launch) diff --git a/common/traffic_light_recognition_marker_publisher/Readme.md b/common/traffic_light_recognition_marker_publisher/Readme.md new file mode 100644 index 0000000000000..1cafc619ff6fb --- /dev/null +++ b/common/traffic_light_recognition_marker_publisher/Readme.md @@ -0,0 +1,40 @@ +# path_distance_calculator + +## Purpose + +This node publishes a marker array for visualizing traffic signal recognition results on Rviz. + +![sample_img](./images/traffic_light_recognition_visualization_sample.png) + +## Inner-workings / Algorithms + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ------------------------------------------------------- | -------------------------------------------------------- | ------------------------------------------------- | +| `/map/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | Vector map for getting traffic signal information | +| `/perception/traffic_light_recognition/traffic_signals` | `autoware_auto_perception_msgs::msg::TrafficSignalArray` | The result of traffic signal recognition | + +### Output + +| Name | Type | Description | +| -------------------------------------------------------------- | -------------------------------------- | ------------------------------------------------------------------------------ | +| `/perception/traffic_light_recognition/traffic_signals_marker` | `visualization_msgs::msg::MarkerArray` | Publish a marker array for visualization of traffic signal recognition results | + +## Parameters + +None. + +### Node Parameters + +None. + +### Core Parameters + +None. + +## Assumptions / Known limits + +TBD. diff --git a/common/traffic_light_recognition_marker_publisher/images/traffic_light_recognition_visualization_sample.png b/common/traffic_light_recognition_marker_publisher/images/traffic_light_recognition_visualization_sample.png new file mode 100644 index 0000000000000..a9a782e4a7012 Binary files /dev/null and b/common/traffic_light_recognition_marker_publisher/images/traffic_light_recognition_visualization_sample.png differ diff --git a/common/traffic_light_recognition_marker_publisher/launch/traffic_light_recognition_marker_publisher.launch.xml b/common/traffic_light_recognition_marker_publisher/launch/traffic_light_recognition_marker_publisher.launch.xml new file mode 100644 index 0000000000000..b982be34e77d3 --- /dev/null +++ b/common/traffic_light_recognition_marker_publisher/launch/traffic_light_recognition_marker_publisher.launch.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/common/traffic_light_recognition_marker_publisher/package.xml b/common/traffic_light_recognition_marker_publisher/package.xml new file mode 100644 index 0000000000000..d12022c01684e --- /dev/null +++ b/common/traffic_light_recognition_marker_publisher/package.xml @@ -0,0 +1,30 @@ + + + + traffic_light_recognition_marker_publisher + 0.0.0 + The traffic_light_recognition_marker_publisher package + Tomoya Kimura + Takeshi Miura + Shumpei Wakabayashi + Apache License 2.0 + + ament_cmake + autoware_cmake + + autoware_auto_mapping_msgs + autoware_auto_perception_msgs + geometry_msgs + lanelet2_extension + rclcpp + rclcpp_components + tier4_autoware_utils + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.cpp b/common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.cpp new file mode 100644 index 0000000000000..6076d3fa32f26 --- /dev/null +++ b/common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.cpp @@ -0,0 +1,164 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "traffic_light_recognition_marker_publisher.hpp" + +#include +#include +#include +#include +#include + +TrafficLightRecognitionMarkerPublisher::TrafficLightRecognitionMarkerPublisher( + const rclcpp::NodeOptions & options) +: Node("traffic_light_recognition_marker_publisher", options) +{ + using std::placeholders::_1; + + sub_map_ptr_ = this->create_subscription( + "~/input/lanelet2_map", rclcpp::QoS{1}.transient_local(), + std::bind(&TrafficLightRecognitionMarkerPublisher::onMap, this, _1)); + sub_tlr_ptr_ = this->create_subscription( + "~/input/traffic_signals", rclcpp::QoS{1}, + std::bind(&TrafficLightRecognitionMarkerPublisher::onTrafficSignalArray, this, _1)); + pub_marker_ptr_ = + this->create_publisher("~/output/marker", rclcpp::QoS{1}); +} + +void TrafficLightRecognitionMarkerPublisher::onMap(const HADMapBin::ConstSharedPtr msg_ptr) +{ + is_map_ready_ = false; + lanelet::LaneletMapPtr viz_lanelet_map(new lanelet::LaneletMap); + lanelet::utils::conversion::fromBinMsg(*msg_ptr, viz_lanelet_map); + lanelet::ConstLanelets lanelets = lanelet::utils::query::laneletLayer(viz_lanelet_map); + std::vector tl_regulatory_elements = + lanelet::utils::query::autowareTrafficLights(lanelets); + for (const auto & tl : tl_regulatory_elements) { + const auto tl_lights = tl->trafficLights(); + for (const auto & tl_light : tl_lights) { + if (tl_light.isLineString()) { + lanelet::ConstLineString3d tl_linestring = + static_cast(tl_light); + int32_t tl_id = static_cast(tl_light.id()); + Pose tl_pose; + tl_pose.position.x = (tl_linestring.front().x() + tl_linestring.back().x()) / 2; + tl_pose.position.y = (tl_linestring.front().y() + tl_linestring.back().y()) / 2; + tl_pose.position.z = tl_linestring.front().z() + 1.0; + tl_position_map_[tl_id] = tl_pose; + } + } + } + is_map_ready_ = true; +} + +void TrafficLightRecognitionMarkerPublisher::onTrafficSignalArray( + const TrafficSignalArray::ConstSharedPtr msg_ptr) +{ + if (!is_map_ready_) { + return; + } + MarkerArray markers; + marker_id = 0; + for (const auto & tl : msg_ptr->signals) { + if (tl_position_map_.count(tl.map_primitive_id) == 0) continue; + auto tl_position = tl_position_map_[tl.map_primitive_id]; + for (const auto tl_light : tl.lights) { + const auto marker = getTrafficLightMarker(tl_position, tl_light.color, tl_light.shape); + markers.markers.emplace_back(marker); + marker_id++; + tl_position.position.z += 0.5; + } + } + pub_marker_ptr_->publish(markers); +} + +visualization_msgs::msg::Marker TrafficLightRecognitionMarkerPublisher::getTrafficLightMarker( + const Pose & tl_pose, const uint8_t tl_color, const uint8_t tl_shape) +{ + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Time(); + marker.ns = "traffic_light_color"; + marker.id = marker_id; + marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + marker.lifetime = rclcpp::Duration::from_seconds(1.0); + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose = tl_pose; + marker.color = getTrafficLightColor(tl_color, tl_shape); + marker.scale.z = 0.5; + marker.frame_locked = false; + marker.text = getTrafficLightString(tl_color, tl_shape); + + return marker; +} + +std::string TrafficLightRecognitionMarkerPublisher::getTrafficLightString( + const uint8_t tl_color, const uint8_t tl_shape) +{ + if (tl_shape == TrafficLight::LEFT_ARROW) return "LEFT_ARROW"; + if (tl_shape == TrafficLight::RIGHT_ARROW) return "RIGHT_ARROW"; + if (tl_shape == TrafficLight::UP_ARROW) return "UP_ARROW"; + if (tl_shape == TrafficLight::DOWN_ARROW) return "DOWN_ARROW"; + if (tl_shape == TrafficLight::DOWN_LEFT_ARROW) return "DOWN_LEFT_ARROW"; + if (tl_shape == TrafficLight::DOWN_RIGHT_ARROW) return "DOWN_RIGHT_ARROW"; + + if (tl_color == TrafficLight::RED) return "RED"; + if (tl_color == TrafficLight::GREEN) return "GREEN"; + if (tl_color == TrafficLight::AMBER) return "AMBER"; + + if (tl_shape == TrafficLight::UNKNOWN) return "UNKNOWN"; + + return "UNKNOWN"; +} + +std_msgs::msg::ColorRGBA TrafficLightRecognitionMarkerPublisher::getTrafficLightColor( + const uint8_t tl_color, const uint8_t tl_shape) +{ + std_msgs::msg::ColorRGBA c; + c.r = 1.0f; + c.g = 1.0f; + c.b = 1.0f; + c.a = 0.999; + + if ( + tl_shape == TrafficLight::LEFT_ARROW || tl_shape == TrafficLight::RIGHT_ARROW || + tl_shape == TrafficLight::UP_ARROW || tl_shape == TrafficLight::DOWN_ARROW || + tl_shape == TrafficLight::DOWN_LEFT_ARROW || tl_shape == TrafficLight::DOWN_RIGHT_ARROW) { + return c; // white + } + + if (tl_color == TrafficLight::RED) { + c.r = 1.0f; + c.g = 0.0f; + c.b = 0.0f; + return c; // red + } + if (tl_color == TrafficLight::GREEN) { + c.r = 0.0f; + c.g = 1.0f; + c.b = 0.0f; + return c; // green + } + if (tl_color == TrafficLight::AMBER) { + c.r = 1.0f; + c.g = 1.0f; + c.b = 0.0f; + return c; // amber + } + + return c; // white +} + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(TrafficLightRecognitionMarkerPublisher) diff --git a/common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.hpp b/common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.hpp new file mode 100644 index 0000000000000..de9aea14b7e5a --- /dev/null +++ b/common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.hpp @@ -0,0 +1,62 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TRAFFIC_LIGHT_RECOGNITION_MARKER_PUBLISHER_HPP_ +#define TRAFFIC_LIGHT_RECOGNITION_MARKER_PUBLISHER_HPP_ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +class TrafficLightRecognitionMarkerPublisher : public rclcpp::Node +{ +public: + using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using TrafficSignalArray = autoware_auto_perception_msgs::msg::TrafficSignalArray; + using TrafficLight = autoware_auto_perception_msgs::msg::TrafficLight; + using MarkerArray = visualization_msgs::msg::MarkerArray; + using Pose = geometry_msgs::msg::Pose; + + explicit TrafficLightRecognitionMarkerPublisher(const rclcpp::NodeOptions & options); + +private: + rclcpp::Subscription::SharedPtr sub_map_ptr_; + rclcpp::Subscription::SharedPtr + sub_tlr_ptr_; + rclcpp::Publisher::SharedPtr pub_marker_ptr_; + + void onMap(const HADMapBin::ConstSharedPtr msg_ptr); + void onTrafficSignalArray(const TrafficSignalArray::ConstSharedPtr msg_ptr); + visualization_msgs::msg::Marker getTrafficLightMarker( + const Pose & tl_pose, const uint8_t tl_color, const uint8_t tl_shape); + + std::string getTrafficLightString(const uint8_t tl_color, const uint8_t tl_shape); + std_msgs::msg::ColorRGBA getTrafficLightColor(const uint8_t tl_color, const uint8_t tl_shape); + + std::map tl_position_map_; + bool is_map_ready_ = false; + int32_t marker_id = 0; +}; + +#endif // TRAFFIC_LIGHT_RECOGNITION_MARKER_PUBLISHER_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 5240146410103..b8d0ca6f58364 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -694,6 +694,11 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( return IntersectionModule::Indecisive{}; } + if (default_stop_line_idx == 0) { + RCLCPP_DEBUG(logger_, "stop line index is 0"); + return IntersectionModule::Indecisive{}; + } + const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; debug_data_.pass_judge_wall_pose = planning_utils::getAheadPose(pass_judge_line_idx, baselink2front, *path); @@ -774,7 +779,11 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( path->points.at(default_stop_line_idx).point.pose.position); const bool approached_stop_line = (std::fabs(dist_stopline) < planner_param_.common.stop_overshoot_margin); + const bool over_stop_line = (dist_stopline < 0.0); const bool is_stopped = planner_data_->isVehicleStopped(); + if (over_stop_line) { + before_creep_state_machine_.setState(StateMachine::State::GO); + } if (before_creep_state_machine_.getState() == StateMachine::State::GO) { if (has_collision) { return IntersectionModule::OccludedCollisionStop{