Avoid the struggle in the last exercise of chapter 2: move_generic_model.py!

[bluebird]

I provide reminders that may prevent the other students stuck with similar difficulties.

1. The cam_bot will not show in the simulation. In fifty times that I turn on the environment, I just see cam_bot once. The topic robot_description seems not working either. I think it may be related to the following errors. But, remember that the TF is always OK. You can see the robot’s bones (frames) in RVIZ2.
   

   

2. You need a launch file for this exercise. For one reason, there are not enough terminal windows. The first terminal may be ‘publish_static_transform_odom_to_world.launch.py’ for the World to Odom. The second is ‘cam_bot_odom_to_tf_pub’ for world to camera_bot_base_link. RVIZ2 is in the third terminal, and this exercise is in the last. There will be no terminal to send speed command to drive the cam_bot.

3. Don’t forget to add ‘use_sim_time’; otherwise, the following line will fail.

            trans = self.tf_buffer.lookup_transform(
                origin_frame,
                dest_frame,
                now)

I will paste my launch file.

#!/usr/bin/env python3
#
#
# Authors: Blue Bird

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription
from launch_ros.actions import Node
from launch.conditions import IfCondition, UnlessCondition
from launch.substitutions import Command, LaunchConfiguration


def generate_launch_description():
    # Create the launch description and populate
    ld = LaunchDescription()

    cam_bot_odom_to_tf_pub_cmd = Node(
        package='my_tf_ros2_course_pkg',
        executable='cam_bot_odom_to_tf_pub',
        name='cam_bot_odom_to_tf_pub',
                output='screen',
                parameters=[{'use_sim_time': True}])
    ld.add_action(cam_bot_odom_to_tf_pub_cmd)

    default_launch_file_path = os.path.join(get_package_share_directory('my_tf_ros2_course_pkg'), 'launch')
    publish_static_transform_odom_to_world_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(default_launch_file_path, 'publish_static_transform_odom_to_world.launch.py')
        )
    )
    ld.add_action(publish_static_transform_odom_to_world_cmd)

    move_generic_model_cmd = Node(
        package='my_tf_ros2_course_pkg',
        executable='move_generic_model',
        name='move_generic_model',
                output='screen',
                parameters=[{'use_sim_time': True}])
    ld.add_action(move_generic_model_cmd)

    return ld

Keep it up, because the above things will not be enough.

1. The rclpy.time.Time() is equivalent to self.get_clock().now(). They are not two different time sources. The professors that write the lecture are ROS masters. But, the novice, such as me, gets confused.

2. In my experiment, the following line still fails.

            trans = self.tf_buffer.lookup_transform(
                origin_frame,
                dest_frame,
                now)

The error is about asking for a position that is not in the TF buffer. I make the following changes.

            now = self.get_clock().now() - Duration(seconds=0.1)
            trans = self._tf_database.lookup_transform(
                target_frame=origin_frame,
                source_frame=dest_frame,
                time=now
            )

Another way may ask for the latest transform in the TF buffer (time = 0).

3. In the function calculate_coord, the professors use euler_from_quaternion.
   

   
   
   However, they use quaternion_from_euler to transfer it back in function move_model.
   
   
   
   It is useless computing from the viewpoint of programming. I think the professors just want to show the existence of these functions.

4. Every member variable in the program is associated with a member function. For example, the variable trans_speed is associated with set_translation_speed. It is a good habit of programming because in the member function you can do a validation check. But for me, it is confusing. I jump into a function just for one line of the code.
   

5. Take care of the extra comment, it is not related to the program. Just ignore them.
   

   

For ROS masters, I think they just scan the codes and know what they are doing immediately. For me, a novice, I need a story about the major flow of the program. Here is my story.

The program is focused on using the service called set_entity_state. It is automatically generated by a Gazebo plugin. The major function of this service is to set the pose or the state of an object in the Gazebo. It likes the magic that can move an object in the air. In this case, we set the pose of the cam_bot to the turtle_attach_frame. So, the cam_bot seems to follow the turtle.

To this end, we need to solve two difficulties. The turtle is moving, so the pose of the frame turtle_attach_frame is changing. To update the latest frame pose, we need a timer. In the meantime, the timer can also send the request to the service set_entity_state to move the cam_bot. However, there is another difficulty. Solving the turtle_attach_frame needs a TF buffer. Since it is a buffer, it needs an update. So, you need a TF listener.

I try to clean the code. Here is my code.

'''
We should pub:
ros2 topic pub /destination_frame std_msgs/msg/String "data: 'turtle_attach_frame'"
'''

from rclpy import Future

import rclpy
from rclpy.node import Node
from rclpy.duration import Duration
from rclpy.qos import QoSProfile

from tf2_ros import TransformException
from tf2_ros.transform_listener import TransformListener
from tf2_ros.buffer import Buffer

from std_msgs.msg import String

from geometry_msgs.msg import TransformStamped
from geometry_msgs.msg import Pose, Twist

from gazebo_msgs.msg import EntityState
from gazebo_msgs.srv import SetEntityState


class CamBotMove(Node):
    def __init__(self, node_name: str):
        super().__init__(node_name=node_name)

        self._tf_database = Buffer()
        self._tf_listener = TransformListener(self._tf_database, self)

        qos_profile = QoSProfile(depth=1)
        self._command_receiver = self.create_subscription(
            String, '/destination_frame', self.process_received_destination_frame, qos_profile=qos_profile)

        self.get_logger().info(f"The node {self.get_name()} is ok.")

        self._flash_destination_timer = self.create_timer(
            timer_period_sec=0.05, callback=self.flash_destination_send_command)

        self._model_name_to_move = "cam_bot"
        self.objective_frame = "camera_bot_base_link"

        self._gazebo_set_entity_state_client = self.create_client(
            SetEntityState,srv_name="/cam_bot/set_entity_state")
        if self._gazebo_set_entity_state_client.wait_for_service(10) is False:
            self.get_logger().error(f"Gazebo service /cam_bot/set_entity_state is not ready.")
            raise RuntimeError

    def flash_destination_send_command(self):
        coordinates_to_move_to = self.get_model_pose_from_tf(
            origin_frame="world", dest_frame=self.objective_frame)
        if coordinates_to_move_to is None:
            return

        self.move_model(coordinates_to_move_to)

    def process_received_destination_frame(self, msg: String):
        self.get_logger().info(f"Receive objective frame {msg.data}")
        self.objective_frame = msg.data

    def get_model_pose_from_tf(self, origin_frame="world", dest_frame="camera_bot_base_link"):
        """
        Extract the pose from the TF
        """
        try:
            now = self.get_clock().now() - Duration(seconds=0.1)
            trans = self._tf_database.lookup_transform(
                target_frame=origin_frame,
                source_frame=dest_frame,
                time=now
            )
            return trans
        except TransformException as ex:
            self.get_logger().error(
                f"Error when transform {dest_frame} into {origin_frame}: {ex}")
            return None

    def move_model(self, coordinates_to_move_to: TransformStamped):
        destination_state = EntityState()
        destination_state.name = self._model_name_to_move
        destination_state.reference_frame = coordinates_to_move_to.header.frame_id

        destination_pose = Pose()
        destination_pose.position.x = coordinates_to_move_to.transform.translation.x
        destination_pose.position.y = coordinates_to_move_to.transform.translation.y
        destination_pose.position.z = coordinates_to_move_to.transform.translation.z
        destination_pose.orientation.x = coordinates_to_move_to.transform.rotation.x
        destination_pose.orientation.y = coordinates_to_move_to.transform.rotation.y
        destination_pose.orientation.z = coordinates_to_move_to.transform.rotation.z
        destination_pose.orientation.w = coordinates_to_move_to.transform.rotation.w
        destination_state.pose = destination_pose

        twist = Twist()
        twist.linear.x = 0.0
        twist.linear.y = 0.0
        twist.linear.z = 0.0
        twist.angular.x = 0.0
        twist.angular.y = 0.0
        twist.angular.z = 0.0
        destination_state.twist = twist

        request_msg = SetEntityState.Request()
        request_msg.state = destination_state

        future_set_entity_state = self._gazebo_set_entity_state_client.call_async(request_msg)
        future_set_entity_state.add_done_callback(self._callback_set_entity_state_response)

    def _callback_set_entity_state_response(self,future:Future):
        try:
            response = future.result()
            self.get_logger().info(f"Gazebo robot returns: {response}.")
        except Exception as e:
            self.get_logger().error(f"Call gazebo_set_entity_state encounter error: {e}")


def main():
    rclpy.init()
    node = CamBotMove("force_move_cam_bot")

    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__name__":
    main()