|
| 1 | +// Copyright 2025 TIER IV, Inc. |
| 2 | +// |
| 3 | +// Licensed under the Apache License, Version 2.0 (the "License"); |
| 4 | +// you may not use this file except in compliance with the License. |
| 5 | +// You may obtain a copy of the License at |
| 6 | +// |
| 7 | +// http://www.apache.org/licenses/LICENSE-2.0 |
| 8 | +// |
| 9 | +// Unless required by applicable law or agreed to in writing, software |
| 10 | +// distributed under the License is distributed on an "AS IS" BASIS, |
| 11 | +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 12 | +// See the License for the specific language governing permissions and |
| 13 | +// limitations under the License. |
| 14 | + |
| 15 | +#include "autoware/multi_object_tracker/odometry.hpp" |
| 16 | + |
| 17 | +#include "autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp" |
| 18 | + |
| 19 | +#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp> |
| 20 | + |
| 21 | +#include <tf2_ros/create_timer_ros.h> |
| 22 | + |
| 23 | +#include <memory> |
| 24 | +#include <string> |
| 25 | + |
| 26 | +namespace autoware::multi_object_tracker |
| 27 | +{ |
| 28 | + |
| 29 | +Odometry::Odometry( |
| 30 | + rclcpp::Node & node, const std::string & world_frame_id, bool enable_odometry_uncertainty) |
| 31 | +: node_(node), |
| 32 | + world_frame_id_(world_frame_id), |
| 33 | + tf_buffer_(node_.get_clock()), |
| 34 | + tf_listener_(tf_buffer_), |
| 35 | + enable_odometry_uncertainty_(enable_odometry_uncertainty) |
| 36 | +{ |
| 37 | + // Create tf timer |
| 38 | + auto cti = std::make_shared<tf2_ros::CreateTimerROS>( |
| 39 | + node_.get_node_base_interface(), node_.get_node_timers_interface()); |
| 40 | + tf_buffer_.setCreateTimerInterface(cti); |
| 41 | +} |
| 42 | + |
| 43 | +void Odometry::updateTfCache( |
| 44 | + const rclcpp::Time & time, const geometry_msgs::msg::Transform & tf) const |
| 45 | +{ |
| 46 | + // update the tf buffer |
| 47 | + tf_cache_.emplace(time, tf); |
| 48 | + |
| 49 | + // remove too old tf |
| 50 | + const auto max_tf_age = rclcpp::Duration::from_seconds(1.0); |
| 51 | + for (auto it = tf_cache_.begin(); it != tf_cache_.end();) { |
| 52 | + if (time - it->first > max_tf_age) { |
| 53 | + it = tf_cache_.erase(it); |
| 54 | + } else { |
| 55 | + ++it; |
| 56 | + } |
| 57 | + } |
| 58 | +} |
| 59 | + |
| 60 | +std::optional<geometry_msgs::msg::Transform> Odometry::getTransform(const rclcpp::Time & time) const |
| 61 | +{ |
| 62 | + // check buffer and return if the transform is found |
| 63 | + for (const auto & tf : tf_cache_) { |
| 64 | + if (tf.first == time) { |
| 65 | + return std::optional<geometry_msgs::msg::Transform>(tf.second); |
| 66 | + } |
| 67 | + } |
| 68 | + // if not found, get the transform from tf |
| 69 | + return getTransform(ego_frame_id_, time); |
| 70 | +} |
| 71 | + |
| 72 | +std::optional<geometry_msgs::msg::Transform> Odometry::getTransform( |
| 73 | + const std::string & source_frame_id, const rclcpp::Time & time) const |
| 74 | +{ |
| 75 | + try { |
| 76 | + // Check if the frames are ready |
| 77 | + std::string errstr; // This argument prevents error msg from being displayed in the terminal. |
| 78 | + if (!tf_buffer_.canTransform( |
| 79 | + world_frame_id_, source_frame_id, tf2::TimePointZero, tf2::Duration::zero(), &errstr)) { |
| 80 | + return std::nullopt; |
| 81 | + } |
| 82 | + |
| 83 | + // Lookup the transform |
| 84 | + geometry_msgs::msg::TransformStamped self_transform_stamped; |
| 85 | + self_transform_stamped = tf_buffer_.lookupTransform( |
| 86 | + world_frame_id_, source_frame_id, time, rclcpp::Duration::from_seconds(0.5)); |
| 87 | + |
| 88 | + // update the cache |
| 89 | + if (source_frame_id == ego_frame_id_) { |
| 90 | + updateTfCache(time, self_transform_stamped.transform); |
| 91 | + } |
| 92 | + |
| 93 | + return std::optional<geometry_msgs::msg::Transform>(self_transform_stamped.transform); |
| 94 | + } catch (tf2::TransformException & ex) { |
| 95 | + RCLCPP_WARN_STREAM(rclcpp::get_logger("multi_object_tracker"), ex.what()); |
| 96 | + return std::nullopt; |
| 97 | + } |
| 98 | +} |
| 99 | + |
| 100 | +std::optional<nav_msgs::msg::Odometry> Odometry::getOdometryFromTf(const rclcpp::Time & time) const |
| 101 | +{ |
| 102 | + const auto self_transform = getTransform(time); |
| 103 | + if (!self_transform) { |
| 104 | + return std::nullopt; |
| 105 | + } |
| 106 | + const auto current_transform = self_transform.value(); |
| 107 | + |
| 108 | + nav_msgs::msg::Odometry odometry; |
| 109 | + { |
| 110 | + odometry.header.stamp = time + rclcpp::Duration::from_seconds(0.00001); |
| 111 | + |
| 112 | + // set the odometry pose |
| 113 | + auto & odom_pose = odometry.pose.pose; |
| 114 | + odom_pose.position.x = current_transform.translation.x; |
| 115 | + odom_pose.position.y = current_transform.translation.y; |
| 116 | + odom_pose.position.z = current_transform.translation.z; |
| 117 | + odom_pose.orientation = current_transform.rotation; |
| 118 | + |
| 119 | + // set odometry twist |
| 120 | + auto & odom_twist = odometry.twist.twist; |
| 121 | + odom_twist.linear.x = 10.0; // m/s |
| 122 | + odom_twist.linear.y = 0.1; // m/s |
| 123 | + odom_twist.angular.z = 0.1; // rad/s |
| 124 | + |
| 125 | + // model the uncertainty |
| 126 | + auto & odom_pose_cov = odometry.pose.covariance; |
| 127 | + odom_pose_cov[0] = 0.1; // x-x |
| 128 | + odom_pose_cov[7] = 0.1; // y-y |
| 129 | + odom_pose_cov[35] = 0.0001; // yaw-yaw |
| 130 | + |
| 131 | + auto & odom_twist_cov = odometry.twist.covariance; |
| 132 | + odom_twist_cov[0] = 2.0; // x-x [m^2/s^2] |
| 133 | + odom_twist_cov[7] = 0.2; // y-y [m^2/s^2] |
| 134 | + odom_twist_cov[35] = 0.001; // yaw-yaw [rad^2/s^2] |
| 135 | + } |
| 136 | + |
| 137 | + return std::optional<nav_msgs::msg::Odometry>(odometry); |
| 138 | +} |
| 139 | + |
| 140 | +std::optional<types::DynamicObjectList> Odometry::transformObjects( |
| 141 | + const types::DynamicObjectList & input_objects) const |
| 142 | +{ |
| 143 | + types::DynamicObjectList output_objects = input_objects; |
| 144 | + |
| 145 | + // transform to world coordinate |
| 146 | + if (input_objects.header.frame_id != world_frame_id_) { |
| 147 | + output_objects.header.frame_id = world_frame_id_; |
| 148 | + tf2::Transform tf_target2objects_world; |
| 149 | + tf2::Transform tf_target2objects; |
| 150 | + tf2::Transform tf_objects_world2objects; |
| 151 | + { |
| 152 | + const auto ros_target2objects_world = |
| 153 | + getTransform(input_objects.header.frame_id, input_objects.header.stamp); |
| 154 | + if (!ros_target2objects_world) { |
| 155 | + return std::nullopt; |
| 156 | + } |
| 157 | + tf2::fromMsg(*ros_target2objects_world, tf_target2objects_world); |
| 158 | + } |
| 159 | + for (auto & object : output_objects.objects) { |
| 160 | + auto & pose_with_cov = object.kinematics.pose_with_covariance; |
| 161 | + tf2::fromMsg(pose_with_cov.pose, tf_objects_world2objects); |
| 162 | + tf_target2objects = tf_target2objects_world * tf_objects_world2objects; |
| 163 | + // transform pose, frame difference and object pose |
| 164 | + tf2::toMsg(tf_target2objects, pose_with_cov.pose); |
| 165 | + // transform covariance, only the frame difference |
| 166 | + pose_with_cov.covariance = |
| 167 | + tf2::transformCovariance(pose_with_cov.covariance, tf_target2objects_world); |
| 168 | + } |
| 169 | + } |
| 170 | + // Add the odometry uncertainty to the object uncertainty |
| 171 | + if (enable_odometry_uncertainty_) { |
| 172 | + // Create a modeled odometry message |
| 173 | + const auto odometry = getOdometryFromTf(input_objects.header.stamp); |
| 174 | + // Add the odometry uncertainty to the object uncertainty |
| 175 | + uncertainty::addOdometryUncertainty(odometry.value(), output_objects); |
| 176 | + } |
| 177 | + |
| 178 | + return std::optional<types::DynamicObjectList>(output_objects); |
| 179 | +} |
| 180 | + |
| 181 | +} // namespace autoware::multi_object_tracker |
0 commit comments