diff --git a/simulator/dummy_perception_publisher/src/node.cpp b/simulator/dummy_perception_publisher/src/node.cpp index da5939bccea7d..cf4caf61d01e8 100644 --- a/simulator/dummy_perception_publisher/src/node.cpp +++ b/simulator/dummy_perception_publisher/src/node.cpp @@ -402,6 +402,18 @@ void DummyPerceptionPublisherNode::objectCallback( dummy_perception_publisher::msg::Object object; objects_.at(i) = *msg; tf2::toMsg(tf_map2object_origin, objects_.at(i).initial_state.pose_covariance.pose); + + // Use base_link Z + geometry_msgs::msg::TransformStamped ros_map2base_link; + try { + ros_map2base_link = tf_buffer_.lookupTransform( + "map", "base_link", rclcpp::Time(0), rclcpp::Duration::from_seconds(0.5)); + objects_.at(i).initial_state.pose_covariance.pose.position.z = + ros_map2base_link.transform.translation.z; + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); + return; + } break; } }