0
$\begingroup$

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?

$\endgroup$

1 Answer 1

0
$\begingroup$

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")
```
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.