crisp-python 1.2.1__py3-none-any.whl

crisp_py/__init__.py

@@ -0,0 +1,9 @@
+"""Initialize crisp_py."""
+
+try:
+    import rclpy  # noqa: F401
+except ImportError:
+    print("ROS2 should be installed and sourced!")
+
+
+__version__ = "1.2.0"

crisp_py/camera/__init__.py

@@ -0,0 +1,6 @@
+"""Initialize the camera module."""
+
+from crisp_py.camera.camera import Camera
+from crisp_py.camera.camera_config import CameraConfig, FrankaCameraConfig
+
+__import__ = [Camera, CameraConfig, FrankaCameraConfig]

crisp_py/camera/camera.py

@@ -0,0 +1,188 @@
+"""Class defining a camera object."""
+
+import threading
+from typing import Optional, Tuple
+
+import cv2
+import numpy as np
+import rclpy
+import rclpy.executors
+from cv_bridge import CvBridge
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.node import Node
+from rclpy.qos import qos_profile_sensor_data, qos_profile_system_default
+from sensor_msgs.msg import CameraInfo, CompressedImage, Image
+
+from crisp_py.camera.camera_config import CameraConfig
+from crisp_py.utils.callback_monitor import CallbackMonitor
+
+
+class Camera:
+    """High level interface for managing cameras in ROS2.
+
+    Images are stored in RGB8 format as a numpy array of shape (H, W, 3).
+    If resolution is set in the camera configuration, the image will be resized while maintaining the aspect ratio.
+    (Note: The image will be cropped to fit the target resolution if necessary.)
+    """
+
+    THREADS_REQUIRED = 2
+
+    def __init__(
+        self,
+        node: Optional[Node] = None,
+        namespace: str = "",
+        config: Optional[CameraConfig] = None,
+        spin_node: bool = True,
+    ):
+        """Initialize the camera.
+
+        Args:
+            node (Node, optional): ROS2 node to use. If None, creates a new node.
+            namespace (str, optional): ROS2 namespace for the camera.
+            config (CameraConfig): Camera configuration.
+            spin_node (bool, optional): Whether to spin the node in a separate thread.
+        """
+        self.config = config if config else CameraConfig()
+
+        if node is None:
+            if not rclpy.ok():
+                rclpy.init()
+            self.node = rclpy.create_node(self.config.camera_name, namespace=namespace)
+        else:
+            self.node = node
+
+        self._current_image: Optional[np.ndarray] = None
+        self._namespace = namespace
+        self._callback_monitor = CallbackMonitor(
+            self.node,
+            stale_threshold=self.config.max_image_delay,
+        )
+
+        self.cv_bridge = CvBridge()
+
+        # self.node.create_subscription(
+        #     Image,
+        #     self.config.camera_color_image_topic,
+        #     self._callback_current_color_image,
+        #     qos_profile_system_default,
+        #     callback_group=ReentrantCallbackGroup(),
+        # )
+        self.node.create_subscription(
+            CompressedImage,
+            f"{self.config.camera_color_image_topic}/compressed",
+            self._callback_monitor.monitor(
+                f"{self._namespace.capitalize()} Camera {self.config.camera_name} Image".strip(),
+                self._callback_current_color_image,
+            ),
+            qos_profile_sensor_data,
+            callback_group=ReentrantCallbackGroup(),
+        )
+        self.node.create_subscription(
+            CameraInfo,
+            self.config.camera_color_info_topic,
+            self._callback_monitor.monitor(
+                f"{self._namespace.capitalize()} Camera {self.config.camera_name} Info".strip(),
+                self._callback_current_color_info,
+            ),
+            qos_profile_system_default,
+            callback_group=ReentrantCallbackGroup(),
+        )
+
+        if spin_node:
+            threading.Thread(target=self._spin_node, daemon=True).start()
+
+    def _spin_node(self):
+        if not rclpy.ok():
+            rclpy.init()
+        executor = rclpy.executors.MultiThreadedExecutor(num_threads=self.THREADS_REQUIRED)
+        executor.add_node(self.node)
+        while rclpy.ok():
+            executor.spin_once(timeout_sec=0.1)
+
+    def _uncompress(self, compressed_image: CompressedImage) -> Image:
+        """Uncompress a CompressedImage message to an Image message."""
+        return np.asarray(
+            self.cv_bridge.compressed_imgmsg_to_cv2(compressed_image, desired_encoding="rgb8")
+        )
+
+    @property
+    def current_image(self) -> np.ndarray:
+        """Get the current color image."""
+        if self._current_image is None:
+            raise RuntimeError(
+                f"We have not received any images of camera {self.config.camera_name}. Call wait_until_ready to be sure that the camera is available!."
+            )
+        # Check if image callback is stale
+        try:
+            # Try to find the callback - need to handle namespace properly
+            for callback_name in self._callback_monitor.callbacks.keys():
+                if (
+                    "Camera" in callback_name
+                    and self.config.camera_name in callback_name
+                    and "Image" in callback_name
+                ):
+                    image_callback_data = self._callback_monitor.get_callback_data(callback_name)
+                    if image_callback_data and image_callback_data.is_stale:
+                        self.node.get_logger().warn(
+                            f"Camera {self.config.camera_name} image data is stale"
+                        )
+                    break
+        except ValueError:
+            # Callback not found, which is expected if no data has been received yet
+            pass
+        return self._current_image
+
+    @property
+    def resolution(self) -> Tuple[int, int]:
+        """Get the current camera info."""
+        return self.config.resolution if self.config.resolution is not None else (0, 0)
+
+    def is_ready(self) -> bool:
+        """Returns True if camera image and resolution are available."""
+        return self._current_image is not None and self.config.resolution is not None
+
+    def wait_until_ready(self, timeout: float = 10.0, check_frequency: float = 10.0):
+        """Wait until camera image and resolution are available."""
+        rate = self.node.create_rate(check_frequency)
+        while not self.is_ready():
+            rate.sleep()
+            timeout -= 1.0 / check_frequency
+            if timeout <= 0:
+                raise TimeoutError(
+                    f"Timeout waiting for camera ({self.config.camera_name}) to become ready."
+                )
+
+    def _callback_current_color_image(self, msg: Image):
+        """Receive and store the current image."""
+        # raw_image = self._image_to_array(msg)
+        # if self.config.resolution is not None:
+        #     raw_image = self._resize_with_aspect_ratio(raw_image, self.config.resolution)
+        self._current_image = self._resize_with_aspect_ratio(
+            self._uncompress(msg), target_res=self.config.resolution
+        )
+
+    def _callback_current_color_info(self, msg: CameraInfo):
+        """Receive and store the current camera info."""
+        if self.config.resolution is None:
+            self.config.resolution = (msg.height, msg.width)
+
+    def _image_to_array(self, msg: Image) -> np.ndarray:
+        """Converts an Image message to a numpy array."""
+        return np.asarray(self.cv_bridge.imgmsg_to_cv2(msg, desired_encoding="rgb8"))
+
+    def _resize_with_aspect_ratio(self, image: np.ndarray, target_res: tuple) -> np.ndarray:
+        """Resize an image to fit within a target resolution while maintaining aspect ratio, cropping if necessary."""
+        h, w = image.shape[:2]
+        target_h, target_w = target_res
+
+        scale = max(target_w / w, target_h / h)
+        new_w, new_h = int(w * scale), int(h * scale)
+
+        resized = cv2.resize(image, (new_w, new_h), interpolation=cv2.INTER_AREA)
+
+        start_x = (new_w - target_w) // 2
+        start_y = (new_h - target_h) // 2
+
+        cropped_image = resized[start_y : start_y + target_h, start_x : start_x + target_w]
+
+        return cropped_image

crisp_py/camera/camera_config.py

@@ -0,0 +1,31 @@
+"""Camera configuration class."""
+
+from dataclasses import dataclass
+
+
+@dataclass
+class CameraConfig:
+    """Default camera configuration."""
+
+    camera_name: str = "camera"
+    camera_frame: str = "camera_link"
+
+    resolution: tuple[int, int] | None = None
+
+    camera_color_image_topic: str | None = None
+    camera_color_info_topic: str | None = None
+
+    max_image_delay: float = 1.0
+
+
+@dataclass
+class FrankaCameraConfig(CameraConfig):
+    """Example camera configuration used with Franka."""
+
+    camera_name: str = "franka"
+    camera_frame: str = "franka_link"
+
+    resolution: tuple[int, int] | None = (256, 256)
+
+    camera_color_image_topic: str | None = "franka/color/image_raw"
+    camera_color_info_topic: str | None = "franka/color/camera_info"

crisp_py/control/__init__.py

@@ -0,0 +1 @@
+"""Initialize the control module."""

crisp_py/control/controller_switcher.py

@@ -0,0 +1,174 @@
+"""Script to switch to a different ros2_controller."""
+
+import rclpy
+from controller_manager_msgs.srv import (
+    ConfigureController,
+    ListControllers,
+    LoadController,
+    SwitchController,
+)
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.node import Node
+
+
+class ControllerSwitcherClient:
+    """ControllerSwitcher class allows user to communicate with the controller_manager and manage controllers in an easy way."""
+
+    def __init__(
+        self, node: Node, always_active: list[str] = ["joint_state_broadcaster", "pose_broadcaster"]
+    ):
+        """Initialize the ControllerSwitcher.
+
+        Args:
+            node (Node): Node used for the communication with the controller_manager.
+            always_active (list[str], optional): List of controllers that are always active. Defaults to ["joint_state_broadcaster", "franka_robot_state_broadcaster"].
+        """
+        self.node = node
+
+        self.always_active = always_active
+
+        self.load_client = node.create_client(
+            LoadController,
+            "controller_manager/load_controller",
+            callback_group=ReentrantCallbackGroup(),
+        )
+        self.configure_client = node.create_client(
+            ConfigureController,
+            "controller_manager/configure_controller",
+            callback_group=ReentrantCallbackGroup(),
+        )
+        self.list_client = node.create_client(
+            ListControllers,
+            "controller_manager/list_controllers",
+            callback_group=ReentrantCallbackGroup(),
+        )
+        self.switch_client = node.create_client(
+            SwitchController,
+            "controller_manager/switch_controller",
+            callback_group=ReentrantCallbackGroup(),
+        )
+
+    def is_server_ready(self) -> bool:
+        """Return True if all services are ready."""
+        return (
+            self.load_client.wait_for_service(timeout_sec=5.0)
+            and self.configure_client.wait_for_service(timeout_sec=5.0)
+            and self.list_client.wait_for_service(timeout_sec=5.0)
+            and self.switch_client.wait_for_service(timeout_sec=5.0)
+        )
+
+    def get_controller_list(self) -> list:
+        """Get a list of all controllers using a service."""
+        if not self.list_client.wait_for_service(timeout_sec=5.0):
+            raise RuntimeError("Timed out waiting for controller list service.")
+        else:
+            self.node.get_logger().debug("Got controller list service.")
+
+        future = self.list_client.call_async(ListControllers.Request())
+
+        while not future.done():
+            self.node.get_logger().debug(
+                "Waiting for controller list...", throttle_duration_sec=1.0
+            )
+
+        response = future.result()
+
+        return response.controller
+
+    def load_controller(self, controller_name: str) -> bool:
+        """Load a controller using a service."""
+        request = LoadController.Request()
+        request.name = controller_name
+        future = self.load_client.call_async(request)
+
+        while not future.done():
+            self.node.get_logger().debug(
+                "Waiting for load controller answer...", throttle_duration_sec=1.0
+            )
+        response = future.result()
+
+        return response.ok
+
+    def configure_controller(self, controller_name: str) -> bool:
+        """Configure a controller using a service."""
+        request = ConfigureController.Request()
+        request.name = controller_name
+        future = self.configure_client.call_async(request)
+
+        while not future.done():
+            self.node.get_logger().debug(
+                "Waiting for configure controller answer...", throttle_duration_sec=1.0
+            )
+        response = future.result()
+
+        return response.ok
+
+    def _switch_controller(self, to_deactivate: list[str], to_activate: list[str]) -> bool:
+        """Switch to a different ros2_controller using a service."""
+        request = SwitchController.Request()
+        request.deactivate_controllers = to_deactivate
+        request.activate_controllers = to_activate
+        request.strictness = SwitchController.Request.BEST_EFFORT
+        request.activate_asap = True
+        request.timeout = rclpy.duration.Duration(seconds=5).to_msg()
+
+        future = self.switch_client.call_async(request)
+
+        while not future.done():
+            self.node.get_logger().debug(
+                "Waiting for switch controller answer...", throttle_duration_sec=1.0
+            )
+        response = future.result()
+
+        return response.ok
+
+    def switch_controller(self, controller_name: str) -> bool | None:
+        """Switch to a different ros2_controller that is already loaded using a service.
+
+        First we request a list of current controllers.
+        If the desired controller is not loaded, then we request to load it and configure it.
+        Finally, we request to switch to the desired controller.
+
+        Args:
+            controller_name (str): Name of the controller to switch to.
+        """
+        controllers = self.get_controller_list()
+
+        active_controllers = [
+            controller.name for controller in controllers if controller.state == "active"
+        ]
+        inactive_controllers = [
+            controller.name for controller in controllers if controller.state == "inactive"
+        ]
+
+        if controller_name in active_controllers:
+            self.node.get_logger().debug(f"Controller {controller_name} is already active.")
+            return True
+
+        if controller_name not in inactive_controllers:
+            ok = self.load_controller(controller_name)
+            if not ok:
+                self.node.get_logger().error(
+                    f"Failed to load controller {controller_name}. Are you sure the controller exists?"
+                )
+                raise RuntimeError(f"Failed to load controller {controller_name}.")
+
+            ok = self.configure_controller(controller_name)
+            if not ok:
+                self.node.get_logger().error(f"Failed to configure controller {controller_name}.")
+                raise RuntimeError(f"Failed to configure controller {controller_name}.")
+
+        to_deactivate = []
+        for active_controller in active_controllers:
+            if active_controller not in self.always_active:
+                to_deactivate.append(active_controller)
+
+        to_activate = [controller_name]
+
+        ok = self._switch_controller(to_deactivate, to_activate)
+
+        if not ok:
+            self.node.get_logger().error(f"Failed to switch to controller {controller_name}.")
+            raise RuntimeError(f"Failed to switch to controller {controller_name}.")
+
+        return True

crisp_py/control/joint_trajectory_controller_client.py

@@ -0,0 +1,111 @@
+"""A ROS2 Action Client for controlling robot joints through trajectory messages.
+
+This module provides a JointTrajectoryControllerClient class that simplifies sending joint
+trajectories to a ROS2-controlled robot. It handles the communication with the robot's
+joint trajectory controller through ROS2 actions.
+
+The client supports:
+- Sending joint configurations with specified execution time
+- Blocking and non-blocking execution modes
+- Automatic namespace handling for multi-robot setups
+
+Example:
+    ```python
+    import rclpy
+    from rclpy.node import Node
+    from joint_trajectory_controller_client import JointTrajectoryControllerClient
+
+    node = Node("my_control_node")
+    client = JointTrajectoryControllerClient(node)
+
+    # Send a joint configuration
+    joint_names = ["joint1", "joint2", "joint3"]
+    joint_positions = [0.5, -0.3, 1.0]
+    client.send_joint_config(joint_names, joint_positions, time_to_goal=3.0)
+    ```
+"""
+
+from typing import Optional
+
+from control_msgs.action import FollowJointTrajectory
+from rclpy.action import ActionClient
+from rclpy.duration import Duration
+from rclpy.node import Node
+from trajectory_msgs.msg import JointTrajectoryPoint
+
+
+class JointTrajectoryControllerClient(ActionClient):
+    """A ROS2 Action Client for controlling robot joints through trajectory messages.
+
+    This class provides a high-level interface for sending joint trajectories to a ROS2-controlled
+    robot. It inherits from ActionClient and handles the communication with the robot's joint
+    trajectory controller through ROS2 actions.
+
+    Args:
+        node (Node): The ROS2 node that will own this client.
+    """
+
+    def __init__(self, node: Node) -> None:
+        """Initialize the JointTrajectoryControllerClient.
+
+        Args:
+            node (Node): The ROS2 node that will own this client.
+        """
+        self.node = node
+        super().__init__(
+            node, FollowJointTrajectory, "joint_trajectory_controller/follow_joint_trajectory"
+        )
+        self._goal = FollowJointTrajectory.Goal()
+        namespace = self.node.get_namespace().strip("/")
+        self._prefix = f"{namespace}_" if namespace else ""
+
+    def send_joint_config(
+        self,
+        joint_names: list[str],
+        joint_config: list[float],
+        time_to_goal: float = 5.0,
+        blocking: bool = True,
+    ) -> Optional[FollowJointTrajectory.Result]:
+        """Send a joint configuration to the robot.
+
+        Args:
+            joint_names (list[str]): List of joint names to control.
+            joint_config (list[float]): List of joint positions corresponding to joint_names.
+            time_to_goal (float, optional): Time in seconds to reach the goal position. Defaults to 5.0.
+            blocking (bool, optional): Whether to wait for the movement to complete. Defaults to True.
+
+        Returns:
+            Optional[FollowJointTrajectory.Result]: If blocking is True, returns the result of the
+                trajectory execution. If blocking is False, returns None.
+        """
+        self._goal.trajectory.joint_names = [
+            self._prefix + joint_name for joint_name in joint_names
+        ]
+        self._goal.trajectory.header.stamp = self.node.get_clock().now().to_msg()
+        self._goal.trajectory.points = []
+        self._goal.trajectory.points.append(
+            JointTrajectoryPoint(
+                positions=joint_config,
+                velocities=len(joint_config) * [0.0],
+                accelerations=len(joint_config) * [0.0],
+                time_from_start=Duration(seconds=int(time_to_goal), nanoseconds=0).to_msg(),
+            )
+        )
+        future = self.send_goal_async(self._goal)
+
+        if blocking:
+            while not future.done():
+                self.node.get_logger().debug(
+                    "Waiting for goal answer...", throttle_duration_sec=1.0
+                )
+
+            goal_handle = future.result()
+
+            future = goal_handle.get_result_async()
+            while not future.done():
+                self.node.get_logger().debug(
+                    "Waiting for goal result...", throttle_duration_sec=1.0
+                )
+
+            self.node.get_logger().debug(f"Goal result: {future.result()}")
+            return future.result()