gazebo_static_camera: add static transform publisher (#175)

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,20 +111,20 @@ 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)
 
     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])

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

simulation/gazebo/gazebo_static_camera/models/camera.sdf.xacro

@@ -3,11 +3,11 @@
   <xacro:arg name="image_width" default="320"/>
   <xacro:arg name="image_height" default="240"/>
   <xacro:arg name="update_rate" default="5"/>
+  <xacro:arg name="camera_name" default="camera"/>
 
   <model name="camera">
     <static>true</static>
       <link name="link">
-        <pose>0.05 0.05 0.05 0 0 0</pose>
         <inertial>
           <mass>0.1</mass>
           <inertia>
@@ -16,14 +16,8 @@
             <izz>0.000166667</izz>
           </inertia>
         </inertial>
-        <collision name="collision">
-          <geometry>
-            <box>
-              <size>0.1 0.1 0.1</size>
-            </box>
-          </geometry>
-        </collision>
         <visual name="visual">
+          <pose>-0.05 -0.05 -0.05 0 0 0</pose>
           <geometry>
             <box>
               <size>0.1 0.1 0.1</size>
@@ -32,6 +26,7 @@
         </visual>
         <sensor name="camera" type="camera">
           <camera>
+            <optical_frame_id>$(arg camera_name)_optical_frame</optical_frame_id>
             <horizontal_fov>1.047</horizontal_fov>
             <image>
               <width>$(arg image_width)</width>
@@ -48,4 +43,4 @@
         </sensor>
       </link>
     </model>
-</sdf>
+</sdf>

simulation/gazebo/gazebo_static_camera/package.xml

@@ -12,7 +12,8 @@
   <exec_depend>ros_gz_bridge</exec_depend>
   <exec_depend>ros_ign_gazebo</exec_depend>
   <exec_depend>xacro</exec_depend>
-
+  <exec_depend>tf2_ros</exec_depend>
+
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
   <test_depend>ament_pep257</test_depend>