Sometimes we need to add new objects to Gazebo to build a dynamic environment, particularly in some manipulation tasks. Suppose we test if the robot can pick up items from the floor. Thus, we may need a program to spawn models in a random position in the environment to verify our manipulation algorithms’ robustness. Like the top picture, a new cylinder is created in TIAGO’s gripper. Then TIAGO can deliver the cylinder to a free container.
Spawning Models
To spawn a new model in the Gazebo, we focus on the function spawn_sdf_model_client of gazebo_interface. If you have time, look at its document. If not, I have already prepared such a function for you.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 |
from gazebo_ros import gazebo_interface def spawn(model_name, positions, orientations, static=False): # type: (str, Iterable[float], Iterable[float],bool) -> str model_database_template = """<sdf version="1.4"> <world name="default"> <include> <uri>model://%MODEL_NAME%</uri> <static>%STATIC%</static> </include> </world> </sdf>""" model_xml = model_database_template \ .replace('%MODEL_NAME%', model_name) \ .replace('%STATIC%', str(int(static))) initial_pose = Pose(Point(*positions), Quaternion(*quaternion_from_euler(*orientations))) gazebo_model_name = "%s_%d" % (model_name, round(time.time() * 1000)) gazebo_interface.spawn_sdf_model_client(gazebo_model_name, model_xml, rospy.get_namespace(), initial_pose, "", "/gazebo") rospy.loginfo("%s spawned in Gazebo as %s", model_name, gazebo_model_name) return gazebo_model_name |
To explain the above function, Let’s say we have a model named red_ball.
Your model directory should have the same name red_ball. And also the same name is in the model.config file.
|
1 2 3 4 5 6 7 8 9 10 11 |
<?xml version="1.0" ?> <model> <name>red_ball</name> <version>1.0</version> <sdf version="1.6">model.sdf</sdf> <author> <name></name> <email></email> </author> <description></description> </model> |
Make sure you model directory is in the Gazebo environment variable (The model name should be visible from the sidebar of Gazebo).
Thus, our first argument passed into the above function should be red_ball. The second argument is a set of float values representing the object’s desired position x, y, and z. The third argument is also a set of float values, representing the desired orientations of the object in Euler angles, which will be transformed to quaternion in line 16 and build the Pose. If the last argument is True, the model will hold the static property (line 9), which means the object is immovable. We add a suffix to the original model name to make it unique in Gazebo (line 17). Last we call the function spawn_sdf_model_client (line 18) with our arguments then object will be visible in Gazebo.
Deleting Models
To delete models from Gazebo, we need to know the exact name of the model, which is a required parameter of /gazebo/delete_model service.
We can also subscribe the topic /gazebo/model_states to have information of all the models, and then search for models with a specific keyword and delete it.
The delete function looks like this.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 |
from gazebo_msgs.msg import ModelStates from gazebo_msgs.srv import DeleteModel import rospy rospy.wait_for_service('/gazebo/delete_model') delete_model = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel) def delete(keyword): states = rospy.wait_for_message('/gazebo/model_states', ModelStates) assert isinstance(states, ModelStates) target_names = [] for model_name in states.name: if keyword in model_name: target_names.append(model_name) if len(target_names) > 0: rospy.loginfo("%d objects need to be deleted", len(target_names)) try: for target_name in target_names: rospy.loginfo("Delete model: %s" % target_name) delete_model(target_name) except rospy.ServiceException as e: rospy.logerr("Delete model failed: %s" % e.message) |
We create the proxy of deleting service in line 6. This delete function accepts one parameter representing the keyword used to search the model. We search objects using that keyword in the for loop of line 12. Finally, we call the deleting service by passing the full model name in line 21.
Overall
In the end, we can make a Class to handle these two functions.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 |
import os from gazebo_ros import gazebo_interface from tf.transformations import quaternion_from_euler from geometry_msgs.msg import Pose, Quaternion, Point import rospy import time from typing import Iterable from gazebo_msgs.msg import ModelStates from gazebo_msgs.srv import DeleteModel class ObjectSpawner(object): def __init__(self): rospy.wait_for_service('/gazebo/delete_model') self._delete_model = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel) def delete(self, keyword): states = rospy.wait_for_message('/gazebo/model_states', ModelStates) assert isinstance(states, ModelStates) target_names = [] for model_name in states.name: if keyword in model_name: target_names.append(model_name) if len(target_names) > 0: rospy.loginfo("%d objects need to be deleted", len(target_names)) try: for target_name in target_names: rospy.loginfo("Delete model: %s" % target_name) self._delete_model(target_name) except rospy.ServiceException as e: rospy.logerr("Delete model failed: %s" % e.message) def spawn(self, model_name, positions, orientations, static=False): # type: (str, Iterable[float], Iterable[float],bool) -> str model_database_template = """<sdf version="1.4"> <world name="default"> <include> <uri>model://%MODEL_NAME%</uri> <static>%STATIC%</static> </include> </world> </sdf>""" model_xml = model_database_template \ .replace('%MODEL_NAME%', model_name) \ .replace('%STATIC%', str(int(static))) initial_pose = Pose(Point(*positions), Quaternion(*quaternion_from_euler(*orientations))) gazebo_model_name = "%s_%d" % (model_name, round(time.time() * 1000)) gazebo_interface.spawn_sdf_model_client(gazebo_model_name, model_xml, rospy.get_namespace(), initial_pose, "", "/gazebo") rospy.loginfo("%s spawned in Gazebo as %s", model_name, gazebo_model_name) return gazebo_model_name |