Skip to content

Commit

Permalink
Merge branch 'main' into feat/keep_distance_against_front_objects
Browse files Browse the repository at this point in the history 
Signed-off-by: kyoichi-sugahara <[email protected]>
  • Loading branch information
@kyoichi-sugahara
kyoichi-sugahara committed Dec 30, 2023
2 parents 7fc8a64 + b076eb7 commit 8e4c33d
Show file tree
Hide file tree
Showing 20 changed files with 445 additions and 104 deletions.
2 changes: 1 addition & 1 deletion common/traffic_light_utils/src/traffic_light_utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,7 @@ void setSignalUnknown(tier4_perception_msgs::msg::TrafficSignal & signal, float
signal.elements[0].shape = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN;
signal.elements[0].color = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN;
// the default value is -1, which means to not set confidence
if (confidence >= 0) {
if (confidence > 0) {
signal.elements[0].confidence = confidence;
}
}
Expand Down
15 changes: 13 additions & 2 deletions launch/tier4_perception_launch/launch/perception.launch.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -86,13 +86,18 @@
default="$(var data_path)/traffic_light_fine_detector"
description="options: `tlr_yolox_s_batch_**`. The batch number must be either one of 1, 4, 6"
/>
<arg name="traffic_light_fine_detector_model_name" default="tlr_yolox_s_batch_6" description="options: `tlr_yolox_s_batch_**`. The batch number must be either one of 1, 4, 6"/>
<arg name="traffic_light_fine_detector_model_name" default="tlr_car_ped_yolox_s_batch_6" description="options: `tlr_car_ped_yolox_s_batch_**`. The batch number must be either one of 1, 4, 6"/>
<arg name="traffic_light_classifier_model_path" default="$(var data_path)/traffic_light_classifier" description="classifier onnx model path"/>
<arg
name="traffic_light_classifier_model_name"
name="car_traffic_light_classifier_model_name"
default="traffic_light_classifier_mobilenetv2_batch_6"
description="options: `traffic_light_classifier_mobilenetv2_batch_*` or `traffic_light_classifier_efficientNet_b1_batch_*`. The batch number must be either one of 1, 4, 6"
/>
<arg
name="pedestrian_traffic_light_classifier_model_name"
default="ped_traffic_light_classifier_mobilenetv2_batch_4"
description="options: `ped_traffic_light_classifier_mobilenetv2_batch_*` or `ped_traffic_light_classifier_efficientNet_b1_batch_*`. The batch number must be either one of 1, 4, 6"
/>

<!-- Camera-Lidar fusion parameters -->
<arg name="remove_unknown" default="true"/>
Expand Down Expand Up @@ -231,6 +236,12 @@
<arg name="enable_fine_detection" value="$(var traffic_light_recognition/enable_fine_detection)"/>
<arg name="fusion_only" value="$(var traffic_light_recognition/fusion_only)"/>
<arg name="image_number" value="$(var traffic_light_image_number)"/>
<arg name="traffic_light_arbiter_param_path" value="$(var traffic_light_arbiter_param_path)"/>
<arg name="traffic_light_fine_detector_model_path" value="$(var traffic_light_fine_detector_model_path)"/>
<arg name="traffic_light_fine_detector_model_name" value="$(var traffic_light_fine_detector_model_name)"/>
<arg name="traffic_light_classifier_model_path" value="$(var traffic_light_classifier_model_path)"/>
<arg name="car_traffic_light_classifier_model_name" value="$(var car_traffic_light_classifier_model_name)"/>
<arg name="pedestrian_traffic_light_classifier_model_name" value="$(var pedestrian_traffic_light_classifier_model_name)"/>
</include>
</group>
</group>
Expand Down
41 changes: 29 additions & 12 deletions launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,8 +7,10 @@
<arg name="traffic_light_fine_detector_label_name" default="tlr_labels.txt" description="fine detector label filename"/>
<arg name="traffic_light_fine_detector_model_name" default="tlr_yolox_s_batch_6" description="fine detector onnx model filename"/>
<arg name="traffic_light_classifier_model_path" default="$(find-pkg-share traffic_light_classifier)/data" description="classifier label path"/>
<arg name="traffic_light_classifier_label_name" default="lamp_labels.txt" description="classifier label filename"/>
<arg name="traffic_light_classifier_model_name" default="traffic_light_classifier_mobilenetv2_batch_6.onnx" description="classifier onnx model filename"/>
<arg name="car_traffic_light_classifier_label_name" default="lamp_labels.txt" description="classifier label filename"/>
<arg name="pedestrian_traffic_light_classifier_label_name" default="lamp_labels_ped.txt" description="classifier label filename"/>
<arg name="car_traffic_light_classifier_model_name" default="traffic_light_classifier_mobilenetv2_batch_6.onnx" description="classifier onnx model filename"/>
<arg name="pedestrian_traffic_light_classifier_model_name" default="ped_traffic_light_classifier_mobilenetv2_batch_6.onnx" description="classifier onnx model filename"/>
<arg name="input/cloud" default="/sensing/lidar/top/pointcloud_raw" description="point cloud for occlusion prediction"/>
<arg name="internal/traffic_signals" default="/perception/traffic_light_recognition/internal/traffic_signals"/>
<arg name="fusion/traffic_signals" default="/perception/traffic_light_recognition/fusion/traffic_signals"/>
Expand All @@ -24,8 +26,10 @@

<arg name="fine_detector_label_path" default="$(var traffic_light_fine_detector_model_path)/$(var traffic_light_fine_detector_label_name)"/>
<arg name="fine_detector_model_path" default="$(var traffic_light_fine_detector_model_path)/$(var traffic_light_fine_detector_model_name).onnx"/>
<arg name="classifier_label_path" default="$(var traffic_light_classifier_model_path)/$(var traffic_light_classifier_label_name)"/>
<arg name="classifier_model_path" default="$(var traffic_light_classifier_model_path)/$(var traffic_light_classifier_model_name).onnx"/>
<arg name="car_classifier_label_path" default="$(var traffic_light_classifier_model_path)/$(var car_traffic_light_classifier_label_name)"/>
<arg name="pedestrian_classifier_label_path" default="$(var traffic_light_classifier_model_path)/$(var pedestrian_traffic_light_classifier_label_name)"/>
<arg name="car_classifier_model_path" default="$(var traffic_light_classifier_model_path)/$(var car_traffic_light_classifier_model_name).onnx"/>
<arg name="pedestrian_classifier_model_path" default="$(var traffic_light_classifier_model_path)/$(var pedestrian_traffic_light_classifier_model_name).onnx"/>

<!-- namespace1 camera TLR pipeline -->
<group>
Expand All @@ -34,6 +38,8 @@
<let name="input/camera_info" value="/sensing/camera/$(var namespace1)/camera_info"/>
<let name="output/rois1" value="/perception/traffic_light_recognition/$(var namespace1)/detection/rois"/>
<let name="output/traffic_signals1" value="/perception/traffic_light_recognition/$(var namespace1)/classification/traffic_signals"/>
<let name="output/car/traffic_signals1" value="/perception/traffic_light_recognition/$(var namespace1)/classification/car/traffic_signals"/>
<let name="output/pedestrian/traffic_signals1" value="/perception/traffic_light_recognition/$(var namespace1)/classification/pedestrian/traffic_signals"/>
<let name="map_based_detector_output_topic" value="rough/rois" if="$(var enable_fine_detection)"/>
<let name="map_based_detector_output_topic" value="$(var output/rois1)" unless="$(var enable_fine_detection)"/>

Expand All @@ -54,15 +60,18 @@
<arg name="input/image" value="$(var input/image)"/>
<arg name="enable_image_decompressor" value="$(var enable_image_decompressor)"/>
<arg name="enable_fine_detection" value="$(var enable_fine_detection)"/>
<arg name="use_crosswalk_traffic_light_estimator" value="$(var use_crosswalk_traffic_light_estimator)"/>
<arg name="use_intra_process" value="true"/>
<arg name="use_multithread" value="true"/>
<arg name="fine_detector_label_path" value="$(var fine_detector_label_path)"/>
<arg name="fine_detector_model_path" value="$(var fine_detector_model_path)"/>
<arg name="classifier_label_path" value="$(var classifier_label_path)"/>
<arg name="classifier_model_path" value="$(var classifier_model_path)"/>
<arg name="car_classifier_label_path" value="$(var car_classifier_label_path)"/>
<arg name="pedestrian_classifier_label_path" value="$(var pedestrian_classifier_label_path)"/>
<arg name="car_classifier_model_path" value="$(var car_classifier_model_path)"/>
<arg name="pedestrian_classifier_model_path" value="$(var pedestrian_classifier_model_path)"/>
<arg name="output/rois" value="$(var output/rois1)"/>
<arg name="output/traffic_signals" value="$(var output/traffic_signals1)"/>
<arg name="output/car/traffic_signals" value="$(var output/car/traffic_signals1)"/>
<arg name="output/pedestrian/traffic_signals" value="$(var output/pedestrian/traffic_signals1)"/>
</include>
</group>

Expand All @@ -72,18 +81,22 @@
<arg name="input/camera_info" value="$(var input/camera_info)"/>
<arg name="input/cloud" value="$(var input/cloud)"/>
<arg name="input/rois" value="$(var output/rois1)"/>
<arg name="input/traffic_signals" value="classified/traffic_signals"/>
<arg name="input/car/traffic_signals" value="classified/car/traffic_signals"/>
<arg name="input/pedestrian/traffic_signals" value="classified/pedestrian/traffic_signals"/>
<arg name="output/traffic_signals" value="$(var output/traffic_signals1)"/>
</include>
</group>
</group>

<!-- namespace2 camera TLR pipeline -->
<group if="$(eval &quot; '$(var image_number)' >= '2' &quot;)">
<push-ros-namespace namespace="$(var namespace2)"/>
<let name="input/image" value="/sensing/camera/$(var namespace2)/image_raw"/>
<let name="input/camera_info" value="/sensing/camera/$(var namespace2)/camera_info"/>
<let name="output/rois2" value="/perception/traffic_light_recognition/$(var namespace2)/detection/rois"/>
<let name="output/traffic_signals2" value="/perception/traffic_light_recognition/$(var namespace2)/classification/traffic_signals"/>
<let name="output/car/traffic_signals2" value="/perception/traffic_light_recognition/$(var namespace2)/classification/car/traffic_signals"/>
<let name="output/pedestrian/traffic_signals2" value="/perception/traffic_light_recognition/$(var namespace2)/classification/pedestrian/traffic_signals"/>
<let name="map_based_detector_output_topic" value="rough/rois" if="$(var enable_fine_detection)"/>
<let name="map_based_detector_output_topic" value="rois" unless="$(var enable_fine_detection)"/>

Expand All @@ -104,15 +117,18 @@
<arg name="input/image" value="$(var input/image)"/>
<arg name="enable_image_decompressor" value="$(var enable_image_decompressor)"/>
<arg name="enable_fine_detection" value="$(var enable_fine_detection)"/>
<arg name="use_crosswalk_traffic_light_estimator" value="$(var use_crosswalk_traffic_light_estimator)"/>
<arg name="use_intra_process" value="true"/>
<arg name="use_multithread" value="true"/>
<arg name="fine_detector_label_path" value="$(var fine_detector_label_path)"/>
<arg name="fine_detector_model_path" value="$(var fine_detector_model_path)"/>
<arg name="classifier_label_path" value="$(var classifier_label_path)"/>
<arg name="classifier_model_path" value="$(var classifier_model_path)"/>
<arg name="car_classifier_label_path" value="$(var car_classifier_label_path)"/>
<arg name="pedestrian_classifier_label_path" value="$(var pedestrian_classifier_label_path)"/>
<arg name="car_classifier_model_path" value="$(var car_classifier_model_path)"/>
<arg name="pedestrian_classifier_model_path" value="$(var pedestrian_classifier_model_path)"/>
<arg name="output/rois" value="$(var output/rois2)"/>
<arg name="output/traffic_signals" value="$(var output/traffic_signals2)"/>
<arg name="output/car/traffic_signals" value="$(var output/car/traffic_signals2)"/>
<arg name="output/pedestrian/traffic_signals" value="$(var output/pedestrian/traffic_signals2)"/>
</include>
</group>

Expand All @@ -122,7 +138,8 @@
<arg name="input/camera_info" value="$(var input/camera_info)"/>
<arg name="input/cloud" value="$(var input/cloud)"/>
<arg name="input/rois" value="$(var output/rois2)"/>
<arg name="input/traffic_signals" value="classified/traffic_signals"/>
<arg name="input/car/traffic_signals" value="classified/car/traffic_signals"/>
<arg name="input/pedestrian/traffic_signals" value="classified/pedestrian/traffic_signals"/>
<arg name="output/traffic_signals" value="$(var output/traffic_signals2)"/>
</include>
</group>
Expand Down
80 changes: 62 additions & 18 deletions ...perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,13 @@ def add_launch_arg(name: str, default_value=None, description=None):
"output/traffic_signals",
"/perception/traffic_light_recognition/traffic_signals",
)
add_launch_arg(
"output/car/traffic_signals", "/perception/traffic_light_recognition/car/traffic_signals"
)
add_launch_arg(
"output/pedestrian/traffic_signals",
"/perception/traffic_light_recognition/pedestrian/traffic_signals",
)

# traffic_light_fine_detector
add_launch_arg(
Expand All @@ -64,16 +71,21 @@ def add_launch_arg(name: str, default_value=None, description=None):
# traffic_light_classifier
add_launch_arg("classifier_type", "1")
add_launch_arg(
"classifier_model_path",
"car_classifier_model_path",
os.path.join(classifier_share_dir, "data", "traffic_light_classifier_efficientNet_b1.onnx"),
)
add_launch_arg(
"pedestrian_classifier_model_path",
os.path.join(
classifier_share_dir,
"data",
"traffic_light_classifier_efficientNet_b1.onnx",
classifier_share_dir, "data", "pedestrian_traffic_light_classifier_efficientNet_b1.onnx"
),
)
add_launch_arg(
"classifier_label_path",
os.path.join(classifier_share_dir, "data", "lamp_labels.txt"),
"car_classifier_label_path", os.path.join(classifier_share_dir, "data", "lamp_labels.txt")
)
add_launch_arg(
"pedestrian_classifier_label_path",
os.path.join(classifier_share_dir, "data", "lamp_labels_ped.txt"),
)
add_launch_arg("classifier_precision", "fp16")
add_launch_arg("classifier_mean", "[123.675, 116.28, 103.53]")
Expand All @@ -98,24 +110,56 @@ def create_parameter_dict(*args):
ComposableNode(
package="traffic_light_classifier",
plugin="traffic_light::TrafficLightClassifierNodelet",
name="traffic_light_classifier",
name="car_traffic_light_classifier",
namespace="classification",
parameters=[
{
"approximate_sync": LaunchConfiguration("approximate_sync"),
"classifier_type": LaunchConfiguration("classifier_type"),
"classify_traffic_light_type": 0,
"classifier_model_path": LaunchConfiguration("car_classifier_model_path"),
"classifier_label_path": LaunchConfiguration("car_classifier_label_path"),
"classifier_precision": LaunchConfiguration("classifier_precision"),
"classifier_mean": LaunchConfiguration("classifier_mean"),
"classifier_std": LaunchConfiguration("classifier_std"),
"backlight_threshold": LaunchConfiguration("backlight_threshold"),
}
],
remappings=[
("~/input/image", LaunchConfiguration("input/image")),
("~/input/rois", LaunchConfiguration("output/rois")),
("~/output/traffic_signals", "classified/car/traffic_signals"),
],
extra_arguments=[
{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
],
),
ComposableNode(
package="traffic_light_classifier",
plugin="traffic_light::TrafficLightClassifierNodelet",
name="pedestrian_traffic_light_classifier",
namespace="classification",
parameters=[
create_parameter_dict(
"approximate_sync",
"classifier_type",
"classifier_model_path",
"classifier_label_path",
"classifier_precision",
"classifier_mean",
"classifier_std",
"backlight_threshold",
)
{
"approximate_sync": LaunchConfiguration("approximate_sync"),
"classifier_type": LaunchConfiguration("classifier_type"),
"classify_traffic_light_type": 1,
"classifier_model_path": LaunchConfiguration(
"pedestrian_classifier_model_path"
),
"classifier_label_path": LaunchConfiguration(
"pedestrian_classifier_label_path"
),
"classifier_precision": LaunchConfiguration("classifier_precision"),
"classifier_mean": LaunchConfiguration("classifier_mean"),
"classifier_std": LaunchConfiguration("classifier_std"),
"backlight_threshold": LaunchConfiguration("backlight_threshold"),
}
],
remappings=[
("~/input/image", LaunchConfiguration("input/image")),
("~/input/rois", LaunchConfiguration("output/rois")),
("~/output/traffic_signals", "classified/traffic_signals"),
("~/output/traffic_signals", "classified/pedestrian/traffic_signals"),
],
extra_arguments=[
{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
Expand Down
1 change: 1 addition & 0 deletions ...ion/crosswalk_traffic_light_estimator/config/crosswalk_traffic_light_estimator.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,3 +16,4 @@
ros__parameters:
use_last_detect_color: true
last_detect_color_hold_time: 2.0
last_colors_hold_time: 1.0
16 changes: 14 additions & 2 deletions ...tion/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,11 +34,11 @@
#include <lanelet2_routing/RoutingGraphContainer.h>
#include <lanelet2_traffic_rules/TrafficRulesFactory.h>

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

namespace traffic_light
{

Expand All @@ -53,6 +53,8 @@ using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement
using TrafficSignalAndTime = std::pair<TrafficSignal, rclcpp::Time>;
using TrafficLightIdMap = std::unordered_map<lanelet::Id, TrafficSignalAndTime>;

using TrafficLightIdArray = std::unordered_map<lanelet::Id, std::vector<TrafficSignalAndTime>>;

class CrosswalkTrafficLightEstimatorNode : public rclcpp::Node
{
public:
Expand All @@ -76,8 +78,12 @@ class CrosswalkTrafficLightEstimatorNode : public rclcpp::Node
void onTrafficLightArray(const TrafficSignalArray::ConstSharedPtr msg);

void updateLastDetectedSignal(const TrafficLightIdMap & traffic_signals);
void updateLastDetectedSignals(const TrafficLightIdMap & traffic_signals);
void updateFlashingState(const TrafficSignal & signal);
uint8_t updateAndGetColorState(const TrafficSignal & signal);
void setCrosswalkTrafficSignal(
const lanelet::ConstLanelet & crosswalk, const uint8_t color, TrafficSignalArray & msg) const;
const lanelet::ConstLanelet & crosswalk, const uint8_t color, const TrafficSignalArray & msg,
TrafficSignalArray & output);

lanelet::ConstLanelets getNonRedLanelets(
const lanelet::ConstLanelets & lanelets, const TrafficLightIdMap & traffic_light_id_map) const;
Expand All @@ -97,9 +103,15 @@ class CrosswalkTrafficLightEstimatorNode : public rclcpp::Node
// Node param
bool use_last_detect_color_;
double last_detect_color_hold_time_;
double last_colors_hold_time_;

// Signal history
TrafficLightIdMap last_detect_color_;
TrafficLightIdArray last_colors_;

// State
std::map<lanelet::Id, bool> is_flashing_;
std::map<lanelet::Id, uint8_t> current_color_state_;

// Stop watch
StopWatch<std::chrono::milliseconds> stop_watch_;
Expand Down
Loading

0 comments on commit 8e4c33d

Please sign in to comment.