Gazebo ROS2 how to set Entity State when calling Set Model State via Node client

Question

new ROS2/Gazebo user here.

I am trying to implement set up a script for reinforcement learning, currently making a gazebo environment class where upon reset the robot is teleported to a random place upon reset.

Currently I am testing with an empty world where I have added the gazebo_ros_state plugin

<?xml version="1.0" ?>
<sdf version="1.5">
  
  <world name="default">
    <plugin name="gazebo_ros_state" filename="libgazebo_ros_state.so">
      <ros>
        <namespace>/plug</namespace>
        <argument>model_states:=model_states_plug</argument>
        <argument>link_states:=link_states_plug</argument>
      </ros>
      <update_rate>1.0</update_rate>
    </plugin>
    <!-- A global light source -->
    <include>
      <uri>model://sun</uri>
    </include>
    <!-- A ground plane -->
    <include>
      <uri>model://ground_plane</uri>
    </include>
  </world>
</sdf>

My robot model 'fin_roboclaw" is spawned using spawn_entity.py

And I have a script where my reset function is defined:

import rclpy
from rclpy.node import Node
from rclpy.executors import MultiThreadedExecutor
from gazebo_msgs.msg import EntityState
from gazebo_msgs.srv import SetEntityState
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from sensor_msgs.msg import LaserScan
from std_srvs.srv import Empty
from visualization_msgs.msg import Marker

...

class GazeboEnv(Node):
    def __init__(self):
        super().__init__('env')

        self.goal_x = 5.0
        self.goal_y = 5.0

        self.set_self_state = EntityState()
        self.set_self_state.name = 'fin_roboclaw'
        self.set_self_state.pose.position.x = 0.0
        self.set_self_state.pose.position.y = 0.0
        self.set_self_state.pose.position.z = 0.0
        self.set_self_state.pose.orientation.x = 0.0
        self.set_self_state.pose.orientation.y = 0.0
        self.set_self_state.pose.orientation.z = 0.0
        self.set_self_state.pose.orientation.w = 1.0

        self.set_state = self.create_client(SetEntityState, 'plug/set_entity_state')
        self.velocity_publisher = self.create_publisher(Twist, 'cmd_vel', 10)
        self.marker_publisher = self.create_publisher(Marker, 'goal_point', 3)
        self.reset_proxy = self.create_client(Empty, "/reset_world")
    
    def reset(self):
        while not self.reset_proxy.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('reset : service not available, waiting again...')

        try:
            self.reset_proxy.call_async(Empty.Request())
        except rclpy.ServiceException as e:
            print("/gazebo/reset_simulation service call failed")
        
        x = rng.uniform(1, ENVIRONMENT_X_LEN - 1)
        y = rng.uniform(1, ENVIRONMENT_Y_LEN - 1)
        while self.check_pos(x, y):
            x = rng.uniform(1, ENVIRONMENT_X_LEN - 1)
            y = rng.uniform(1, ENVIRONMENT_Y_LEN - 1)

        self.goal_x = x
        self.goal_y = y

        x = rng.uniform(1, ENVIRONMENT_X_LEN - 1)
        y = rng.uniform(1, ENVIRONMENT_Y_LEN - 1)
        while self.check_pos(x, y):
            x = rng.uniform(1, ENVIRONMENT_X_LEN - 1)
            y = rng.uniform(1, ENVIRONMENT_Y_LEN - 1)

        object_state = self.set_self_state
        angle = np.random.uniform(-np.pi, np.pi)
        quaternion = Quaternion.from_euler(0.0, 0.0, angle)

        object_state.pose.position.x = x
        object_state.pose.position.y = y
        object_state.pose.orientation.x = quaternion.x
        object_state.pose.orientation.y = quaternion.y
        object_state.pose.orientation.z = quaternion.z
        object_state.pose.orientation.w = quaternion.w
        
        
        while not self.set_state.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('set : service not available, waiting again...')

        try:
            self.set_state.call_async(SetEntityState.Request())
        except rclpy.ServiceException as e:
            print("/gazebo/reset_simulation service call failed")

        time.sleep(0.2)

Now I know I can teleport my robot outside my script by using this in terminal

ros2 service call /plug/set_entity_state 'gazebo_msgs/SetEntityState' '{state: {name: "fin_roboclaw", pose: {position: {y: 2}}}}'

However I want this handled by the script by calling set_state but I do not know how to populate the SetEntityState with the object_state values. How can I do this?

Answer 1

I managed to figure it out so I figured I'd post the solution in case anyone else encounters this. You need to assign the SetEntityState.Request() and then assign the ._state as your EntityState. I.e.

        object_state.pose.position.x = x
        object_state.pose.position.y = y
        object_state.pose.orientation.x = quaternion.x
        object_state.pose.orientation.y = quaternion.y
        object_state.pose.orientation.z = quaternion.z
        object_state.pose.orientation.w = quaternion.w

        state = SetEntityState.Request()
        state._state = object_state
        
        
        while not self.reset_proxy.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('reset : service not available, waiting again...')

        try:
            self.set_state.call_async(state)
        except rclpy.ServiceException as e:
            print("plug/set_entity_state service call failed")
```