diff --git a/mil_common/utils/mil_tools/mil_ros_tools/image_helpers.py b/mil_common/utils/mil_tools/mil_ros_tools/image_helpers.py index d87d7f487..b99561c17 100644 --- a/mil_common/utils/mil_tools/mil_ros_tools/image_helpers.py +++ b/mil_common/utils/mil_tools/mil_ros_tools/image_helpers.py @@ -24,14 +24,16 @@ def get_parameter_range(parameter_root: str): """ low_param, high_param = parameter_root + "/hsv_low", parameter_root + "/hsv_high" - rclpy.logwarn(f"Blocking -- waiting for parameters {low_param} and {high_param}") + Node.get_logger().warn( + f"Blocking -- waiting for parameters {low_param} and {high_param}", + ) wait_for_param(low_param) wait_for_param(high_param) low = Node.declare_parameter(low_param) high = Node.declare_parameter(high_param) - Node.get_logger.info()(f"Got {low_param} and {high_param}") + Node.get_logger().info()(f"Got {low_param} and {high_param}") return np.array([low, high]).transpose() @@ -82,7 +84,7 @@ def publish(self, cv_image: np.ndarray): self.im_pub.publish(image_message) except CvBridgeError as e: # Intentionally absorb CvBridge Errors - rclpy.logerr(str(e)) + self.get_logger().warn(str(e)) class Image_Subscriber: @@ -142,7 +144,7 @@ def wait_for_camera_info(self, timeout: int = 10): Raises: Exception: No camera info was found after the timeout had finished. """ - rclpy.logwarn( + self.get_logger().warn( "Blocking -- waiting at most %d seconds for camera info." % timeout, ) @@ -152,11 +154,11 @@ def wait_for_camera_info(self, timeout: int = 10): while (rclpy.Time.now() - start_time < timeout) and (not rclpy.is_shutdown()): if self.camera_info is not None: - rclpy.loginfo("Camera info found!") + self.get_logger().info("Camera info found!") return self.camera_info rclpy.sleep(0.2) - rclpy.logerr("Camera info not found.") + self.get_logger().warn("Camera info not found.") raise Exception("Camera info not found.") def wait_for_camera_model(self, **kwargs): @@ -204,7 +206,7 @@ def convert(self, data: Image): self.callback(image) except CvBridgeError as e: # Intentionally absorb CvBridge Errors - rclpy.logerr(e) + self.get_logger().warn(e) class StereoImageSubscriber: @@ -377,4 +379,4 @@ def _image_callback(self, left_img: Image, right_img: Image): self.callback(img_left, img_right) except CvBridgeError as e: # Intentionally absorb CvBridge Errors - rclpy.logerr(e) + self.get_logger().warn(e) diff --git a/mil_common/utils/mil_tools/mil_ros_tools/init_helpers.py b/mil_common/utils/mil_tools/mil_ros_tools/init_helpers.py index c1d10653e..2db7cc5df 100644 --- a/mil_common/utils/mil_tools/mil_ros_tools/init_helpers.py +++ b/mil_common/utils/mil_tools/mil_ros_tools/init_helpers.py @@ -105,5 +105,5 @@ def wait_for_service( except rclpy.ROSException: if timeout is not None: timeout = timeout - warn_time - rclpy.logwarn(warn_msg) + Node.get_logger().warm(warn_msg) service.wait_for_service(timeout) diff --git a/mil_common/utils/mil_tools/mil_ros_tools/mag_to_marker.py b/mil_common/utils/mil_tools/mil_ros_tools/mag_to_marker.py index be4e78347..9163987bd 100755 --- a/mil_common/utils/mil_tools/mil_ros_tools/mag_to_marker.py +++ b/mil_common/utils/mil_tools/mil_ros_tools/mag_to_marker.py @@ -60,7 +60,7 @@ def publish(self, vec: MagneticField): if self.length is not None: norm = np.linalg.norm(vec) if norm == 0: - rclpy.logwarn("Zero vector received, skipping") + self.get_logger().warn("Zero vector received, skipping") return vec = (self.length / norm) * vec marker.points.append(numpy_to_point(vec)) diff --git a/mil_common/utils/mil_tools/mil_ros_tools/vector_to_marker.py b/mil_common/utils/mil_tools/mil_ros_tools/vector_to_marker.py index 081619edf..25d75e211 100755 --- a/mil_common/utils/mil_tools/mil_ros_tools/vector_to_marker.py +++ b/mil_common/utils/mil_tools/mil_ros_tools/vector_to_marker.py @@ -63,7 +63,7 @@ def publish(self, vec): if self.length is not None: norm = np.linalg.norm(vec) if norm == 0: - rclpy.logwarn("Zero vector received, skipping") + self.get_logger().warn("Zero vector received, skipping") return vec = (self.length / norm) * vec marker.points.append(numpy_to_point(vec)) diff --git a/mil_common/utils/mil_tools/nodes/network_broadcaster.py b/mil_common/utils/mil_tools/nodes/network_broadcaster.py index ee328f64e..69b8a2a64 100755 --- a/mil_common/utils/mil_tools/nodes/network_broadcaster.py +++ b/mil_common/utils/mil_tools/nodes/network_broadcaster.py @@ -27,7 +27,7 @@ def __init__(self): hz = self.declare_parameter("~hz", 20) topic = self.declare_parameter("~topic", "network") - rclpy.loginfo(f"NETWORK BROADCASTER: publishing to {topic} at {hz}hz") + self.get_logger().info(f"NETWORK BROADCASTER: publishing to {topic} at {hz}hz") self.msg = Header() self.msg.seq = 0 self.num_connections = -1 diff --git a/mil_common/utils/mil_tools/nodes/online_bagger.py b/mil_common/utils/mil_tools/nodes/online_bagger.py index 5839afd38..288d9dcf9 100755 --- a/mil_common/utils/mil_tools/nodes/online_bagger.py +++ b/mil_common/utils/mil_tools/nodes/online_bagger.py @@ -61,7 +61,7 @@ def __init__(self): self.streaming = True self.get_params() if len(self.subscriber_list) == 0: - rclpy.logwarn("No topics selected to subscribe to. Closing.") + self.get_logger().warn("No topics selected to subscribe to. Closing.") rclpy.signal_shutdown("No topics to subscribe to") return self.make_dicts()