Skip to content

Commit f89751b

Browse files
SakodaShintaroSakodaShintaro
authored
fix(localization): add SmartPoseBuffer to handle ekf pose (#5713)
* Added smart_pose_buffer Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Added rpy_*_to_quaternion Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp> * Fixed arg name Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp> * Added test_smart_pose_buffer Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp> * Fixed time jump issue in SmartPoseBuffer Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp> * Removed pose_array_interpolator Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp> * Removed unused functions Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp> * Added a test about detect_time_jump_to_past Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed process of frame_id mismatch in pose callback Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed as pointed out by linter Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> * Fixed mutex comment in smart_pose_buffer.hpp Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp> * Added comment about covariance interpolation in SmartPoseBuffer Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp> * Added const to some variables Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp> * Fixed lock range Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp> * Added const to some variables Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp> * Removed a waste cast Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp> * Fixed the code about get self pose Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp> * Removed detect_time_jump_to_past Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp> --------- Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp> Signed-off-by: Shintaro SAKODA <shintaro.sakoda@tier4.jp>
1 parent b5417a9 commit f89751b

File tree

11 files changed

+382
-360
lines changed

11 files changed

+382
-360
lines changed

‎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

‎localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -46,6 +46,7 @@
4646
#define AR_TAG_BASED_LOCALIZER_HPP_
4747

4848
#include "landmark_manager/landmark_manager.hpp"
49+
#include "localization_util/smart_pose_buffer.hpp"
4950

5051
#include <image_transport/image_transport.hpp>
5152
#include <rclcpp/rclcpp.hpp>
@@ -124,8 +125,7 @@ class ArTagBasedLocalizer : public rclcpp::Node
124125
aruco::MarkerDetector detector_;
125126
aruco::CameraParameters cam_param_;
126127
bool cam_info_received_;
127-
std::mutex self_pose_array_mtx_;
128-
std::deque<PoseWithCovarianceStamped::ConstSharedPtr> self_pose_msg_ptr_array_;
128+
std::unique_ptr<SmartPoseBuffer> ekf_pose_buffer_;
129129
std::map<std::string, Pose> landmark_map_;
130130
};
131131

‎localization/localization_util/CMakeLists.txt

+9-1
Original file line numberDiff line numberDiff line change
@@ -6,10 +6,18 @@ autoware_package()
66

77
ament_auto_add_library(localization_util SHARED
88
src/util_func.cpp
9-
src/pose_array_interpolator.cpp
109
src/tf2_listener_module.cpp
10+
src/smart_pose_buffer.cpp
1111
)
1212

13+
if(BUILD_TESTING)
14+
find_package(ament_cmake_gtest REQUIRED)
15+
ament_auto_add_gtest(test_smart_pose_buffer
16+
test/test_smart_pose_buffer.cpp
17+
src/smart_pose_buffer.cpp
18+
)
19+
endif()
20+
1321
ament_auto_package(
1422
INSTALL_TO_SHARE
1523
)

‎localization/localization_util/include/localization_util/pose_array_interpolator.hpp ‎localization/localization_util/include/localization_util/smart_pose_buffer.hpp

+26-20
Original file line numberDiff line numberDiff line change
@@ -12,8 +12,8 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#ifndef LOCALIZATION_UTIL__POSE_ARRAY_INTERPOLATOR_HPP_
16-
#define LOCALIZATION_UTIL__POSE_ARRAY_INTERPOLATOR_HPP_
15+
#ifndef LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_
16+
#define LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_
1717

1818
#include "localization_util/util_func.hpp"
1919

@@ -23,33 +23,39 @@
2323

2424
#include <deque>
2525

26-
class PoseArrayInterpolator
26+
class SmartPoseBuffer
2727
{
2828
private:
2929
using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
3030

3131
public:
32-
PoseArrayInterpolator(
33-
rclcpp::Node * node, const rclcpp::Time & target_ros_time,
34-
const std::deque<PoseWithCovarianceStamped::ConstSharedPtr> & pose_msg_ptr_array,
35-
const double & pose_timeout_sec, const double & pose_distance_tolerance_meters);
32+
struct InterpolateResult
33+
{
34+
PoseWithCovarianceStamped old_pose;
35+
PoseWithCovarianceStamped new_pose;
36+
PoseWithCovarianceStamped interpolated_pose;
37+
};
3638

37-
PoseArrayInterpolator(
38-
rclcpp::Node * node, const rclcpp::Time & target_ros_time,
39-
const std::deque<PoseWithCovarianceStamped::ConstSharedPtr> & pose_msg_ptr_array);
39+
SmartPoseBuffer() = delete;
40+
SmartPoseBuffer(
41+
const rclcpp::Logger & logger, const double & pose_timeout_sec,
42+
const double & pose_distance_tolerance_meters);
4043

41-
PoseWithCovarianceStamped get_current_pose();
42-
PoseWithCovarianceStamped get_old_pose();
43-
PoseWithCovarianceStamped get_new_pose();
44-
[[nodiscard]] bool is_success() const;
44+
std::optional<InterpolateResult> interpolate(const rclcpp::Time & target_ros_time);
45+
46+
void push_back(const PoseWithCovarianceStamped::ConstSharedPtr & pose_msg_ptr);
47+
48+
void pop_old(const rclcpp::Time & target_ros_time);
49+
50+
void clear();
4551

4652
private:
4753
rclcpp::Logger logger_;
48-
rclcpp::Clock clock_;
49-
const PoseWithCovarianceStamped::SharedPtr current_pose_ptr_;
50-
PoseWithCovarianceStamped::SharedPtr old_pose_ptr_;
51-
PoseWithCovarianceStamped::SharedPtr new_pose_ptr_;
52-
bool success_;
54+
std::deque<PoseWithCovarianceStamped::ConstSharedPtr> pose_buffer_;
55+
std::mutex mutex_; // This mutex is for pose_buffer_
56+
57+
const double pose_timeout_sec_;
58+
const double pose_distance_tolerance_meters_;
5359

5460
[[nodiscard]] bool validate_time_stamp_difference(
5561
const rclcpp::Time & target_time, const rclcpp::Time & reference_time,
@@ -59,4 +65,4 @@ class PoseArrayInterpolator
5965
const geometry_msgs::msg::Point & reference_point, const double distance_tolerance_m_) const;
6066
};
6167

62-
#endif // LOCALIZATION_UTIL__POSE_ARRAY_INTERPOLATOR_HPP_
68+
#endif // LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_

‎localization/localization_util/include/localization_util/util_func.hpp

+6-16
Original file line numberDiff line numberDiff line change
@@ -46,16 +46,14 @@ geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::Pose & pose);
4646
geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseStamped & pose);
4747
geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseWithCovarianceStamped & pose);
4848

49+
geometry_msgs::msg::Quaternion rpy_rad_to_quaternion(
50+
const double r_rad, const double p_rad, const double y_rad);
51+
geometry_msgs::msg::Quaternion rpy_deg_to_quaternion(
52+
const double r_deg, const double p_deg, const double y_deg);
53+
4954
geometry_msgs::msg::Twist calc_twist(
5055
const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b);
5156

52-
void get_nearest_timestamp_pose(
53-
const std::deque<geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr> &
54-
pose_cov_msg_ptr_array,
55-
const rclcpp::Time & time_stamp,
56-
geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & output_old_pose_cov_msg_ptr,
57-
geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & output_new_pose_cov_msg_ptr);
58-
5957
geometry_msgs::msg::PoseStamped interpolate_pose(
6058
const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b,
6159
const rclcpp::Time & time_stamp);
@@ -64,19 +62,11 @@ geometry_msgs::msg::PoseStamped interpolate_pose(
6462
const geometry_msgs::msg::PoseWithCovarianceStamped & pose_a,
6563
const geometry_msgs::msg::PoseWithCovarianceStamped & pose_b, const rclcpp::Time & time_stamp);
6664

67-
void pop_old_pose(
68-
std::deque<geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr> &
69-
pose_cov_msg_ptr_array,
70-
const rclcpp::Time & time_stamp);
71-
7265
Eigen::Affine3d pose_to_affine3d(const geometry_msgs::msg::Pose & ros_pose);
7366
Eigen::Matrix4f pose_to_matrix4f(const geometry_msgs::msg::Pose & ros_pose);
7467
geometry_msgs::msg::Pose matrix4f_to_pose(const Eigen::Matrix4f & eigen_pose_matrix);
7568
Eigen::Vector3d point_to_vector3d(const geometry_msgs::msg::Point & ros_pos);
7669

77-
std::vector<geometry_msgs::msg::Pose> create_random_pose_array(
78-
const geometry_msgs::msg::PoseWithCovarianceStamped & base_pose_with_cov, const int particle_num);
79-
8070
template <class T>
8171
T transform(const T & input, const geometry_msgs::msg::TransformStamped & transform)
8272
{
@@ -88,7 +78,7 @@ T transform(const T & input, const geometry_msgs::msg::TransformStamped & transf
8878
double norm(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2);
8979

9080
void output_pose_with_cov_to_log(
91-
const rclcpp::Logger logger, const std::string & prefix,
81+
const rclcpp::Logger & logger, const std::string & prefix,
9282
const geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_cov);
9383

9484
#endif // LOCALIZATION_UTIL__UTIL_FUNC_HPP_

‎localization/localization_util/src/pose_array_interpolator.cpp

-109
This file was deleted.

0 commit comments

Comments
 (0)