From 55dc0ccc563231ee142114fe7f9e697de38dcefe Mon Sep 17 00:00:00 2001 From: fred-labs Date: Wed, 4 Sep 2024 17:48:45 +0200 Subject: [PATCH] gazebo_static_camera: add static transform publisher (#175) --- .../actions/tf_close_to.py | 52 +++++-------------- .../launch/spawn_static_camera_launch.py | 22 +++++++- .../models/camera.sdf.xacro | 13 ++--- .../gazebo/gazebo_static_camera/package.xml | 3 +- 4 files changed, 41 insertions(+), 49 deletions(-) diff --git a/scenario_execution_ros/scenario_execution_ros/actions/tf_close_to.py b/scenario_execution_ros/scenario_execution_ros/actions/tf_close_to.py index 7eb668e5..567b63d9 100644 --- a/scenario_execution_ros/scenario_execution_ros/actions/tf_close_to.py +++ b/scenario_execution_ros/scenario_execution_ros/actions/tf_close_to.py @@ -21,7 +21,6 @@ import py_trees from py_trees.common import Status from visualization_msgs.msg import Marker -from geometry_msgs.msg import PoseStamped from tf2_ros.buffer import Buffer from tf2_ros import TransformException # pylint: disable= no-name-in-module @@ -88,6 +87,8 @@ def setup(self, **kwargs): self.name, self.__class__.__name__ ) raise ActionError(error_message, action=self) from e + + self.reference_point = (float(self.reference_point['x']), float(self.reference_point['y'])) self.feedback_message = f"Waiting for transform map --> base_link" # pylint: disable= attribute-defined-outside-init self.tf_buffer = Buffer() tf_prefix = self.namespace @@ -110,8 +111,8 @@ def setup(self, **kwargs): marker.color.r = 0.5 marker.color.g = 0.5 marker.color.b = 0.5 - marker.pose.position.x = self.reference_point['x'] - marker.pose.position.y = self.reference_point['y'] + marker.pose.position.x = self.reference_point[0] + marker.pose.position.y = self.reference_point[1] marker.pose.position.z = 0.0 self.marker_id = self.marker_handler.add_marker(marker) @@ -119,11 +120,11 @@ def update(self) -> py_trees.common.Status: """ Check if the subscriber already received the right msg while ticking """ - robot_pose, success = self.get_robot_pose_from_tf() + translation, success = self.get_translation_from_tf() if not success: self.feedback_message = f"the pose of {self.robot_frame_id} could not be retrieved from tf" # pylint: disable= attribute-defined-outside-init return Status.RUNNING - dist = self.euclidean_dist(robot_pose.pose.position) + dist = self.euclidean_dist(translation) marker = self.marker_handler.get_marker(self.marker_id) self.success = dist <= self.threshold if self.success: @@ -141,43 +142,18 @@ def update(self) -> py_trees.common.Status: self.marker_handler.update_marker(self.marker_id, marker) return Status.RUNNING - def get_robot_pose_from_tf(self): - ''' - function to get pose of the robot (i.e., the base_link frame) with - respect to the map frame via tf - - returns: - pose: pose of the robot in the map frame - ''' - pose = PoseStamped() - when = self.node.get_clock().now() - if self.sim: - when = rclpy.time.Time() + def get_translation_from_tf(self): + t = None try: - t = self.tf_buffer.lookup_transform( - 'map', - self.robot_frame_id, - when, - timeout=rclpy.duration.Duration(seconds=1.0), - ) + t = self.tf_buffer.lookup_transform('map', self.robot_frame_id, rclpy.time.Time()) self.feedback_message = f"Transform map -> base_link got available." # pylint: disable= attribute-defined-outside-init - except TransformException as ex: + except TransformException as e: self.feedback_message = f"Could not transform map to base_link" # pylint: disable= attribute-defined-outside-init self.node.get_logger().warn( - f'Could not transform map to base_link at time {when}: {ex}') - return pose, False - - pose.header = t.header - pose.header.frame_id = 'map' - - pose.pose.position.x = t.transform.translation.x - pose.pose.position.y = t.transform.translation.y - pose.pose.orientation.x = t.transform.rotation.x - pose.pose.orientation.y = t.transform.rotation.y - pose.pose.orientation.z = t.transform.rotation.z - pose.pose.orientation.w = t.transform.rotation.w + f'Could not transform map to base_link: {e}') + return None, False - return pose, True + return t.transform.translation, True def euclidean_dist(self, pos): ''' @@ -189,7 +165,7 @@ def euclidean_dist(self, pos): return: Euclidean distance in float ''' - return sqrt((self.reference_point['x'] - pos.x) ** 2 + (self.reference_point['y'] - pos.y) ** 2) + return sqrt((self.reference_point[0] - pos.x) ** 2 + (self.reference_point[1] - pos.y) ** 2) def shutdown(self): self.marker_handler.remove_markers([self.marker_id]) diff --git a/simulation/gazebo/gazebo_static_camera/launch/spawn_static_camera_launch.py b/simulation/gazebo/gazebo_static_camera/launch/spawn_static_camera_launch.py index 29b5cabf..aabddb1b 100644 --- a/simulation/gazebo/gazebo_static_camera/launch/spawn_static_camera_launch.py +++ b/simulation/gazebo/gazebo_static_camera/launch/spawn_static_camera_launch.py @@ -93,11 +93,31 @@ def generate_launch_description(): '-Y', yaw, '-param', 'robot_description'], parameters=[{ - 'robot_description': Command(['xacro ', 'update_rate:=', update_rate, ' image_width:=', image_width, ' image_height:=', image_height, ' ', PathJoinSubstitution([gazebo_static_camera_dir, "models", "camera.sdf.xacro"])])}], + 'robot_description': Command(['xacro ', 'update_rate:=', update_rate, ' image_width:=', image_width, ' image_height:=', image_height, ' camera_name:=', camera_name, ' ', PathJoinSubstitution([gazebo_static_camera_dir, "models", "camera.sdf.xacro"])])}], output='screen' ) + static_transform = Node( + package='tf2_ros', + executable='static_transform_publisher', + name='static_camera_transform', + arguments=['--x', x, '--y', y, '--z', z, '--roll', roll, '--pitch', pitch, + '--yaw', yaw, '--frame-id', '/map', '--child-frame-id', camera_name], + remappings=[('/tf_static', 'tf_static')], + ) + + static_transform_2 = Node( + package='tf2_ros', + executable='static_transform_publisher', + name='static_camera_transform_2', + arguments=['--x', '0', '--y', '0', '--z', '0', '--roll', "-1.57", '--pitch', "0", '--yaw', + "-1.57", '--frame-id', camera_name, '--child-frame-id', [camera_name, '_optical_frame']], + remappings=[('/tf_static', 'tf_static')], + ) + ld = LaunchDescription(ARGUMENTS) ld.add_action(camera_bridge) ld.add_action(spawn_camera) + ld.add_action(static_transform) + ld.add_action(static_transform_2) return ld diff --git a/simulation/gazebo/gazebo_static_camera/models/camera.sdf.xacro b/simulation/gazebo/gazebo_static_camera/models/camera.sdf.xacro index 28ee7986..bd85f69e 100644 --- a/simulation/gazebo/gazebo_static_camera/models/camera.sdf.xacro +++ b/simulation/gazebo/gazebo_static_camera/models/camera.sdf.xacro @@ -3,11 +3,11 @@ + true - 0.05 0.05 0.05 0 0 0 0.1 @@ -16,14 +16,8 @@ 0.000166667 - - - - 0.1 0.1 0.1 - - - + -0.05 -0.05 -0.05 0 0 0 0.1 0.1 0.1 @@ -32,6 +26,7 @@ + $(arg camera_name)_optical_frame 1.047 $(arg image_width) @@ -48,4 +43,4 @@ - \ No newline at end of file + diff --git a/simulation/gazebo/gazebo_static_camera/package.xml b/simulation/gazebo/gazebo_static_camera/package.xml index 62b5d6bd..1d0be96f 100644 --- a/simulation/gazebo/gazebo_static_camera/package.xml +++ b/simulation/gazebo/gazebo_static_camera/package.xml @@ -12,7 +12,8 @@ ros_gz_bridge ros_ign_gazebo xacro - + tf2_ros + ament_copyright ament_flake8 ament_pep257