Skip to content

Commit

Permalink
feat!: replace autoware_auto_msgs with autoware_msgs for perception m…
Browse files Browse the repository at this point in the history
…odules (autowarefoundation#7245)

Signed-off-by: Ryohsuke Mitsudome <[email protected]>
Co-authored-by: Cynthia Liu <[email protected]>
Co-authored-by: NorahXiong <[email protected]>
Co-authored-by: beginningfan <[email protected]>
Co-authored-by: M. Fatih Cırıt <[email protected]>
Co-authored-by: Yukihito Saito <[email protected]>
  • Loading branch information
6 people authored and a-maumau committed Jun 7, 2024
1 parent ca8ca2f commit 8049571
Show file tree
Hide file tree
Showing 240 changed files with 1,225 additions and 1,265 deletions.
2 changes: 1 addition & 1 deletion perception/bytetrack/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@
<buildtool_export_depend>cudnn_cmake_module</buildtool_export_depend>
<buildtool_export_depend>tensorrt_cmake_module</buildtool_export_depend>

<depend>autoware_auto_perception_msgs</depend>
<depend>autoware_perception_msgs</depend>
<depend>cuda_utils</depend>
<depend>cv_bridge</depend>
<depend>eigen</depend>
Expand Down
6 changes: 3 additions & 3 deletions perception/bytetrack/src/bytetrack_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@
#include <bytetrack/bytetrack_node.hpp>
#include <rclcpp/qos.hpp>

#include "autoware_auto_perception_msgs/msg/object_classification.hpp"
#include "autoware_perception_msgs/msg/object_classification.hpp"

#include <rmw/qos_profiles.h>

Expand Down Expand Up @@ -62,7 +62,7 @@ void ByteTrackNode::on_connect()
void ByteTrackNode::on_rect(
const tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr msg)
{
using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
using Label = autoware_perception_msgs::msg::ObjectClassification;

tier4_perception_msgs::msg::DetectedObjectsWithFeature out_objects;
tier4_perception_msgs::msg::DynamicObjectArray out_objects_uuid;
Expand Down Expand Up @@ -97,7 +97,7 @@ void ByteTrackNode::on_rect(
object.feature.roi.height = static_cast<uint32_t>(output_height);
object.object.existence_probability = tracked_object.score;
object.object.classification.emplace_back(
autoware_auto_perception_msgs::build<Label>().label(tracked_object.type).probability(1.0f));
autoware_perception_msgs::build<Label>().label(tracked_object.type).probability(1.0f));

out_objects.feature_objects.push_back(object);

Expand Down
6 changes: 3 additions & 3 deletions perception/cluster_merger/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -19,9 +19,9 @@ The clusters of merged topics are simply concatenated from clusters of input top

### Output

| Name | Type | Description |
| ----------------- | ----------------------------------------------------- | --------------- |
| `output/clusters` | `autoware_auto_perception_msgs::msg::DetectedObjects` | merged clusters |
| Name | Type | Description |
| ----------------- | -------------------------------------------------------- | --------------- |
| `output/clusters` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | merged clusters |

## Parameters

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` | `autoware_perception_msgs::msg::TrafficSignalArray` | classified signals |
| Name | Type | Description |
| ------------------------------------ | ------------------------------------------------ | ------------------ |
| `~/input/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | vector map |
| `~/input/route` | `autoware_planning_msgs::msg::LaneletRoute` | route |
| `~/input/classified/traffic_signals` | `tier4_perception_msgs::msg::TrafficSignalArray` | classified signals |

### Output

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

## Parameters

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -19,8 +19,8 @@
#include <tier4_autoware_utils/ros/debug_publisher.hpp>
#include <tier4_autoware_utils/system/stop_watch.hpp>

#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
#include <autoware_perception_msgs/msg/traffic_signal_array.hpp>
#include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
#include <autoware_perception_msgs/msg/traffic_light_group_array.hpp>
#include <autoware_planning_msgs/msg/lanelet_route.hpp>
#include <tier4_debug_msgs/msg/float64_stamped.hpp>

Expand All @@ -39,14 +39,14 @@
namespace traffic_light
{

using autoware_auto_mapping_msgs::msg::HADMapBin;
using autoware_map_msgs::msg::LaneletMapBin;
using autoware_planning_msgs::msg::LaneletRoute;
using tier4_autoware_utils::DebugPublisher;
using tier4_autoware_utils::StopWatch;
using tier4_debug_msgs::msg::Float64Stamped;
using TrafficSignal = autoware_perception_msgs::msg::TrafficSignal;
using TrafficSignalArray = autoware_perception_msgs::msg::TrafficSignalArray;
using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement;
using TrafficSignal = autoware_perception_msgs::msg::TrafficLightGroup;
using TrafficSignalArray = autoware_perception_msgs::msg::TrafficLightGroupArray;
using TrafficSignalElement = autoware_perception_msgs::msg::TrafficLightElement;
using TrafficSignalAndTime = std::pair<TrafficSignal, rclcpp::Time>;
using TrafficLightIdMap = std::unordered_map<lanelet::Id, TrafficSignalAndTime>;

Expand All @@ -58,7 +58,7 @@ class CrosswalkTrafficLightEstimatorNode : public rclcpp::Node
explicit CrosswalkTrafficLightEstimatorNode(const rclcpp::NodeOptions & options);

private:
rclcpp::Subscription<HADMapBin>::SharedPtr sub_map_;
rclcpp::Subscription<LaneletMapBin>::SharedPtr sub_map_;
rclcpp::Subscription<LaneletRoute>::SharedPtr sub_route_;
rclcpp::Subscription<TrafficSignalArray>::SharedPtr sub_traffic_light_array_;
rclcpp::Publisher<TrafficSignalArray>::SharedPtr pub_traffic_light_array_;
Expand All @@ -70,7 +70,7 @@ class CrosswalkTrafficLightEstimatorNode : public rclcpp::Node

lanelet::ConstLanelets conflicting_crosswalks_;

void onMap(const HADMapBin::ConstSharedPtr msg);
void onMap(const LaneletMapBin::ConstSharedPtr msg);
void onRoute(const LaneletRoute::ConstSharedPtr msg);
void onTrafficLightArray(const TrafficSignalArray::ConstSharedPtr msg);

Expand Down
3 changes: 1 addition & 2 deletions perception/crosswalk_traffic_light_estimator/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,7 @@
<buildtool_depend>ament_cmake_auto</buildtool_depend>
<buildtool_depend>autoware_cmake</buildtool_depend>

<depend>autoware_auto_mapping_msgs</depend>
<depend>autoware_auto_planning_msgs</depend>
<depend>autoware_map_msgs</depend>
<depend>autoware_perception_msgs</depend>
<depend>autoware_planning_msgs</depend>
<depend>lanelet2_extension</depend>
Expand Down
40 changes: 21 additions & 19 deletions perception/crosswalk_traffic_light_estimator/src/node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -85,7 +85,7 @@ CrosswalkTrafficLightEstimatorNode::CrosswalkTrafficLightEstimatorNode(
last_detect_color_hold_time_ = declare_parameter<double>("last_detect_color_hold_time");
last_colors_hold_time_ = declare_parameter<double>("last_colors_hold_time");

sub_map_ = create_subscription<HADMapBin>(
sub_map_ = create_subscription<LaneletMapBin>(
"~/input/vector_map", rclcpp::QoS{1}.transient_local(),
std::bind(&CrosswalkTrafficLightEstimatorNode::onMap, this, _1));
sub_route_ = create_subscription<LaneletRoute>(
Expand All @@ -100,7 +100,7 @@ CrosswalkTrafficLightEstimatorNode::CrosswalkTrafficLightEstimatorNode(
pub_processing_time_ = std::make_shared<DebugPublisher>(this, "~/debug");
}

void CrosswalkTrafficLightEstimatorNode::onMap(const HADMapBin::ConstSharedPtr msg)
void CrosswalkTrafficLightEstimatorNode::onMap(const LaneletMapBin::ConstSharedPtr msg)
{
RCLCPP_DEBUG(get_logger(), "[CrosswalkTrafficLightEstimatorNode]: Start loading lanelet");
lanelet_map_ptr_ = std::make_shared<lanelet::LaneletMap>();
Expand Down Expand Up @@ -160,8 +160,8 @@ void CrosswalkTrafficLightEstimatorNode::onTrafficLightArray(
TrafficSignalArray output = *msg;

TrafficLightIdMap traffic_light_id_map;
for (const auto & traffic_signal : msg->signals) {
traffic_light_id_map[traffic_signal.traffic_signal_id] =
for (const auto & traffic_signal : msg->traffic_light_groups) {
traffic_light_id_map[traffic_signal.traffic_light_group_id] =
std::pair<TrafficSignal, rclcpp::Time>(traffic_signal, get_clock()->now());
}
for (const auto & crosswalk : conflicting_crosswalks_) {
Expand Down Expand Up @@ -198,7 +198,7 @@ void CrosswalkTrafficLightEstimatorNode::updateLastDetectedSignal(
continue;
}

const auto & id = input_traffic_signal.second.first.traffic_signal_id;
const auto & id = input_traffic_signal.second.first.traffic_light_group_id;

if (last_detect_color_.count(id) == 0) {
last_detect_color_.insert(std::make_pair(id, input_traffic_signal.second));
Expand All @@ -210,7 +210,7 @@ void CrosswalkTrafficLightEstimatorNode::updateLastDetectedSignal(

std::vector<int32_t> erase_id_list;
for (auto & last_traffic_signal : last_detect_color_) {
const auto & id = last_traffic_signal.second.first.traffic_signal_id;
const auto & id = last_traffic_signal.second.first.traffic_light_group_id;

if (traffic_light_id_map.count(id) == 0) {
// hold signal recognition results for [last_detect_color_hold_time_] seconds.
Expand Down Expand Up @@ -243,7 +243,7 @@ void CrosswalkTrafficLightEstimatorNode::updateLastDetectedSignals(
continue;
}

const auto & id = input_traffic_signal.second.first.traffic_signal_id;
const auto & id = input_traffic_signal.second.first.traffic_light_group_id;

if (last_colors_.count(id) == 0) {
std::vector<TrafficSignalAndTime> signal{input_traffic_signal.second};
Expand Down Expand Up @@ -284,36 +284,36 @@ void CrosswalkTrafficLightEstimatorNode::setCrosswalkTrafficSignal(

std::unordered_map<lanelet::Id, size_t> valid_id2idx_map; // detected traffic light

for (size_t i = 0; i < msg.signals.size(); ++i) {
auto signal = msg.signals[i];
valid_id2idx_map[signal.traffic_signal_id] = i;
for (size_t i = 0; i < msg.traffic_light_groups.size(); ++i) {
auto signal = msg.traffic_light_groups[i];
valid_id2idx_map[signal.traffic_light_group_id] = i;
}

for (const auto & tl_reg_elem : tl_reg_elems) {
auto id = tl_reg_elem->id();
// if valid prediction exists, overwrite the estimation; else, use the estimation
if (valid_id2idx_map.count(id)) {
size_t idx = valid_id2idx_map[id];
auto signal = msg.signals[idx];
auto signal = msg.traffic_light_groups[idx];
updateFlashingState(signal); // check if it is flashing
// update output msg according to flashing and current state
output.signals[idx].elements[0].color = updateAndGetColorState(signal);
output.traffic_light_groups[idx].elements[0].color = updateAndGetColorState(signal);
} else {
TrafficSignal output_traffic_signal;
TrafficSignalElement output_traffic_signal_element;
output_traffic_signal_element.color = color;
output_traffic_signal_element.shape = TrafficSignalElement::CIRCLE;
output_traffic_signal_element.confidence = 1.0;
output_traffic_signal.elements.push_back(output_traffic_signal_element);
output_traffic_signal.traffic_signal_id = id;
output.signals.push_back(output_traffic_signal);
output_traffic_signal.traffic_light_group_id = id;
output.traffic_light_groups.push_back(output_traffic_signal);
}
}
}

void CrosswalkTrafficLightEstimatorNode::updateFlashingState(const TrafficSignal & signal)
{
const auto id = signal.traffic_signal_id;
const auto id = signal.traffic_light_group_id;

// no record of detected color in last_detect_color_hold_time_
if (is_flashing_.count(id) == 0) {
Expand Down Expand Up @@ -350,7 +350,7 @@ void CrosswalkTrafficLightEstimatorNode::updateFlashingState(const TrafficSignal

uint8_t CrosswalkTrafficLightEstimatorNode::updateAndGetColorState(const TrafficSignal & signal)
{
const auto id = signal.traffic_signal_id;
const auto id = signal.traffic_light_group_id;
const auto color = signal.elements[0].color;

if (current_color_state_.count(id) == 0) {
Expand Down Expand Up @@ -524,15 +524,17 @@ boost::optional<uint8_t> CrosswalkTrafficLightEstimatorNode::getHighestConfidenc

void CrosswalkTrafficLightEstimatorNode::removeDuplicateIds(TrafficSignalArray & signal_array) const
{
auto & signals = signal_array.signals;
auto & signals = signal_array.traffic_light_groups;
std::sort(signals.begin(), signals.end(), [](const auto & s1, const auto & s2) {
return s1.traffic_signal_id < s2.traffic_signal_id;
return s1.traffic_light_group_id < s2.traffic_light_group_id;
});

signals.erase(
std::unique(
signals.begin(), signals.end(),
[](const auto & s1, const auto s2) { return s1.traffic_signal_id == s2.traffic_signal_id; }),
[](const auto & s1, const auto s2) {
return s1.traffic_light_group_id == s2.traffic_light_group_id;
}),
signals.end());
}

Expand Down
6 changes: 3 additions & 3 deletions perception/detected_object_feature_remover/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -16,9 +16,9 @@ The `detected_object_feature_remover` is a package to convert topic-type from `D

### Output

| Name | Type | Description |
| ---------- | ----------------------------------------------------- | ---------------- |
| `~/output` | `autoware_auto_perception_msgs::msg::DetectedObjects` | detected objects |
| Name | Type | Description |
| ---------- | ------------------------------------------------ | ---------------- |
| `~/output` | `autoware_perception_msgs::msg::DetectedObjects` | detected objects |

## Parameters

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -18,14 +18,14 @@
#include <rclcpp/rclcpp.hpp>
#include <tier4_autoware_utils/ros/published_time_publisher.hpp>

#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
#include <autoware_perception_msgs/msg/detected_objects.hpp>
#include <tier4_perception_msgs/msg/detected_objects_with_feature.hpp>

#include <memory>

namespace detected_object_feature_remover
{
using autoware_auto_perception_msgs::msg::DetectedObjects;
using autoware_perception_msgs::msg::DetectedObjects;
using tier4_perception_msgs::msg::DetectedObjectsWithFeature;

class DetectedObjectFeatureRemover : public rclcpp::Node
Expand Down
2 changes: 1 addition & 1 deletion perception/detected_object_feature_remover/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>autoware_cmake</buildtool_depend>

<depend>autoware_auto_perception_msgs</depend>
<depend>autoware_perception_msgs</depend>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
<depend>tier4_autoware_utils</depend>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -23,8 +23,8 @@
#include <tier4_autoware_utils/ros/debug_publisher.hpp>
#include <tier4_autoware_utils/ros/published_time_publisher.hpp>

#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
#include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
#include <autoware_perception_msgs/msg/detected_objects.hpp>

#include <lanelet2_core/Forward.h>
#include <tf2_ros/buffer.h>
Expand All @@ -46,12 +46,12 @@ class ObjectLaneletFilterNode : public rclcpp::Node
explicit ObjectLaneletFilterNode(const rclcpp::NodeOptions & node_options);

private:
void objectCallback(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr);
void mapCallback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr);
void objectCallback(const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr);
void mapCallback(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr);

rclcpp::Publisher<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr object_pub_;
rclcpp::Subscription<autoware_auto_mapping_msgs::msg::HADMapBin>::SharedPtr map_sub_;
rclcpp::Subscription<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr object_sub_;
rclcpp::Publisher<autoware_perception_msgs::msg::DetectedObjects>::SharedPtr object_pub_;
rclcpp::Subscription<autoware_map_msgs::msg::LaneletMapBin>::SharedPtr map_sub_;
rclcpp::Subscription<autoware_perception_msgs::msg::DetectedObjects>::SharedPtr object_sub_;
std::unique_ptr<tier4_autoware_utils::DebugPublisher> debug_publisher_{nullptr};

lanelet::LaneletMapPtr lanelet_map_ptr_;
Expand All @@ -72,23 +72,22 @@ class ObjectLaneletFilterNode : public rclcpp::Node
} filter_settings_;

bool filterObject(
const autoware_auto_perception_msgs::msg::DetectedObject & transformed_object,
const autoware_auto_perception_msgs::msg::DetectedObject & input_object,
const autoware_perception_msgs::msg::DetectedObject & transformed_object,
const autoware_perception_msgs::msg::DetectedObject & input_object,
const lanelet::ConstLanelets & intersected_road_lanelets,
const lanelet::ConstLanelets & intersected_shoulder_lanelets,
autoware_auto_perception_msgs::msg::DetectedObjects & output_object_msg);
LinearRing2d getConvexHull(const autoware_auto_perception_msgs::msg::DetectedObjects &);
autoware_perception_msgs::msg::DetectedObjects & output_object_msg);
LinearRing2d getConvexHull(const autoware_perception_msgs::msg::DetectedObjects &);
lanelet::ConstLanelets getIntersectedLanelets(
const LinearRing2d &, const lanelet::ConstLanelets &);
bool isObjectOverlapLanelets(
const autoware_auto_perception_msgs::msg::DetectedObject & object,
const autoware_perception_msgs::msg::DetectedObject & object,
const lanelet::ConstLanelets & intersected_lanelets);
bool isPolygonOverlapLanelets(const Polygon2d &, const lanelet::ConstLanelets &);
bool isSameDirectionWithLanelets(
const lanelet::ConstLanelets & lanelets,
const autoware_auto_perception_msgs::msg::DetectedObject & object);
geometry_msgs::msg::Polygon setFootprint(
const autoware_auto_perception_msgs::msg::DetectedObject &);
const autoware_perception_msgs::msg::DetectedObject & object);
geometry_msgs::msg::Polygon setFootprint(const autoware_perception_msgs::msg::DetectedObject &);

std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
};
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -21,8 +21,8 @@
#include <tier4_autoware_utils/geometry/geometry.hpp>
#include <tier4_autoware_utils/ros/published_time_publisher.hpp>

#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
#include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
#include <autoware_perception_msgs/msg/detected_objects.hpp>

#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
Expand All @@ -39,10 +39,10 @@ class ObjectPositionFilterNode : public rclcpp::Node
explicit ObjectPositionFilterNode(const rclcpp::NodeOptions & node_options);

private:
void objectCallback(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr);
void objectCallback(const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr);

rclcpp::Publisher<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr object_pub_;
rclcpp::Subscription<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr object_sub_;
rclcpp::Publisher<autoware_perception_msgs::msg::DetectedObjects>::SharedPtr object_pub_;
rclcpp::Subscription<autoware_perception_msgs::msg::DetectedObjects>::SharedPtr object_sub_;

tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_;
Expand All @@ -52,7 +52,7 @@ class ObjectPositionFilterNode : public rclcpp::Node
float lower_bound_x_;
float lower_bound_y_;
utils::FilterTargetLabel filter_target_;
bool isObjectInBounds(const autoware_auto_perception_msgs::msg::DetectedObject & object) const;
bool isObjectInBounds(const autoware_perception_msgs::msg::DetectedObject & object) const;

std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
};
Expand Down
Loading

0 comments on commit 8049571

Please sign in to comment.