Skip to content

Commit

Permalink
fix: conflict
Browse files Browse the repository at this point in the history 
Signed-off-by: tzhong518 <[email protected]>
  • Loading branch information
@tzhong518
tzhong518 committed Dec 31, 2023
2 parents 8b65e59 + b076eb7 commit cd6fb1a
Show file tree
Hide file tree
Showing 28 changed files with 585 additions and 189 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
8 changes: 3 additions & 5 deletions control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -555,9 +555,7 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
return;
};

const auto info_throttle = [this](const auto & s) {
RCLCPP_INFO_SKIPFIRST_THROTTLE(logger_, *clock_, 5000, "%s", s);
};
const auto info_once = [this](const auto & s) { RCLCPP_INFO_ONCE(logger_, "%s", s); };

const bool is_under_control = m_current_operation_mode.is_autoware_control_enabled &&
m_current_operation_mode.mode == OperationModeState::AUTONOMOUS;
Expand Down Expand Up @@ -623,10 +621,10 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
if (m_control_state == ControlState::STOPPED) {
// -- debug print --
if (has_nonzero_target_vel && !departure_condition_from_stopped) {
info_throttle("target speed > 0, but departure condition is not met. Keep STOPPED.");
info_once("target speed > 0, but departure condition is not met. Keep STOPPED.");
}
if (has_nonzero_target_vel && keep_stopped_condition) {
info_throttle("target speed > 0, but keep stop condition is met. Keep STOPPED.");
info_once("target speed > 0, but keep stop condition is met. Keep STOPPED.");
}
// ---------------

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
Loading

0 comments on commit cd6fb1a

Please sign in to comment.