[ROS2] Unable to get object pose with /get_entity_state service

[nicholaspalomo]

Description

ROS2 hangs upon calling /get_entity_state message to query pose information about model spawned in Gazebo.

To Reproduce

Steps to reproduce the behavior:

1. source /opt/ros/foxy/setup.bash
2. ros2 launch gazebo_ros gazebo.launch.py
3. (If your Gazebo window doesn't open automatically, open it with gzclient)
4. Run the following script:

import sys

import rclpy
from rclpy.node import Node
from rclpy.client import Client

from gazebo_msgs.srv import SpawnEntity, GetEntityState
from geometry_msgs.msg import Pose

MODEL_DATABASE_TEMPLATE = """\
<sdf version="1.4">
    <world name="default">
        <include>
            <uri>model://{}</uri>
        </include>
    </world>
</sdf>"""

def spin_until_service_complete(node, response):
    rclpy.spin_until_future_complete(node, response)
    if response.result() is not None:
        node.get_logger().info('SERVICE COMPLETE! RESULT:\n{}'.format(response.result()))
        return response.result()
def main():
    rclpy.init(args=sys.argv)
    node: Node = rclpy.create_node('sandbox')

    spawn_client: Client = node.create_client(SpawnEntity, '/spawn_entity')

    spawn_request: SpawnEntity.Request = SpawnEntity.Request()
    spawn_request.name = "cube"
    spawn_request.xml = MODEL_DATABASE_TEMPLATE.format('wood_cube_5cm')
    spawn_request.reference_frame = "world"
    spawn_request.initial_pose = Pose()
    spawn_request.initial_pose.position.x = 0.4 # [m]
    spawn_request.initial_pose.position.y = 0.0 # [m]
    spawn_request.initial_pose.position.z = 0.025 # [m]

    node.get_logger().info('TRYING TO SPAWN CUBE INTO GAZEBO!')
    response = spawn_client.call_async(spawn_request)
    spin_until_service_complete(node, response)

    node.get_logger().info('CUBE SUCCESSFULLY SPAWNED!')

    get_entity_state_client: Client = node.create_client(GetEntityState, '/get_entity_state')
    get_entity_state_request: GetEntityState.Request = GetEntityState.Request()
    get_entity_state_request.name = "cube"
    get_entity_state_request.reference_frame = "world"

    node.get_logger().info('TRYING TO GET CUBE POSE!')
    response: GetEntityState.Response = get_entity_state_client.call_async(get_entity_state_request)

    result = spin_until_service_complete(node, response)
    node.get_logger().info('GOT CUBE POSE!\n{}'.format(result))

    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

Expected behavior

/get_entity_state should return a GetEntityState.Response containing the pose information about the cube (or whatever object) you spawn in Gazebo.

Environment:

• OS: Ubuntu 20.04
• Gazebo version: Gazebo 11.6.0
• ROS version: Foxy
• gazebo_ros_pkgs: debian install
• glx server in use: NVIDIA

[solid-sinusoid]

@nicholaspalomo Hello, did you solve this problem? Now I myself need to implement this service. And I would like to know your feedback. At the moment, I need to contact this service, but I don’t even see it when gazebo.launch.py is running when executing the ros2 service list command

[tianyilim]

Hi, I was trying to get this working as well, but also could not see the /get_entity_state service when running ros2 service list while using Gazebo 11 / ROS Galactic.

A workaround I used was to embed the gazebo_ros_p3d plugin in the model's SDF/URDF file. By looking at the source code in this repo (here) and ctrl-F-ing for the required XML tags, here is an example of the arguments that can be used.

<plugin name="p3d_base_footprint" filename="libgazebo_ros_p3d.so">
  <ros>
    <namespace>$(arg prefix)</namespace>
    <remapping>odom:=$(arg prefix)_odom</remapping>
  </ros>
  <always_on>true</always_on>
  <update_rate>100</update_rate>
  <body_name>$(arg prefix)_base_footprint</body_name>
  <frame_name>world</frame_name>

  <gaussian_noise>0.00</gaussian_noise>
  <xyz_offset>0 0 0</xyz_offset>
  <rpy_offset>0 0 0</rpy_offset>
</plugin>

I was working on a multi-robot example and thus needed to start each plugin in its own namespace. The message topics can also be remapped. The plugin publishes nav_msgs/Odometry messages on the selected topic.

However, this does not publish any TF messages, so I guess you have to write a TF publisher node that subscribes to the topic that was specified earlier.

My actual application was to get the ground truth positions of diff drive robots in my Gazebo environment, so a more direct solution was using an option in the diff drive controller source. Setting the odometry_source flag lets you change from a simulated encoder and the ground truth value.

If you set <odometry_source>1</odometry_source>, it selects a ground truth value. Setting odometry_source to 0 (by default) simulates an encoder. The great thing is that the diff drive plugin also publishes TF transforms for both the robot base as well as its wheels, which is more direct.

An example of what I did:

<!-- ++++++++++++++++++++++++++++++ Diff Drive Plugin ++++++++++++++++++++++++++++ -->
<plugin name="$(arg prefix)_diff_drive" filename="libgazebo_ros_diff_drive.so">
  <ros>
    <remapping>/tf:=tf</remapping>
    <namespace>$(arg prefix)</namespace>
  </ros>

  <always_on>true</always_on>
  <update_rate>100</update_rate>
  <left_joint>$(arg prefix)_left_wheel_joint</left_joint>
  <right_joint>$(arg prefix)_right_wheel_joint</right_joint>
  <wheel_separation>${chassis_y+wheelwidth+2*wheelgap}</wheel_separation>
  <wheel_diameter>${wheeldiam}</wheel_diameter>
  <max_wheel_torque>20</max_wheel_torque>
  <max_wheel_acceleration>1.0</max_wheel_acceleration>

  <odometry_frame>$(arg prefix)odom</odometry_frame>
  <robot_base_frame>$(arg prefix)_base_footprint</robot_base_frame>

  <!-- 0 is encoder, 1 is world (ground truth) -->
  <odometry_source>1</odometry_source>
  
  <publish_odom>true</publish_odom>
  <publish_odom_tf>true</publish_odom_tf>
  <publish_wheel_tf>true</publish_wheel_tf>

</plugin>

<!-- +++++++++++++++++++++++++++++ Joint State Plugin ++++++++++++++++++++++++++++ -->
<plugin name="$(arg prefix)_joint_states" filename="libgazebo_ros_joint_state_publisher.so">
  <ros>
    <namespace>$(arg prefix)</namespace>
  </ros>
  <joint_name>$(arg prefix)_right_wheel_joint</joint_name>
  <joint_name>$(arg prefix)_left_wheel_joint</joint_name>
  <update_rate>100</update_rate>
</plugin>

I hope this helps!

[swuuu]

Hi, I was also trying to read the robot pose from gazebo and could not find the /get_entity_state service. I found a workaround, which is to add the lib_gazebo_ros_state.so plugin in the world file being launched. (In my case, it was /usr/share/gazebo-11/worlds/empty.world). This link helped me: https://answers.ros.org/question/360161/ros2-dashing-service-get_entity_state-is-missing/.