diff --git a/opennav_docking/src/simple_charging_dock.cpp b/opennav_docking/src/simple_charging_dock.cpp index a577868..cde4d77 100644 --- a/opennav_docking/src/simple_charging_dock.cpp +++ b/opennav_docking/src/simple_charging_dock.cpp @@ -182,7 +182,10 @@ bool SimpleChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pose) // Validate that external pose is new enough auto timeout = rclcpp::Duration::from_seconds(external_detection_timeout_); if (node_->now() - detected.header.stamp > timeout) { - RCLCPP_WARN(node_->get_logger(), "Lost detection or did not detect: timeout exceeded"); + RCLCPP_WARN( + node_->get_logger(), "Lost detection or did not detect: " + "timeout exceeded (is %2.2f seconds old)", + static_cast((node_->now() - detected.header.stamp).seconds())); return false; } @@ -195,12 +198,19 @@ bool SimpleChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pose) pose.header.frame_id, detected.header.frame_id, detected.header.stamp, rclcpp::Duration::from_seconds(0.2))) { - RCLCPP_WARN(node_->get_logger(), "Failed to transform detected dock pose"); + RCLCPP_WARN( + node_->get_logger(), "Failed to transform detected dock pose: " + "cannot transform %s to %s (at time %2.2f s)", + detected.header.frame_id.c_str(), + pose.header.frame_id.c_str(), + static_cast( + detected.header.stamp.sec + detected.header.stamp.nanosec * 1e-9 + )); return false; } tf2_buffer_->transform(detected, detected, pose.header.frame_id); } catch (const tf2::TransformException & ex) { - RCLCPP_WARN(node_->get_logger(), "Failed to transform detected dock pose"); + RCLCPP_WARN(node_->get_logger(), "Failed to transform detected dock pose: %s", ex.what()); return false; } }