From 2bb75e4a99a498fb79d37f9870ec88321de58022 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 17 Apr 2023 15:31:34 +0200 Subject: [PATCH] show position even when invalid --- src/beacon_positioning/src/tracker_position.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/beacon_positioning/src/tracker_position.cpp b/src/beacon_positioning/src/tracker_position.cpp index 4678c6f7..b46b2620 100644 --- a/src/beacon_positioning/src/tracker_position.cpp +++ b/src/beacon_positioning/src/tracker_position.cpp @@ -116,6 +116,11 @@ int main(int argc, char **argv) if (tracker_msg.is_valid_position) { RCLCPP_INFO(node->get_logger(), "x = %f, y = %f, z = %f", tracker_msg.tracker_position_xyz.at(0), tracker_msg.tracker_position_xyz.at(1), tracker_msg.tracker_position_xyz.at(2)); + for (const &anchor_data_t : tracker_msg.anchor_data) + { + RCLCPP_INFO(node->get_logger(), "anchor number= %d, distance = %f, x = %f, y = %f, z = %f", anchor_data_t.number, anchor_data_t.distance, anchor_data_t.pos_x, anchor_data_t.pos_y, anchor_data_t.pos_z); + } + RCLCPP_INFO(node->get_logger(), ""); auto message = std_msgs::msg::String(); message.data = "Hello from tracker callback!"; node->publish(message);