Skip to content

Commit bc06079

Browse files
isamu-takagiisamu-takagi
committed
Merge branch 'main' into feat/adapi-door
2 parents 9fabcd1 + 85656ce commit bc06079

File tree

167 files changed

+4960
-1314
lines changed

Some content is hidden

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

167 files changed

+4960
-1314
lines changed

‎common/interpolation/test/src/test_spline_interpolation.cpp

+41-9
Original file line numberDiff line numberDiff line change
@@ -222,16 +222,48 @@ TEST(spline_interpolation, splineByAkima)
222222

223223
TEST(spline_interpolation, SplineInterpolation)
224224
{
225-
// curve: query_keys is random
226-
const std::vector<double> base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0};
227-
const std::vector<double> base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0};
228-
const std::vector<double> query_keys{0.0, 8.0, 18.0};
229-
const std::vector<double> ans{-0.075611, 0.997242, 1.573258};
225+
{
226+
// curve: query_keys is random
227+
const std::vector<double> base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0};
228+
const std::vector<double> base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0};
229+
const std::vector<double> query_keys{0.0, 8.0, 18.0};
230+
const std::vector<double> ans{-0.075611, 0.997242, 1.573258};
231+
232+
SplineInterpolation s(base_keys, base_values);
233+
const std::vector<double> query_values = s.getSplineInterpolatedValues(query_keys);
234+
235+
for (size_t i = 0; i < query_values.size(); ++i) {
236+
EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
237+
}
238+
}
239+
240+
{
241+
// getSplineInterpolatedDiffValues
242+
const std::vector<double> base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0};
243+
const std::vector<double> base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0};
244+
const std::vector<double> query_keys{0.0, 8.0, 12.0, 18.0};
245+
const std::vector<double> ans{0.671301, 0.0509853, 0.209426, -0.253628};
230246

231-
SplineInterpolation s(base_keys, base_values);
232-
const std::vector<double> query_values = s.getSplineInterpolatedValues(query_keys);
247+
SplineInterpolation s(base_keys, base_values);
248+
const std::vector<double> query_values = s.getSplineInterpolatedDiffValues(query_keys);
233249

234-
for (size_t i = 0; i < query_values.size(); ++i) {
235-
EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
250+
for (size_t i = 0; i < query_values.size(); ++i) {
251+
EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
252+
}
253+
}
254+
255+
{
256+
// getSplineInterpolatedQuadDiffValues
257+
const std::vector<double> base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0};
258+
const std::vector<double> base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0};
259+
const std::vector<double> query_keys{0.0, 8.0, 12.0, 18.0};
260+
const std::vector<double> ans{-0.156582, 0.0440771, -0.0116873, -0.0495025};
261+
262+
SplineInterpolation s(base_keys, base_values);
263+
const std::vector<double> query_values = s.getSplineInterpolatedQuadDiffValues(query_keys);
264+
265+
for (size_t i = 0; i < query_values.size(); ++i) {
266+
EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
267+
}
236268
}
237269
}

‎common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -462,7 +462,8 @@ bool isDrivingForward(const Pose1 & src_pose, const Pose2 & dst_pose)
462462
* pose.
463463
*/
464464
geometry_msgs::msg::Pose calcOffsetPose(
465-
const geometry_msgs::msg::Pose & p, const double x, const double y, const double z);
465+
const geometry_msgs::msg::Pose & p, const double x, const double y, const double z,
466+
const double yaw = 0.0);
466467

467468
/**
468469
* @brief Calculate a point by linear interpolation.

‎common/tier4_autoware_utils/src/geometry/geometry.cpp

+4-2
Original file line numberDiff line numberDiff line change
@@ -327,12 +327,13 @@ double calcCurvature(
327327
* pose.
328328
*/
329329
geometry_msgs::msg::Pose calcOffsetPose(
330-
const geometry_msgs::msg::Pose & p, const double x, const double y, const double z)
330+
const geometry_msgs::msg::Pose & p, const double x, const double y, const double z,
331+
const double yaw)
331332
{
332333
geometry_msgs::msg::Pose pose;
333334
geometry_msgs::msg::Transform transform;
334335
transform.translation = createTranslation(x, y, z);
335-
transform.rotation = createQuaternion(0.0, 0.0, 0.0, 1.0);
336+
transform.rotation = createQuaternionFromYaw(yaw);
336337
tf2::Transform tf_pose;
337338
tf2::Transform tf_offset;
338339
tf2::fromMsg(transform, tf_offset);
@@ -378,6 +379,7 @@ std::optional<geometry_msgs::msg::Point> intersect(
378379
geometry_msgs::msg::Point intersect_point;
379380
intersect_point.x = t * p1.x + (1.0 - t) * p2.x;
380381
intersect_point.y = t * p1.y + (1.0 - t) * p2.y;
382+
intersect_point.z = t * p1.z + (1.0 - t) * p2.z;
381383
return intersect_point;
382384
}
383385

‎common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp

+35
Original file line numberDiff line numberDiff line change
@@ -1138,6 +1138,7 @@ TEST(geometry, calcOffsetPose)
11381138
using tier4_autoware_utils::createQuaternionFromRPY;
11391139
using tier4_autoware_utils::deg2rad;
11401140

1141+
// Only translation
11411142
{
11421143
geometry_msgs::msg::Pose p_in;
11431144
p_in.position = createPoint(1.0, 2.0, 3.0);
@@ -1185,6 +1186,40 @@ TEST(geometry, calcOffsetPose)
11851186
EXPECT_DOUBLE_EQ(p_out.orientation.z, 0.25881904510252068);
11861187
EXPECT_DOUBLE_EQ(p_out.orientation.w, 0.96592582628906831);
11871188
}
1189+
1190+
// Only rotation
1191+
{
1192+
geometry_msgs::msg::Pose p_in;
1193+
p_in.position = createPoint(2.0, 1.0, 1.0);
1194+
p_in.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(30));
1195+
1196+
const auto p_out = calcOffsetPose(p_in, 0.0, 0.0, 0.0, deg2rad(20));
1197+
1198+
EXPECT_DOUBLE_EQ(p_out.position.x, 2.0);
1199+
EXPECT_DOUBLE_EQ(p_out.position.y, 1.0);
1200+
EXPECT_DOUBLE_EQ(p_out.position.z, 1.0);
1201+
EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0);
1202+
EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0);
1203+
EXPECT_NEAR(p_out.orientation.z, 0.42261826174069944, epsilon);
1204+
EXPECT_NEAR(p_out.orientation.w, 0.9063077870366499, epsilon);
1205+
}
1206+
1207+
// Both translation and rotation
1208+
{
1209+
geometry_msgs::msg::Pose p_in;
1210+
p_in.position = createPoint(2.0, 1.0, 1.0);
1211+
p_in.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(30));
1212+
1213+
const auto p_out = calcOffsetPose(p_in, 2.0, 0.0, -1.0, deg2rad(20));
1214+
1215+
EXPECT_DOUBLE_EQ(p_out.position.x, 3.73205080756887729);
1216+
EXPECT_DOUBLE_EQ(p_out.position.y, 2.0);
1217+
EXPECT_DOUBLE_EQ(p_out.position.z, 0.0);
1218+
EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0);
1219+
EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0);
1220+
EXPECT_NEAR(p_out.orientation.z, 0.42261826174069944, epsilon);
1221+
EXPECT_NEAR(p_out.orientation.w, 0.9063077870366499, epsilon);
1222+
}
11881223
}
11891224

11901225
TEST(geometry, isDrivingForward)

‎common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp

+4-86
Original file line numberDiff line numberDiff line change
@@ -107,62 +107,9 @@ void VelocitySteeringFactorsPanel::onVelocityFactors(const VelocityFactorArray::
107107
for (std::size_t i = 0; i < msg->factors.size(); i++) {
108108
const auto & e = msg->factors.at(i);
109109

110-
// type
110+
// behavior
111111
{
112-
auto label = new QLabel();
113-
switch (e.type) {
114-
case VelocityFactor::SURROUNDING_OBSTACLE:
115-
label->setText("SURROUNDING_OBSTACLE");
116-
break;
117-
case VelocityFactor::ROUTE_OBSTACLE:
118-
label->setText("ROUTE_OBSTACLE");
119-
break;
120-
case VelocityFactor::INTERSECTION:
121-
label->setText("INTERSECTION");
122-
break;
123-
case VelocityFactor::CROSSWALK:
124-
label->setText("CROSSWALK");
125-
break;
126-
case VelocityFactor::REAR_CHECK:
127-
label->setText("REAR_CHECK");
128-
break;
129-
case VelocityFactor::USER_DEFINED_DETECTION_AREA:
130-
label->setText("USER_DEFINED_DETECTION_AREA");
131-
break;
132-
case VelocityFactor::NO_STOPPING_AREA:
133-
label->setText("NO_STOPPING_AREA");
134-
break;
135-
case VelocityFactor::STOP_SIGN:
136-
label->setText("STOP_SIGN");
137-
break;
138-
case VelocityFactor::TRAFFIC_SIGNAL:
139-
label->setText("TRAFFIC_SIGNAL");
140-
break;
141-
case VelocityFactor::V2I_GATE_CONTROL_ENTER:
142-
label->setText("V2I_GATE_CONTROL_ENTER");
143-
break;
144-
case VelocityFactor::V2I_GATE_CONTROL_LEAVE:
145-
label->setText("V2I_GATE_CONTROL_LEAVE");
146-
break;
147-
case VelocityFactor::MERGE:
148-
label->setText("MERGE");
149-
break;
150-
case VelocityFactor::SIDEWALK:
151-
label->setText("SIDEWALK");
152-
break;
153-
case VelocityFactor::LANE_CHANGE:
154-
label->setText("LANE_CHANGE");
155-
break;
156-
case VelocityFactor::AVOIDANCE:
157-
label->setText("AVOIDANCE");
158-
break;
159-
case VelocityFactor::EMERGENCY_STOP_OPERATION:
160-
label->setText("EMERGENCY_STOP_OPERATION");
161-
break;
162-
default:
163-
label->setText("UNKNOWN");
164-
break;
165-
}
112+
auto label = new QLabel(e.behavior.empty() ? "UNKNOWN" : e.behavior.c_str());
166113
label->setAlignment(Qt::AlignCenter);
167114
velocity_factors_table_->setCellWidget(i, 0, label);
168115
}
@@ -213,38 +160,9 @@ void VelocitySteeringFactorsPanel::onSteeringFactors(const SteeringFactorArray::
213160
for (std::size_t i = 0; i < msg->factors.size(); i++) {
214161
const auto & e = msg->factors.at(i);
215162

216-
// type
163+
// behavior
217164
{
218-
auto label = new QLabel();
219-
switch (e.type) {
220-
case SteeringFactor::INTERSECTION:
221-
label->setText("INTERSECTION");
222-
break;
223-
case SteeringFactor::LANE_CHANGE:
224-
label->setText("LANE_CHANGE");
225-
break;
226-
case SteeringFactor::AVOIDANCE_PATH_CHANGE:
227-
label->setText("AVOIDANCE_PATH_CHANGE");
228-
break;
229-
case SteeringFactor::AVOIDANCE_PATH_RETURN:
230-
label->setText("AVOIDANCE_PATH_RETURN");
231-
break;
232-
case SteeringFactor::STATION:
233-
label->setText("STATION");
234-
break;
235-
case SteeringFactor::START_PLANNER:
236-
label->setText("START_PLANNER");
237-
break;
238-
case SteeringFactor::GOAL_PLANNER:
239-
label->setText("GOAL_PLANNER");
240-
break;
241-
case SteeringFactor::EMERGENCY_OPERATION:
242-
label->setText("EMERGENCY_OPERATION");
243-
break;
244-
default:
245-
label->setText("UNKNOWN");
246-
break;
247-
}
165+
auto label = new QLabel(e.behavior.empty() ? "UNKNOWN" : e.behavior.c_str());
248166
label->setAlignment(Qt::AlignCenter);
249167
steering_factors_table_->setCellWidget(i, 0, label);
250168
}

‎common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.hpp

+2
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,7 @@
2626
#include <rclcpp/rclcpp.hpp>
2727
#include <rviz_common/panel.hpp>
2828

29+
#include <autoware_adapi_v1_msgs/msg/planning_behavior.hpp>
2930
#include <autoware_adapi_v1_msgs/msg/steering_factor_array.hpp>
3031
#include <autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp>
3132

@@ -35,6 +36,7 @@ namespace rviz_plugins
3536
{
3637
class VelocitySteeringFactorsPanel : public rviz_common::Panel
3738
{
39+
using PlanningBehavior = autoware_adapi_v1_msgs::msg::PlanningBehavior;
3840
using VelocityFactorArray = autoware_adapi_v1_msgs::msg::VelocityFactorArray;
3941
using VelocityFactor = autoware_adapi_v1_msgs::msg::VelocityFactor;
4042
using SteeringFactorArray = autoware_adapi_v1_msgs::msg::SteeringFactorArray;

‎control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp

+3-3
Original file line numberDiff line numberDiff line change
@@ -53,13 +53,13 @@ std::array<geometry_msgs::msg::Point, 3> triangle2points(
5353
return points;
5454
}
5555

56-
std::map<int, lanelet::ConstLanelet> getRouteLanelets(
56+
std::map<lanelet::Id, lanelet::ConstLanelet> getRouteLanelets(
5757
const lanelet::LaneletMapPtr & lanelet_map,
5858
const lanelet::routing::RoutingGraphPtr & routing_graph,
5959
const autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr & route_ptr,
6060
const double vehicle_length)
6161
{
62-
std::map<int, lanelet::ConstLanelet> route_lanelets;
62+
std::map<lanelet::Id, lanelet::ConstLanelet> route_lanelets;
6363

6464
bool is_route_valid = lanelet::utils::route::isRouteValid(*route_ptr, lanelet_map);
6565
if (!is_route_valid) {
@@ -315,7 +315,7 @@ void LaneDepartureCheckerNode::onTimer()
315315

316316
// In order to wait for both of map and route will be ready, write this not in callback but here
317317
if (last_route_ != route_ && !route_->segments.empty()) {
318-
std::map<int, lanelet::ConstLanelet>::iterator itr;
318+
std::map<lanelet::Id, lanelet::ConstLanelet>::iterator itr;
319319

320320
auto map_route_lanelets_ =
321321
getRouteLanelets(lanelet_map_, routing_graph_, route_, vehicle_length_m_);

‎launch/tier4_system_launch/package.xml

+1-1
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,7 @@
55
<version>0.1.0</version>
66
<description>The tier4_system_launch package</description>
77
<maintainer email="fumihito.ito@tier4.jp">Fumihito Ito</maintainer>
8-
<maintainer email="akihiro.sakurai@tier4.jp">asana17</maintainer>
8+
<maintainer email="tetsuhiro.kawaguchi@tier4.jp">TetsuKawa</maintainer>
99
<license>Apache License 2.0</license>
1010

1111
<buildtool_depend>ament_cmake_auto</buildtool_depend>

‎localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp

+16-46
Original file line numberDiff line numberDiff line change
@@ -44,7 +44,6 @@
4444

4545
#include "ar_tag_based_localizer.hpp"
4646

47-
#include "localization_util/pose_array_interpolator.hpp"
4847
#include "localization_util/util_func.hpp"
4948

5049
#include <Eigen/Core>
@@ -94,6 +93,8 @@ bool ArTagBasedLocalizer::setup()
9493
RCLCPP_ERROR_STREAM(this->get_logger(), "Invalid detection_mode: " << detection_mode);
9594
return false;
9695
}
96+
ekf_pose_buffer_ = std::make_unique<SmartPoseBuffer>(
97+
this->get_logger(), ekf_time_tolerance_, ekf_position_tolerance_);
9798

9899
/*
99100
Log parameter info
@@ -177,24 +178,16 @@ void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg)
177178
return;
178179
}
179180

180-
// get self pose
181181
const builtin_interfaces::msg::Time sensor_stamp = msg->header.stamp;
182-
Pose self_pose;
183-
{
184-
// get self-position on map
185-
std::unique_lock<std::mutex> self_pose_array_lock(self_pose_array_mtx_);
186-
if (self_pose_msg_ptr_array_.size() <= 1) {
187-
RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1, "No Pose!");
188-
return;
189-
}
190-
PoseArrayInterpolator interpolator(
191-
this, sensor_stamp, self_pose_msg_ptr_array_, ekf_time_tolerance_, ekf_position_tolerance_);
192-
if (!interpolator.is_success()) {
193-
return;
194-
}
195-
pop_old_pose(self_pose_msg_ptr_array_, sensor_stamp);
196-
self_pose = interpolator.get_current_pose().pose.pose;
182+
183+
// get self pose
184+
const std::optional<SmartPoseBuffer::InterpolateResult> interpolate_result =
185+
ekf_pose_buffer_->interpolate(sensor_stamp);
186+
if (!interpolate_result) {
187+
return;
197188
}
189+
ekf_pose_buffer_->pop_old(sensor_stamp);
190+
const Pose self_pose = interpolate_result.value().interpolated_pose.pose.pose;
198191

199192
// detect
200193
const std::vector<landmark_manager::Landmark> landmarks = detect_landmarks(msg);
@@ -305,37 +298,14 @@ void ArTagBasedLocalizer::cam_info_callback(const CameraInfo::ConstSharedPtr & m
305298

306299
void ArTagBasedLocalizer::ekf_pose_callback(const PoseWithCovarianceStamped::ConstSharedPtr & msg)
307300
{
308-
// lock mutex for initial pose
309-
std::lock_guard<std::mutex> self_pose_array_lock(self_pose_array_mtx_);
310-
// if rosbag restart, clear buffer
311-
if (!self_pose_msg_ptr_array_.empty()) {
312-
const builtin_interfaces::msg::Time & t_front = self_pose_msg_ptr_array_.front()->header.stamp;
313-
const builtin_interfaces::msg::Time & t_msg = msg->header.stamp;
314-
if (t_front.sec > t_msg.sec || (t_front.sec == t_msg.sec && t_front.nanosec > t_msg.nanosec)) {
315-
self_pose_msg_ptr_array_.clear();
316-
}
317-
}
318-
319301
if (msg->header.frame_id == "map") {
320-
self_pose_msg_ptr_array_.push_back(msg);
302+
ekf_pose_buffer_->push_back(msg);
321303
} else {
322-
TransformStamped transform_self_pose_frame_to_map;
323-
try {
324-
transform_self_pose_frame_to_map = tf_buffer_->lookupTransform(
325-
"map", msg->header.frame_id, msg->header.stamp, rclcpp::Duration::from_seconds(0.1));
326-
327-
// transform self_pose_frame to map_frame
328-
auto self_pose_on_map_ptr = std::make_shared<PoseWithCovarianceStamped>();
329-
self_pose_on_map_ptr->pose.pose =
330-
tier4_autoware_utils::transformPose(msg->pose.pose, transform_self_pose_frame_to_map);
331-
// self_pose_on_map_ptr->pose.covariance; // TODO(YamatoAndo)
332-
self_pose_on_map_ptr->header.stamp = msg->header.stamp;
333-
self_pose_msg_ptr_array_.push_back(self_pose_on_map_ptr);
334-
} catch (tf2::TransformException & ex) {
335-
RCLCPP_WARN(
336-
get_logger(), "cannot get map to %s transform. %s", msg->header.frame_id.c_str(),
337-
ex.what());
338-
}
304+
RCLCPP_ERROR_STREAM_THROTTLE(
305+
get_logger(), *this->get_clock(), 1000,
306+
"Received initial pose message with frame_id "
307+
<< msg->header.frame_id << ", but expected map. "
308+
<< "Please check the frame_id in the input topic and ensure it is correct.");
339309
}
340310
}
341311

0 commit comments

Comments
 (0)