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.

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

refactor(tier4_perception_msgs): rename traffic_signal to traffic_light #6375

Merged
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
Original file line number Diff line number Diff line change
Expand Up @@ -17,9 +17,9 @@

#include "autoware_perception_msgs/msg/traffic_signal.hpp"
#include "autoware_perception_msgs/msg/traffic_signal_element.hpp"
#include "tier4_perception_msgs/msg/traffic_light.hpp"
#include "tier4_perception_msgs/msg/traffic_light_element.hpp"
#include "tier4_perception_msgs/msg/traffic_light_roi.hpp"
#include "tier4_perception_msgs/msg/traffic_signal.hpp"

#include <lanelet2_core/primitives/Lanelet.h>
#include <lanelet2_core/primitives/LineString.h>
Expand All @@ -35,9 +35,9 @@ bool isRoiValid(

void setRoiInvalid(tier4_perception_msgs::msg::TrafficLightRoi & roi);

bool isSignalUnknown(const tier4_perception_msgs::msg::TrafficSignal & signal);
bool isSignalUnknown(const tier4_perception_msgs::msg::TrafficLight & signal);

void setSignalUnknown(tier4_perception_msgs::msg::TrafficSignal & signal, float confidence = -1);
void setSignalUnknown(tier4_perception_msgs::msg::TrafficLight & signal, float confidence = -1);

/**
* @brief Checks if a traffic light state includes a circle-shaped light with the specified color.
Expand Down
4 changes: 2 additions & 2 deletions common/traffic_light_utils/src/traffic_light_utils.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -33,14 +33,14 @@ void setRoiInvalid(tier4_perception_msgs::msg::TrafficLightRoi & roi)
roi.roi.height = roi.roi.width = 0;
}

bool isSignalUnknown(const tier4_perception_msgs::msg::TrafficSignal & signal)
bool isSignalUnknown(const tier4_perception_msgs::msg::TrafficLight & signal)
{
return signal.elements.size() == 1 &&
signal.elements[0].shape == tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN &&
signal.elements[0].color == tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN;
}

void setSignalUnknown(tier4_perception_msgs::msg::TrafficSignal & signal, float confidence)
void setSignalUnknown(tier4_perception_msgs::msg::TrafficLight & signal, float confidence)
{
signal.elements.resize(1);
signal.elements[0].shape = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN;
Expand Down
4 changes: 2 additions & 2 deletions common/traffic_light_utils/test/test_traffic_light_utils.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@ TEST(setRoiInvalid, set_roi_size)

TEST(isSignalUnknown, signal_element)
{
tier4_perception_msgs::msg::TrafficSignal test_signal;
tier4_perception_msgs::msg::TrafficLight test_signal;
tier4_perception_msgs::msg::TrafficLightElement element;
element.color = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN;
element.shape = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN;
Expand All @@ -60,7 +60,7 @@ TEST(isSignalUnknown, signal_element)

TEST(setSignalUnknown, set_signal_element)
{
tier4_perception_msgs::msg::TrafficSignal test_signal;
tier4_perception_msgs::msg::TrafficLight test_signal;
tier4_perception_msgs::msg::TrafficLightElement element;
element.color = tier4_perception_msgs::msg::TrafficLightElement::RED;
element.shape = tier4_perception_msgs::msg::TrafficLightElement::CROSS;
Expand Down
16 changes: 8 additions & 8 deletions perception/crosswalk_traffic_light_estimator/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -8,17 +8,17 @@

### Input

| Name | Type | Description |
| ------------------------------------ | ------------------------------------------------ | ------------------ |
| `~/input/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | vector map |
| `~/input/route` | `autoware_planning_msgs::msg::LaneletRoute` | route |
| `~/input/classified/traffic_signals` | `tier4_perception_msgs::msg::TrafficSignalArray` | classified signals |
| Name | Type | Description |
| ------------------------------------ | --------------------------------------------------- | ------------------ |
| `~/input/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | vector map |
| `~/input/route` | `autoware_planning_msgs::msg::LaneletRoute` | route |
| `~/input/classified/traffic_signals` | `autoware_perception_msgs::msg::TrafficSignalArray` | classified signals |

### Output

| Name | Type | Description |
| -------------------------- | ------------------------------------------------ | --------------------------------------------------------- |
| `~/output/traffic_signals` | `tier4_perception_msgs::msg::TrafficSignalArray` | output that contains estimated pedestrian traffic signals |
| Name | Type | Description |
| -------------------------- | --------------------------------------------------- | --------------------------------------------------------- |
| `~/output/traffic_signals` | `autoware_perception_msgs::msg::TrafficSignalArray` | output that contains estimated pedestrian traffic signals |

## Parameters

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -23,9 +23,6 @@
#include <autoware_perception_msgs/msg/traffic_signal_array.hpp>
#include <autoware_planning_msgs/msg/lanelet_route.hpp>
#include <tier4_debug_msgs/msg/float64_stamped.hpp>
#include <tier4_perception_msgs/msg/traffic_light_element.hpp>
#include <tier4_perception_msgs/msg/traffic_signal.hpp>
#include <tier4_perception_msgs/msg/traffic_signal_array.hpp>

#include <lanelet2_core/Attribute.h>
#include <lanelet2_core/LaneletMap.h>
Expand Down
8 changes: 4 additions & 4 deletions perception/traffic_light_classifier/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -43,10 +43,10 @@ These colors and shapes are assigned to the message as follows:

### Output

| Name | Type | Description |
| -------------------------- | ------------------------------------------------ | ------------------- |
| `~/output/traffic_signals` | `tier4_perception_msgs::msg::TrafficSignalArray` | classified signals |
| `~/output/debug/image` | `sensor_msgs::msg::Image` | image for debugging |
| Name | Type | Description |
| -------------------------- | ----------------------------------------------- | ------------------- |
| `~/output/traffic_signals` | `tier4_perception_msgs::msg::TrafficLightArray` | classified signals |
| `~/output/debug/image` | `sensor_msgs::msg::Image` | image for debugging |

## Parameters

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <tier4_perception_msgs/msg/traffic_signal_array.hpp>
#include <tier4_perception_msgs/msg/traffic_light_array.hpp>

#include <vector>

Expand All @@ -29,7 +29,7 @@ class ClassifierInterface
public:
virtual bool getTrafficSignals(
const std::vector<cv::Mat> & input_image,
tier4_perception_msgs::msg::TrafficSignalArray & traffic_signals) = 0;
tier4_perception_msgs::msg::TrafficLightArray & traffic_signals) = 0;
};
} // namespace traffic_light

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -57,14 +57,14 @@ class CNNClassifier : public ClassifierInterface

bool getTrafficSignals(
const std::vector<cv::Mat> & images,
tier4_perception_msgs::msg::TrafficSignalArray & traffic_signals) override;
tier4_perception_msgs::msg::TrafficLightArray & traffic_signals) override;

private:
void postProcess(int cls, float prob, tier4_perception_msgs::msg::TrafficSignal & traffic_signal);
void postProcess(int cls, float prob, tier4_perception_msgs::msg::TrafficLight & traffic_signal);
bool readLabelfile(std::string filepath, std::vector<std::string> & labels);
bool isColorLabel(const std::string label);
void outputDebugImage(
cv::Mat & debug_image, const tier4_perception_msgs::msg::TrafficSignal & traffic_signal);
cv::Mat & debug_image, const tier4_perception_msgs::msg::TrafficLight & traffic_signal);

private:
std::map<int, std::string> state2label_{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@
#include <opencv2/highgui/highgui.hpp>
#include <rclcpp/rclcpp.hpp>

#include <tier4_perception_msgs/msg/traffic_signal_array.hpp>
#include <tier4_perception_msgs/msg/traffic_light_array.hpp>

#if __has_include(<cv_bridge/cv_bridge.hpp>)
#include <cv_bridge/cv_bridge.hpp>
Expand Down Expand Up @@ -64,7 +64,7 @@ class ColorClassifier : public ClassifierInterface

bool getTrafficSignals(
const std::vector<cv::Mat> & images,
tier4_perception_msgs::msg::TrafficSignalArray & traffic_signals) override;
tier4_perception_msgs::msg::TrafficLightArray & traffic_signals) override;

private:
bool filterHSV(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -24,10 +24,10 @@
#include <sensor_msgs/image_encodings.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <std_msgs/msg/header.hpp>
#include <tier4_perception_msgs/msg/traffic_light.hpp>
#include <tier4_perception_msgs/msg/traffic_light_array.hpp>
#include <tier4_perception_msgs/msg/traffic_light_element.hpp>
#include <tier4_perception_msgs/msg/traffic_light_roi_array.hpp>
#include <tier4_perception_msgs/msg/traffic_signal.hpp>
#include <tier4_perception_msgs/msg/traffic_signal_array.hpp>

#if __has_include(<cv_bridge/cv_bridge.hpp>)
#include <cv_bridge/cv_bridge.hpp>
Expand Down Expand Up @@ -85,7 +85,7 @@ class TrafficLightClassifierNodelet : public rclcpp::Node
typedef message_filters::Synchronizer<ApproximateSyncPolicy> ApproximateSync;
std::shared_ptr<ApproximateSync> approximate_sync_;
bool is_approximate_sync_;
rclcpp::Publisher<tier4_perception_msgs::msg::TrafficSignalArray>::SharedPtr
rclcpp::Publisher<tier4_perception_msgs::msg::TrafficLightArray>::SharedPtr
traffic_signal_array_pub_;
std::shared_ptr<ClassifierInterface> classifier_ptr_;

Expand Down
8 changes: 4 additions & 4 deletions perception/traffic_light_classifier/src/cnn_classifier.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -65,7 +65,7 @@ CNNClassifier::CNNClassifier(rclcpp::Node * node_ptr) : node_ptr_(node_ptr)

bool CNNClassifier::getTrafficSignals(
const std::vector<cv::Mat> & images,
tier4_perception_msgs::msg::TrafficSignalArray & traffic_signals)
tier4_perception_msgs::msg::TrafficLightArray & traffic_signals)
{
if (images.size() != traffic_signals.signals.size()) {
RCLCPP_WARN(node_ptr_->get_logger(), "image number should be equal to traffic signal number!");
Expand Down Expand Up @@ -106,7 +106,7 @@ bool CNNClassifier::getTrafficSignals(
}

void CNNClassifier::outputDebugImage(
cv::Mat & debug_image, const tier4_perception_msgs::msg::TrafficSignal & traffic_signal)
cv::Mat & debug_image, const tier4_perception_msgs::msg::TrafficLight & traffic_signal)
{
float probability;
std::string label;
Expand Down Expand Up @@ -138,7 +138,7 @@ void CNNClassifier::outputDebugImage(
}

void CNNClassifier::postProcess(
int class_index, float prob, tier4_perception_msgs::msg::TrafficSignal & traffic_signal)
int class_index, float prob, tier4_perception_msgs::msg::TrafficLight & traffic_signal)
{
std::string match_label = labels_[class_index];

Expand Down Expand Up @@ -196,7 +196,7 @@ bool CNNClassifier::readLabelfile(std::string filepath, std::vector<std::string>

bool CNNClassifier::isColorLabel(const std::string label)
{
using tier4_perception_msgs::msg::TrafficSignal;
using tier4_perception_msgs::msg::TrafficLight;
if (
label == state2label_[tier4_perception_msgs::msg::TrafficLightElement::GREEN] ||
label == state2label_[tier4_perception_msgs::msg::TrafficLightElement::AMBER] ||
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ ColorClassifier::ColorClassifier(rclcpp::Node * node_ptr) : node_ptr_(node_ptr)

bool ColorClassifier::getTrafficSignals(
const std::vector<cv::Mat> & images,
tier4_perception_msgs::msg::TrafficSignalArray & traffic_signals)
tier4_perception_msgs::msg::TrafficLightArray & traffic_signals)
{
if (images.size() != traffic_signals.signals.size()) {
RCLCPP_WARN(node_ptr_->get_logger(), "image number should be equal to traffic signal number!");
Expand Down
9 changes: 4 additions & 5 deletions perception/traffic_light_classifier/src/nodelet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -42,9 +42,8 @@ TrafficLightClassifierNodelet::TrafficLightClassifierNodelet(const rclcpp::NodeO
std::bind(&TrafficLightClassifierNodelet::imageRoiCallback, this, _1, _2));
}

traffic_signal_array_pub_ =
this->create_publisher<tier4_perception_msgs::msg::TrafficSignalArray>(
"~/output/traffic_signals", rclcpp::QoS{1});
traffic_signal_array_pub_ = this->create_publisher<tier4_perception_msgs::msg::TrafficLightArray>(
"~/output/traffic_signals", rclcpp::QoS{1});

using std::chrono_literals::operator""ms;
timer_ = rclcpp::create_timer(
Expand Down Expand Up @@ -95,7 +94,7 @@ void TrafficLightClassifierNodelet::imageRoiCallback(
input_image_msg->encoding.c_str());
}

tier4_perception_msgs::msg::TrafficSignalArray output_msg;
tier4_perception_msgs::msg::TrafficLightArray output_msg;

output_msg.signals.resize(input_rois_msg->rois.size());

Expand Down Expand Up @@ -131,7 +130,7 @@ void TrafficLightClassifierNodelet::imageRoiCallback(
// append the undetected rois as unknown
for (const auto & input_roi : input_rois_msg->rois) {
if (input_roi.roi.height == 0 && input_roi.traffic_light_type == classify_traffic_light_type_) {
tier4_perception_msgs::msg::TrafficSignal tlr_sig;
tier4_perception_msgs::msg::TrafficLight tlr_sig;
tlr_sig.traffic_light_id = input_roi.traffic_light_id;
tlr_sig.traffic_light_type = input_roi.traffic_light_type;
tier4_perception_msgs::msg::TrafficLightElement element;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -123,7 +123,7 @@ class SingleImageDebugInferenceNode : public rclcpp::Node
return;
}
cv::cvtColor(crop, crop, cv::COLOR_BGR2RGB);
tier4_perception_msgs::msg::TrafficSignalArray traffic_signal;
tier4_perception_msgs::msg::TrafficLightArray traffic_signal;
if (!classifier_ptr_->getTrafficSignals({crop}, traffic_signal)) {
RCLCPP_ERROR(get_logger(), "failed to classify image");
return;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -20,8 +20,8 @@
#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
#include <autoware_perception_msgs/msg/traffic_signal_array.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <tier4_perception_msgs/msg/traffic_light_array.hpp>
#include <tier4_perception_msgs/msg/traffic_light_roi_array.hpp>
#include <tier4_perception_msgs/msg/traffic_signal_array.hpp>

#include <lanelet2_core/Forward.h>
#include <message_filters/subscriber.h>
Expand All @@ -46,15 +46,15 @@ struct FusionRecord
std_msgs::msg::Header header;
sensor_msgs::msg::CameraInfo cam_info;
tier4_perception_msgs::msg::TrafficLightRoi roi;
tier4_perception_msgs::msg::TrafficSignal signal;
tier4_perception_msgs::msg::TrafficLight signal;
};

struct FusionRecordArr
{
std_msgs::msg::Header header;
sensor_msgs::msg::CameraInfo cam_info;
tier4_perception_msgs::msg::TrafficLightRoiArray rois;
tier4_perception_msgs::msg::TrafficSignalArray signals;
tier4_perception_msgs::msg::TrafficLightArray signals;
};

bool operator<(const FusionRecordArr & r1, const FusionRecordArr & r2)
Expand All @@ -67,8 +67,8 @@ class MultiCameraFusion : public rclcpp::Node
public:
typedef sensor_msgs::msg::CameraInfo CamInfoType;
typedef tier4_perception_msgs::msg::TrafficLightRoi RoiType;
typedef tier4_perception_msgs::msg::TrafficSignal SignalType;
typedef tier4_perception_msgs::msg::TrafficSignalArray SignalArrayType;
typedef tier4_perception_msgs::msg::TrafficLight SignalType;
typedef tier4_perception_msgs::msg::TrafficLightArray SignalArrayType;
typedef tier4_perception_msgs::msg::TrafficLightRoiArray RoiArrayType;
typedef tier4_perception_msgs::msg::TrafficLightRoi::_traffic_light_id_type IdType;
typedef autoware_perception_msgs::msg::TrafficSignal NewSignalType;
Expand Down
2 changes: 1 addition & 1 deletion perception/traffic_light_multi_camera_fusion/src/node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@
namespace
{

bool isUnknown(const tier4_perception_msgs::msg::TrafficSignal & signal)
bool isUnknown(const tier4_perception_msgs::msg::TrafficLight & signal)
{
return signal.elements.size() == 1 &&
signal.elements[0].color == tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN &&
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -24,8 +24,8 @@
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <tier4_perception_msgs/msg/traffic_light_array.hpp>
#include <tier4_perception_msgs/msg/traffic_light_roi_array.hpp>
#include <tier4_perception_msgs/msg/traffic_signal_array.hpp>

#include <image_geometry/pinhole_camera_model.h>
#include <message_filters/subscriber.h>
Expand Down Expand Up @@ -69,7 +69,7 @@ class TrafficLightOcclusionPredictorNodelet : public rclcpp::Node
*
*/
void syncCallback(
const tier4_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr in_signal_msg,
const tier4_perception_msgs::msg::TrafficLightArray::ConstSharedPtr in_signal_msg,
const tier4_perception_msgs::msg::TrafficLightRoiArray::ConstSharedPtr in_roi_msg,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr in_cam_info_msg,
const sensor_msgs::msg::PointCloud2::ConstSharedPtr in_cloud_msg,
Expand All @@ -80,7 +80,7 @@ class TrafficLightOcclusionPredictorNodelet : public rclcpp::Node
* @brief publishers
*
*/
rclcpp::Publisher<tier4_perception_msgs::msg::TrafficSignalArray>::SharedPtr signal_pub_;
rclcpp::Publisher<tier4_perception_msgs::msg::TrafficLightArray>::SharedPtr signal_pub_;

tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_;
Expand All @@ -92,17 +92,16 @@ class TrafficLightOcclusionPredictorNodelet : public rclcpp::Node
*/
std::shared_ptr<CloudOcclusionPredictor> cloud_occlusion_predictor_;
typedef perception_utils::PrimeSynchronizer<
tier4_perception_msgs::msg::TrafficSignalArray,
tier4_perception_msgs::msg::TrafficLightRoiArray, sensor_msgs::msg::CameraInfo,
sensor_msgs::msg::PointCloud2>
tier4_perception_msgs::msg::TrafficLightArray, tier4_perception_msgs::msg::TrafficLightRoiArray,
sensor_msgs::msg::CameraInfo, sensor_msgs::msg::PointCloud2>
SynchronizerType;

std::shared_ptr<SynchronizerType> synchronizer_;
std::shared_ptr<SynchronizerType> synchronizer_ped_;

std::vector<bool> subscribed_;
std::vector<int> occlusion_ratios_;
tier4_perception_msgs::msg::TrafficSignalArray out_msg_;
tier4_perception_msgs::msg::TrafficLightArray out_msg_;
};
} // namespace traffic_light
#endif // TRAFFIC_LIGHT_OCCLUSION_PREDICTOR__NODELET_HPP_
Loading
Loading