diff --git a/ros2/src/robobin/robobin/control_feedback.py b/ros2/src/robobin/robobin/control_feedback.py index 4b37348ce4354817811c6a2a61d40a319ddc18b6..78a1edb74b03fbdf991c9e38c1b8980ee52b3945 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 56ff5210a3f190397a329dbe8aeb34fed8ff0b12..2ce81956db0f2383a13367cf874c56c9a916b075 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)