Skip to content

Commit d70fff8

Browse files
storrrrrrrrmstorrrrrrrrm
authored
Merge branch 'main' into yolov10
2 parents f175e06 + ea448a9 commit d70fff8

File tree

82 files changed

+313
-289
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

82 files changed

+313
-289
lines changed

‎perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp

+10-6
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,7 @@
2626
#include <tf2/utils.h>
2727

2828
#include <algorithm>
29+
#include <limits>
2930
#include <string>
3031
#include <vector>
3132

@@ -92,17 +93,20 @@ bool convertConvexHullToBoundingBox(
9293
}
9394

9495
// look for bounding box boundary
95-
float max_x = 0;
96-
float max_y = 0;
97-
float min_x = 0;
98-
float min_y = 0;
99-
float max_z = 0;
96+
float max_x = -std::numeric_limits<float>::infinity();
97+
float max_y = -std::numeric_limits<float>::infinity();
98+
float min_x = std::numeric_limits<float>::infinity();
99+
float min_y = std::numeric_limits<float>::infinity();
100+
float max_z = -std::numeric_limits<float>::infinity();
101+
float min_z = std::numeric_limits<float>::infinity();
102+
100103
for (const auto & point : input_object.shape.footprint.points) {
101104
max_x = std::max(max_x, point.x);
102105
max_y = std::max(max_y, point.y);
103106
min_x = std::min(min_x, point.x);
104107
min_y = std::min(min_y, point.y);
105108
max_z = std::max(max_z, point.z);
109+
min_z = std::min(min_z, point.z);
106110
}
107111

108112
// calc new center
@@ -120,7 +124,7 @@ bool convertConvexHullToBoundingBox(
120124
output_object.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX;
121125
output_object.shape.dimensions.x = max_x - min_x;
122126
output_object.shape.dimensions.y = max_y - min_y;
123-
output_object.shape.dimensions.z = max_z;
127+
output_object.shape.dimensions.z = max_z - min_z;
124128

125129
return true;
126130
}

‎perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp

+1
Original file line numberDiff line numberDiff line change
@@ -144,6 +144,7 @@ bool UnknownTracker::measure(const types::DynamicObject & object, const rclcpp::
144144
{
145145
// update object shape
146146
object_.shape = object.shape;
147+
object_.pose.orientation = object.pose.orientation;
147148

148149
// check time gap
149150
const double dt = motion_model_.getDeltaTime(time);

‎perception/autoware_multi_object_tracker/src/processor/input_manager.cpp

+14-14
Original file line numberDiff line numberDiff line change
@@ -56,6 +56,20 @@ void InputStream::onMessage(
5656

5757
types::DynamicObjectList dynamic_objects = types::toDynamicObjectList(objects, channel_.index);
5858

59+
// Model the object uncertainty only if it is not available
60+
types::DynamicObjectList objects_with_uncertainty =
61+
uncertainty::modelUncertainty(dynamic_objects);
62+
63+
// Transform the objects to the world frame
64+
auto transformed_objects = odometry_->transformObjects(objects_with_uncertainty);
65+
if (!transformed_objects) {
66+
RCLCPP_WARN(
67+
node_.get_logger(), "InputManager::onMessage %s: Failed to transform objects.",
68+
channel_.long_name.c_str());
69+
return;
70+
}
71+
dynamic_objects = transformed_objects.value();
72+
5973
// object shape processing
6074
for (auto & object : dynamic_objects.objects) {
6175
const auto label =
@@ -91,20 +105,6 @@ void InputStream::onMessage(
91105
// if object extension is not reliable, enlarge covariance of position and extend shape
92106
}
93107

94-
// Model the object uncertainty only if it is not available
95-
types::DynamicObjectList objects_with_uncertainty =
96-
uncertainty::modelUncertainty(dynamic_objects);
97-
98-
// Transform the objects to the world frame
99-
auto transformed_objects = odometry_->transformObjects(objects_with_uncertainty);
100-
if (!transformed_objects) {
101-
RCLCPP_WARN(
102-
node_.get_logger(), "InputManager::onMessage %s: Failed to transform objects.",
103-
channel_.long_name.c_str());
104-
return;
105-
}
106-
dynamic_objects = transformed_objects.value();
107-
108108
// Normalize the object uncertainty
109109
uncertainty::normalizeUncertainty(dynamic_objects);
110110

‎perception/autoware_traffic_light_classifier/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919
<depend>autoware_cuda_utils</depend>
2020
<depend>autoware_tensorrt_classifier</depend>
2121
<depend>autoware_tensorrt_common</depend>
22+
<depend>autoware_utils</depend>
2223
<depend>cv_bridge</depend>
2324
<depend>image_transport</depend>
2425
<depend>libboost-filesystem-dev</depend>

‎perception/autoware_traffic_light_classifier/src/traffic_light_classifier_node.cpp

+15
Original file line numberDiff line numberDiff line change
@@ -13,6 +13,7 @@
1313
// limitations under the License.
1414
#include "traffic_light_classifier_node.hpp"
1515

16+
#include <diagnostic_msgs/msg/diagnostic_status.hpp>
1617
#include <tier4_perception_msgs/msg/traffic_light_element.hpp>
1718

1819
#include <iostream>
@@ -60,6 +61,9 @@ TrafficLightClassifierNodelet::TrafficLightClassifierNodelet(const rclcpp::NodeO
6061
this->get_logger(), "please install CUDA, CUDNN and TensorRT to use cnn classifier");
6162
#endif
6263
}
64+
65+
diagnostics_interface_ptr_ =
66+
std::make_unique<autoware_utils::DiagnosticsInterface>(this, "traffic_light_classifier");
6367
}
6468

6569
void TrafficLightClassifierNodelet::connectCb()
@@ -165,6 +169,17 @@ void TrafficLightClassifierNodelet::imageRoiCallback(
165169

166170
output_msg.header = input_image_msg->header;
167171
traffic_signal_array_pub_->publish(output_msg);
172+
173+
// publish diagnostics
174+
diagnostics_interface_ptr_->clear();
175+
bool found_harsh_backlight = backlight_indices.size() != 0;
176+
diagnostics_interface_ptr_->add_key_value("found_harsh_backlight", found_harsh_backlight);
177+
if (found_harsh_backlight) {
178+
diagnostics_interface_ptr_->update_level_and_message(
179+
diagnostic_msgs::msg::DiagnosticStatus::WARN,
180+
"Found harsh backlight in ROI(s) and corresponding ROI(s) were overwritten by UNKNOWN");
181+
}
182+
diagnostics_interface_ptr_->publish(output_msg.header.stamp);
168183
}
169184

170185
bool TrafficLightClassifierNodelet::is_harsh_backlight(const cv::Mat & img) const

‎perception/autoware_traffic_light_classifier/src/traffic_light_classifier_node.hpp

+4
Original file line numberDiff line numberDiff line change
@@ -48,6 +48,7 @@
4848

4949
#include "classifier/color_classifier.hpp"
5050

51+
#include <autoware_utils/ros/diagnostics_interface.hpp>
5152
#include <opencv2/core/core.hpp>
5253
#include <opencv2/highgui/highgui.hpp>
5354

@@ -89,6 +90,9 @@ class TrafficLightClassifierNodelet : public rclcpp::Node
8990
traffic_signal_array_pub_;
9091
std::shared_ptr<ClassifierInterface> classifier_ptr_;
9192

93+
std::unique_ptr<autoware_utils::DiagnosticsInterface>
94+
diagnostics_interface_ptr_; //!< Diagnostic handler.
95+
9296
double backlight_threshold_;
9397
bool is_harsh_backlight(const cv::Mat & img) const;
9498
};

‎planning/autoware_external_velocity_limit_selector/README.md

+8-8
Original file line numberDiff line numberDiff line change
@@ -37,17 +37,17 @@ Example:
3737

3838
## Inputs
3939

40-
| Name | Type | Description |
41-
| --------------------------------------------------- | ---------------------------------------------- | --------------------------------------------- |
42-
| `~input/velocity_limit_from_api` | tier4_planning_msgs::VelocityLimit | velocity limit from api |
43-
| `~input/velocity_limit_from_internal` | tier4_planning_msgs::VelocityLimit | velocity limit from autoware internal modules |
44-
| `~input/velocity_limit_clear_command_from_internal` | tier4_planning_msgs::VelocityLimitClearCommand | velocity limit clear command |
40+
| Name | Type | Description |
41+
| --------------------------------------------------- | --------------------------------------------------------------- | --------------------------------------------- |
42+
| `~input/velocity_limit_from_api` | autoware_internal_planning_msgs::msg::VelocityLimit | velocity limit from api |
43+
| `~input/velocity_limit_from_internal` | autoware_internal_planning_msgs::msg::VelocityLimit | velocity limit from autoware internal modules |
44+
| `~input/velocity_limit_clear_command_from_internal` | autoware_internal_planning_msgs::msg::VelocityLimitClearCommand | velocity limit clear command |
4545

4646
## Outputs
4747

48-
| Name | Type | Description |
49-
| ---------------------- | ---------------------------------- | ------------------------------------------------- |
50-
| `~output/max_velocity` | tier4_planning_msgs::VelocityLimit | current information of the hardest velocity limit |
48+
| Name | Type | Description |
49+
| ---------------------- | --------------------------------------------------- | ------------------------------------------------- |
50+
| `~output/max_velocity` | autoware_internal_planning_msgs::msg::VelocityLimit | current information of the hardest velocity limit |
5151

5252
## Parameters
5353

‎planning/autoware_external_velocity_limit_selector/include/autoware/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp

+5-5
Original file line numberDiff line numberDiff line change
@@ -19,8 +19,8 @@
1919
#include <rclcpp/rclcpp.hpp>
2020

2121
#include <autoware_internal_debug_msgs/msg/string_stamped.hpp>
22-
#include <tier4_planning_msgs/msg/velocity_limit.hpp>
23-
#include <tier4_planning_msgs/msg/velocity_limit_clear_command.hpp>
22+
#include <autoware_internal_planning_msgs/msg/velocity_limit.hpp>
23+
#include <autoware_internal_planning_msgs/msg/velocity_limit_clear_command.hpp>
2424

2525
#include <memory>
2626
#include <string>
@@ -30,9 +30,9 @@ namespace autoware::external_velocity_limit_selector
3030
{
3131

3232
using autoware_internal_debug_msgs::msg::StringStamped;
33-
using tier4_planning_msgs::msg::VelocityLimit;
34-
using tier4_planning_msgs::msg::VelocityLimitClearCommand;
35-
using tier4_planning_msgs::msg::VelocityLimitConstraints;
33+
using autoware_internal_planning_msgs::msg::VelocityLimit;
34+
using autoware_internal_planning_msgs::msg::VelocityLimitClearCommand;
35+
using autoware_internal_planning_msgs::msg::VelocityLimitConstraints;
3636

3737
using VelocityLimitTable = std::unordered_map<std::string, VelocityLimit>;
3838

‎planning/autoware_external_velocity_limit_selector/package.xml

+1-1
Original file line numberDiff line numberDiff line change
@@ -18,10 +18,10 @@
1818
<buildtool_depend>autoware_cmake</buildtool_depend>
1919

2020
<depend>autoware_internal_debug_msgs</depend>
21+
<depend>autoware_internal_planning_msgs</depend>
2122
<depend>generate_parameter_library</depend>
2223
<depend>rclcpp</depend>
2324
<depend>rclcpp_components</depend>
24-
<depend>tier4_planning_msgs</depend>
2525

2626
<exec_depend>ros2cli</exec_depend>
2727
<exec_depend>topic_tools</exec_depend>

‎planning/autoware_external_velocity_limit_selector/test/test_external_velocity_limit_selector_node_launch.py

+2-2
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,8 @@
1818
import unittest
1919

2020
from ament_index_python import get_package_share_directory
21+
from autoware_internal_planning_msgs.msg import VelocityLimit
22+
from autoware_internal_planning_msgs.msg import VelocityLimitClearCommand
2123
import launch
2224
from launch.actions import IncludeLaunchDescription
2325
from launch.launch_description_sources import AnyLaunchDescriptionSource
@@ -30,8 +32,6 @@
3032
from rcl_interfaces.srv import SetParameters
3133
import rclpy
3234
import rclpy.qos
33-
from tier4_planning_msgs.msg import VelocityLimit
34-
from tier4_planning_msgs.msg import VelocityLimitClearCommand
3535

3636
logger = get_logger(__name__)
3737

‎planning/autoware_obstacle_cruise_planner/README.md

+5-5
Original file line numberDiff line numberDiff line change
@@ -23,11 +23,11 @@ The `autoware_obstacle_cruise_planner` package has following modules.
2323

2424
### Output topics
2525

26-
| Name | Type | Description |
27-
| ------------------------------- | ---------------------------------------------- | -------------------------------- |
28-
| `~/output/trajectory` | autoware_planning_msgs::Trajectory | output trajectory |
29-
| `~/output/velocity_limit` | tier4_planning_msgs::VelocityLimit | velocity limit for cruising |
30-
| `~/output/clear_velocity_limit` | tier4_planning_msgs::VelocityLimitClearCommand | clear command for velocity limit |
26+
| Name | Type | Description |
27+
| ------------------------------- | --------------------------------------------------------------- | -------------------------------- |
28+
| `~/output/trajectory` | autoware_planning_msgs::Trajectory | output trajectory |
29+
| `~/output/velocity_limit` | autoware_internal_planning_msgs::msg::VelocityLimit | velocity limit for cruising |
30+
| `~/output/clear_velocity_limit` | autoware_internal_planning_msgs::msg::VelocityLimitClearCommand | clear command for velocity limit |
3131

3232
## Design
3333

‎planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp

+4-4
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,8 @@
2121

2222
#include "autoware_internal_debug_msgs/msg/float32_stamped.hpp"
2323
#include "autoware_internal_debug_msgs/msg/float64_stamped.hpp"
24+
#include "autoware_internal_planning_msgs/msg/velocity_limit.hpp"
25+
#include "autoware_internal_planning_msgs/msg/velocity_limit_clear_command.hpp"
2426
#include "autoware_perception_msgs/msg/predicted_object.hpp"
2527
#include "autoware_perception_msgs/msg/predicted_objects.hpp"
2628
#include "autoware_planning_msgs/msg/trajectory.hpp"
@@ -31,8 +33,6 @@
3133
#include "nav_msgs/msg/odometry.hpp"
3234
#include "sensor_msgs/msg/point_cloud2.hpp"
3335
#include "tier4_planning_msgs/msg/stop_speed_exceeded.hpp"
34-
#include "tier4_planning_msgs/msg/velocity_limit.hpp"
35-
#include "tier4_planning_msgs/msg/velocity_limit_clear_command.hpp"
3636
#include "visualization_msgs/msg/marker_array.hpp"
3737

3838
#include <pcl/point_cloud.h>
@@ -41,6 +41,8 @@
4141
using autoware::vehicle_info_utils::VehicleInfo;
4242
using autoware_internal_debug_msgs::msg::Float32Stamped;
4343
using autoware_internal_debug_msgs::msg::Float64Stamped;
44+
using autoware_internal_planning_msgs::msg::VelocityLimit;
45+
using autoware_internal_planning_msgs::msg::VelocityLimitClearCommand;
4446
using autoware_perception_msgs::msg::ObjectClassification;
4547
using autoware_perception_msgs::msg::PredictedObject;
4648
using autoware_perception_msgs::msg::PredictedObjects;
@@ -54,8 +56,6 @@ using geometry_msgs::msg::Twist;
5456
using nav_msgs::msg::Odometry;
5557
using sensor_msgs::msg::PointCloud2;
5658
using tier4_planning_msgs::msg::StopSpeedExceeded;
57-
using tier4_planning_msgs::msg::VelocityLimit;
58-
using tier4_planning_msgs::msg::VelocityLimitClearCommand;
5959
using visualization_msgs::msg::Marker;
6060
using visualization_msgs::msg::MarkerArray;
6161
namespace bg = boost::geometry;

‎planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -21,7 +21,7 @@
2121
#include "autoware_utils/ros/marker_helper.hpp"
2222
#include "autoware_utils/ros/update_param.hpp"
2323

24-
#include "tier4_planning_msgs/msg/velocity_limit.hpp"
24+
#include "autoware_internal_planning_msgs/msg/velocity_limit.hpp"
2525

2626
#include <algorithm>
2727
#include <memory>

‎planning/autoware_obstacle_stop_planner/src/node.hpp

+4-4
Original file line numberDiff line numberDiff line change
@@ -34,6 +34,8 @@
3434
#include <autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp>
3535
#include <autoware_internal_debug_msgs/msg/float32_stamped.hpp>
3636
#include <autoware_internal_debug_msgs/msg/float64_stamped.hpp>
37+
#include <autoware_internal_planning_msgs/msg/velocity_limit.hpp>
38+
#include <autoware_internal_planning_msgs/msg/velocity_limit_clear_command.hpp>
3739
#include <autoware_perception_msgs/msg/predicted_objects.hpp>
3840
#include <autoware_planning_msgs/msg/trajectory.hpp>
3941
#include <diagnostic_msgs/msg/diagnostic_status.hpp>
@@ -42,8 +44,6 @@
4244
#include <geometry_msgs/msg/twist_stamped.hpp>
4345
#include <sensor_msgs/msg/point_cloud2.hpp>
4446
#include <tier4_planning_msgs/msg/expand_stop_range.hpp>
45-
#include <tier4_planning_msgs/msg/velocity_limit.hpp>
46-
#include <tier4_planning_msgs/msg/velocity_limit_clear_command.hpp>
4747

4848
#include <boost/assert.hpp>
4949
#include <boost/assign/list_of.hpp>
@@ -83,15 +83,15 @@ using autoware_internal_debug_msgs::msg::BoolStamped;
8383
using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped;
8484
using autoware_internal_debug_msgs::msg::Float32Stamped;
8585
using autoware_internal_debug_msgs::msg::Float64Stamped;
86+
using autoware_internal_planning_msgs::msg::VelocityLimit;
87+
using autoware_internal_planning_msgs::msg::VelocityLimitClearCommand;
8688
using autoware_perception_msgs::msg::PredictedObjects;
8789
using autoware_planning_msgs::msg::Trajectory;
8890
using autoware_planning_msgs::msg::TrajectoryPoint;
8991
using autoware_utils::Point2d;
9092
using autoware_utils::Polygon2d;
9193
using autoware_utils::StopWatch;
9294
using tier4_planning_msgs::msg::ExpandStopRange;
93-
using tier4_planning_msgs::msg::VelocityLimit;
94-
using tier4_planning_msgs::msg::VelocityLimitClearCommand;
9595

9696
using TrajectoryPoints = std::vector<TrajectoryPoint>;
9797
using PointCloud = pcl::PointCloud<pcl::PointXYZ>;

‎planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/geometry_utils.hpp

+7-2
Original file line numberDiff line numberDiff line change
@@ -36,14 +36,19 @@
3636
#include <string>
3737
#include <vector>
3838

39-
namespace autoware_utils
39+
namespace autoware_utils_geometry
4040
{
41+
4142
template <>
4243
geometry_msgs::msg::Point get_point(const autoware::path_optimizer::ReferencePoint & p);
4344

4445
template <>
4546
geometry_msgs::msg::Pose get_pose(const autoware::path_optimizer::ReferencePoint & p);
46-
} // namespace autoware_utils
47+
48+
template <>
49+
double get_longitudinal_velocity(const autoware::path_optimizer::ReferencePoint & p);
50+
51+
} // namespace autoware_utils_geometry
4752

4853
namespace autoware::path_optimizer
4954
{

‎planning/autoware_path_optimizer/include/autoware/path_optimizer/utils/trajectory_utils.hpp

+2-19
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@
2121
#include "autoware/motion_utils/trajectory/trajectory.hpp"
2222
#include "autoware/path_optimizer/common_structs.hpp"
2323
#include "autoware/path_optimizer/type_alias.hpp"
24+
#include "autoware/path_optimizer/utils/geometry_utils.hpp"
2425
#include "autoware_utils/geometry/geometry.hpp"
2526

2627
#include <Eigen/Core>
@@ -36,18 +37,6 @@
3637
#include <string>
3738
#include <vector>
3839

39-
namespace autoware_utils
40-
{
41-
template <>
42-
geometry_msgs::msg::Point get_point(const autoware::path_optimizer::ReferencePoint & p);
43-
44-
template <>
45-
geometry_msgs::msg::Pose get_pose(const autoware::path_optimizer::ReferencePoint & p);
46-
47-
template <>
48-
double get_longitudinal_velocity(const autoware::path_optimizer::ReferencePoint & p);
49-
} // namespace autoware_utils
50-
5140
namespace autoware::path_optimizer
5241
{
5342
namespace trajectory_utils
@@ -107,13 +96,7 @@ TrajectoryPoint convertToTrajectoryPoint(const T & point)
10796
}
10897

10998
template <>
110-
inline TrajectoryPoint convertToTrajectoryPoint(const ReferencePoint & ref_point)
111-
{
112-
TrajectoryPoint traj_point;
113-
traj_point.pose = autoware_utils::get_pose(ref_point);
114-
traj_point.longitudinal_velocity_mps = autoware_utils::get_longitudinal_velocity(ref_point);
115-
return traj_point;
116-
}
99+
TrajectoryPoint convertToTrajectoryPoint(const ReferencePoint & ref_point);
117100

118101
template <typename T>
119102
std::vector<TrajectoryPoint> convertToTrajectoryPoints(const std::vector<T> & points)

0 commit comments

Comments
 (0)