From ba8ead16ce76929be4e3f2099da509f937578e8b Mon Sep 17 00:00:00 2001
From: Paul-Winpenny <92634321+Paul-Winpenny@users.noreply.github.com>
Date: Wed, 18 Dec 2024 14:24:41 +0000
Subject: [PATCH] Bunch of logging statements, issue with converting to a Point
message
---
ros2/src/robobin/robobin/control_feedback.py | 2 +-
ros2/src/robobin/robobin/uwb_navigation_node.py | 8 ++++++--
2 files changed, 7 insertions(+), 3 deletions(-)
diff --git a/ros2/src/robobin/robobin/control_feedback.py b/ros2/src/robobin/robobin/control_feedback.py
index 4b37348c..78a1edb7 100644
--- a/ros2/src/robobin/robobin/control_feedback.py
+++ b/ros2/src/robobin/robobin/control_feedback.py
@@ -252,7 +252,7 @@ class MotorControlNode(Node):
def stop_motors(self):
self.motor.stop()
- self.get_logger().info('Motors have been stopped.')
+ # self.get_logger().info('Motors have been stopped.')
class Motor:
def __init__(self,node, EnaA, In1A, In2A, EnaB, In1B, In2B):
diff --git a/ros2/src/robobin/robobin/uwb_navigation_node.py b/ros2/src/robobin/robobin/uwb_navigation_node.py
index 56ff5210..2ce81956 100644
--- a/ros2/src/robobin/robobin/uwb_navigation_node.py
+++ b/ros2/src/robobin/robobin/uwb_navigation_node.py
@@ -299,18 +299,22 @@ class UWBNode(Node):
if self.mode == "Call" and self.call_mode_active:
+ self.get_logger().info("Call mode active.")
if self.current_tag1_position is not None and self.current_target_index < len(self.path):
+ self.get_logger().info(f"Current target index: {self.current_target_index}")
target_node_name = self.path[self.current_target_index]
target_pos = self.get_node_position(target_node_name)
-
+ self.get_logger().info(f"Target position: {target_pos}")
# Publish current position
position_msg = Point(x=self.current_tag1_position[0], y=self.current_tag1_position[1], z=0.0)
self.start_publisher.publish(position_msg)
+ self.get_logger().info(f"Current position: {self.current_tag1_position} Success!")
+
# Publish target position
target_msg = Point(x=target_pos[0], y=target_pos[1], z=0.0)
self.target_location_pub.publish(target_msg)
-
+ self.get_logger().info(f"Target position: {target_pos} Success!")
# Check if close to the node
dist = math.sqrt((self.current_tag1_position[0] - target_pos[0])**2 +
(self.current_tag1_position[1] - target_pos[1])**2)
--
GitLab