def shutdown_cb(self):
rospy.loginfo("%s: Calling gazebo delete_models", self.node_name)
try:
delete_cube = DeleteModelRequest('aruco_cube')
delete_model_res = self.delete_model_srv(delete_cube)
except rospy.ServiceException, e:
print "Service call to gazebo delete_models failed: %s"%e
def cleanup_cb(self):
rospy.loginfo("%s: Calling gazebo delete_models", self.node_name)
try:
delete_robot = DeleteModelRequest(self.auv_name)
delete_model_res = self.delete_model_srv(delete_robot)
except rospy.ServiceException, e:
print "Service call to gazebo delete_models failed: %s"%e
def spawn_box():
# pauseSim()
rospy.wait_for_service("/gazebo/spawn_urdf_model")
file_localition = roslib.packages.get_pkg_dir(
'aliengo_gazebo') + '/world/my_box.urdf'
p = os.popen("rosrun xacro xacro " + file_localition)
xml_string = p.read()
p.close()
srv_spawn_model = rospy.ServiceProxy('/gazebo/spawn_urdf_model',
SpawnModel)
srv_delete_model = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel)
req = DeleteModelRequest()
name = "box"
req.model_name = name
try:
res = srv_delete_model(name)
except rospy.ServiceException, e:
rospy.logdebug("Model %s does not exist in gazebo.", name)
def delete_model(mav_sys_id, vehicle_type, ros_master_uri=None):
if ros_master_uri:
original_uri = os.environ[ROS_MASTER_URI]
os.environ[ROS_MASTER_URI] = ros_master_uri
try:
srv = ServiceProxy("/gazebo/delete_model", DeleteModel)
req = DeleteModelRequest()
unique_name = "uav" + str(mav_sys_id)
req.model_name = unique_name
resp = srv(req)
except ServiceException, e:
print_error(
"==================================================================================="
)
print_error(" Service call failed: %s" % e)
print_error(
"==================================================================================="
)
sys.exit(2)
def delete_model(self, model_name):
# check if object is already spawned
srv_delete_model = rospy.ServiceProxy('gazebo/delete_model',
DeleteModel)
req = DeleteModelRequest()
req.model_name = model_name
try:
res = srv_delete_model(req)
result_ok = res.success
result_msg = res.status_message
rospy.loginfo(result_msg)
except rospy.ServiceException as e:
result_ok = False
result_msg = "Model " + model_name + " does not exist in gazebo, e=" + str(
e)
rospy.logerr(result_msg)
return result_ok, result_msg
def delAllModelSrvCb(req):
worldProp = gazeboModelCli.get_worldProperties_proxy(
GetWorldPropertiesRequest())
if len(worldProp.model_names) > 2:
modelsToDel = worldProp.model_names[2:]
for modelName in modelsToDel:
delModelReq = DeleteModelRequest()
delModelReq.model_name = modelName
gazeboModelCli.delete_model_proxy(delModelReq)
gazeboModelCli.added_models = []
gazeboModelCli.modelsPoses = []
gazeboModelCli.noisyPoses = []
msg = "All model deleted!"
else:
msg = "No model to be deleted!"
fakeRecPub.setMsg(gazeboModelCli.added_models, gazeboModelCli.modelsPoses)
fakeRecPub.pubOnce(add_noise=True)
resp = TriggerResponse()
resp.success = True
rospy.loginfo(msg)
resp.message = msg
gazeboModelCli.reset()
return resp
def delete_model(mav_sys_id, vehicle_type, ros_master_uri=None):
if ros_master_uri:
original_uri = os.environ[ROS_MASTER_URI]
os.environ[ROS_MASTER_URI] = ros_master_uri
srv = ServiceProxy('/gazebo/delete_model', DeleteModel)
req = DeleteModelRequest()
unique_name = vehicle_type + '_' + str(mav_sys_id)
req.model_name = unique_name
resp = srv(req)
if ros_master_uri:
os.environ[ROS_MASTER_URI] = original_uri
if resp.success:
print(resp.status_message, '(%s)' % unique_name)
return 0
else:
print("failed to delete model [%s]: %s" %
(unique_name, resp.status_message),
file=sys.stderr)
return 1
class ModelsHandler(object):
def __init__(self):
self.node_name = "gazebo_models_handler"
rospy.loginfo("Gazebo_models_handler launched")
self.aruco_cube = rospy.get_param(rospy.get_name() + '/aruco_cube_sdf',
'')
self.dlt_model_srv = "/gazebo/delete_model"
self.spawn_model_srv = "/gazebo/spawn_sdf_model"
# Wait for service providers
rospy.wait_for_service(self.dlt_model_srv, timeout=30)
rospy.wait_for_service(self.spawn_model_srv, timeout=30)
self.delete_model_srv = rospy.ServiceProxy(self.dlt_model_srv,
DeleteModel)
rospy.loginfo("%s: Services received!", self.node_name)
# Initial pose of the cube
initial_pose = Pose()
initial_pose.position.x = -1.130530
initial_pose.position.y = -6.653650
initial_pose.position.z = 0.862500
f = open(self.aruco_cube, 'r')
sdffile = f.read()
spawn_model_prox = rospy.ServiceProxy(self.spawn_model_srv, SpawnModel)
spawn_model_prox("aruco_cube", sdffile, "/", initial_pose, "world")
rospy.on_shutdown(self.shutdown_cb)
rospy.spin()
def shutdown_cb(self):
rospy.loginfo("%s: Calling gazebo delete_models", self.node_name)
try:
delete_cube = DeleteModelRequest('aruco_cube')
delete_model_res = self.delete_model_srv(delete_cube)
except rospy.ServiceException, e:
print "Service call to gazebo delete_models failed: %s" % e
try:
delete_robot = DeleteModelRequest('tiago_steel')
delete_model_res = self.delete_model_srv(delete_robot)
except rospy.ServiceException, e:
print "Service call to gazebo delete_models failed: %s" % e
def reset(self):
"""
Resets the state of the thimblerigger.
Specifically, it:
- Hides the ball
- Chooses a new mug for the ball to be under
:returns True.
"""
clientLogger.info("(Re)setting thimblerigger experiment.")
self._despawn_ball()
self._hide_ball()
self.send_training_signal = False
for mug_name in self.mug_order:
msg = DeleteModelRequest()
msg.model_name = mug_name
self._despawn_proxy(msg)
self._spawn_mugs()
self.choose_mug_for_ball()
return True
def setup_experiment_table():
global gazebo_links_to_pubish_to_tf
model_name = 'experiment_table'
delete_model_service.call(DeleteModelRequest(model_name=model_name))
table_urdf_xacro = os.path.join(CONSTANT.gazebo_model_dir,
'table.urdf.xacro')
table_urdf = subprocess.check_output("rosrun xacro xacro %s" %
table_urdf_xacro,
shell=True)
rospy.set_param("table_urdf", table_urdf)
subprocess.call(
"rosrun gazebo_ros spawn_model -urdf -param table_urdf -model %s -reference_frame baxter::base -x 0 -y 0 -z -0.7"
% (model_name, ),
shell=True)
gazebo_links_to_pubish_to_tf.append('experiment_table::table_top_link')
gazebo_links_tf_name.append('table_top_link')
def setup_object():
global gazebo_links_to_pubish_to_tf
model_name = 'experiment_box'
delete_model_service.call(DeleteModelRequest(model_name=model_name))
box_urdf_xacro = os.path.join(CONSTANT.gazebo_model_dir, 'box.urdf.xacro')
box_urdf = subprocess.check_output("rosrun xacro xacro %s" %
box_urdf_xacro,
shell=True)
rospy.set_param("box_urdf", box_urdf)
subprocess.call(
"rosrun gazebo_ros spawn_model -urdf -param box_urdf -model %s -reference_frame baxter::base -x 0.85 -y 0 -z 1"
% (model_name, ),
shell=True)
gazebo_links_to_pubish_to_tf.append('experiment_box::experiment_box_link')
gazebo_links_tf_name.append('experiment_box_link')
def setup_camera():
model_name = 'experiment_camera'
camera_sdf = os.path.join(CONSTANT.gazebo_model_dir, 'camera.sdf')
delete_model_service.call(DeleteModelRequest(model_name=model_name))
x, y, z = [1.5, -0.5, 0.2]
roll, pitch, yaw = [0, math.pi / 8, math.pi * 6.5 / 8]
subprocess.call(
"rosrun gazebo_ros spawn_model -sdf -file %s -model %s -reference_frame baxter::base -x %s -y %s -z %s -R %s -P %s -Y %s"
% (
camera_sdf,
model_name,
x,
y,
z,
roll,
pitch,
yaw,
),
shell=True)
rviz_frame_correction_rpy = [-math.pi / 2, 0, -math.pi / 2]
tf_roll, tf_pitch, tf_yaw = euler_from_matrix(
np.dot(euler_matrix(roll, pitch, yaw),
euler_matrix(*rviz_frame_correction_rpy)))
tf_pub_process = subprocess.Popen(
'rosrun tf static_transform_publisher %s %s %s %s %s %s /base /gps_rgb_camera_frame 1000'
% (
x,
y,
z,
tf_yaw,
tf_pitch,
tf_roll,
),
shell=True)
return tf_pub_process
def initial_setup(self):
'''
Remove all the models in the sim and spawn a ground plane to run tests with.
TODO: Sometimes all the models don't get deleted correctly - probably a gazeb problem.
'''
models = yield self.model_states.get_next_message()
if 'ground_plane' not in models.name:
s = SpawnModelRequest()
s.model_name = 'ground_plane'
s.model_xml = ground_plane
s.initial_pose = msg_helpers.numpy_quat_pair_to_pose([0, 0, -5], [0, 0, 0, 1])
yield self.spawn_model(s)
for model in models.name:
if model == "channel_marker_1" or model == "sub8" or model == 'ground_plane':
continue
print "MARKER_TEST - Deleting {}".format(model)
self.delete_model(DeleteModelRequest(model_name=model))
self.nh.sleep(1)
#! /usr/bin/env python
import rospy
from gazebo_msgs.srv import DeleteModel, DeleteModelRequest
import sys
rospy.init_node('remove_model_service_client')
print "waiting for service..."
rospy.wait_for_service('/gazebo/delete_model')
delete_model_service = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel)
kk = DeleteModelRequest()
kk.model_name = 'mira'
print "Deleting model = " + str(kk)
status_msg = delete_model_service(kk)
print status_msg
#! /usr/bin/env python
import rospy
from gazebo_msgs.srv import DeleteModel, DeleteModelRequest # Import the service message used by the service /gazebo/delete_model
import sys
rospy.init_node(
'service_client') # Initialise a ROS node with the name service_client
rospy.wait_for_service(
'/gazebo/delete_model'
) # Wait for the service client /gazebo/delete_model to be running
delete_model_service = rospy.ServiceProxy(
'/gazebo/delete_model',
DeleteModel) # Create the connection to the service
kk = DeleteModelRequest() # Create an object of type DeleteModelRequest
kk.model_name = "bowl_2" # Fill the variable model_name of this object with the desired value
result = delete_model_service(
kk
) # Send through the connection the name of the object to be deleted by the service
print result # Print the result given by the service called
objects = {}
for object_name in object_names:
if object_name in list(flat_objects.keys()):
objects.update({object_name: flat_objects[object_name]})
elif sys.argv[1] not in list(flat_objects.keys()):
rospy.logerr("Object %s not found", sys.argv[1])
sys.exit()
else:
objects = {sys.argv[1]: flat_objects[sys.argv[1]]}
rospy.loginfo("Trying to remove %s", list(objects.keys()))
for name in objects:
# check if object is already spawned
srv_delete_model = rospy.ServiceProxy('gazebo/delete_model',
DeleteModel)
req = DeleteModelRequest()
req.model_name = name
exists = True
try:
rospy.wait_for_service('/gazebo/delete_model')
res = srv_delete_model(name)
except rospy.ServiceException:
exists = False
rospy.logdebug("Model %s does not exist in gazebo.", name)
if exists:
rospy.loginfo("Model %s removed.", name)
else:
rospy.logerr("Model %s not found in gazebo.", name)
def remove_model(self, model_name="coke_can"):
del_model_req = DeleteModelRequest(model_name)
self.delete_model_service_proxy(del_model_req)
#! /usr/bin/env python
import rospy
# Import the service message used by the service /gazebo/delete_model
from gazebo_msgs.srv import DeleteModel, DeleteModelRequest
import sys
# Initialise a ROS node with the name service_client
rospy.init_node('service_client')
# Wait for the service client /gazebo/delete_model to be running
rospy.wait_for_service('/gazebo/delete_model')
# Create the connection to the service
delete_model_service = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel)
# Create an object of type DeleteModelRequest
delete_model_object = DeleteModelRequest()
# Fill the variable model_name of this object with the desired value
delete_model_object.model_name = "bowl_1"
# Send through the connection the name of the object to be deleted by the service
result = delete_model_service(delete_model_object)
# Print the result given by the service called
print result
def call_delete_ramp(self):
rospy.loginfo("Deleting ramp")
del_req = DeleteModelRequest()
del_req.model_name = "ramp"
resp = self.delete_srv.call(del_req)
rospy.loginfo("Resp of delete: " + str(resp))
def call_delete(self):
rospy.loginfo("Deleting " + str(self.curr_cfg["MODELNAME_"]))
del_req = DeleteModelRequest()
del_req.model_name = str(self.curr_cfg["MODELNAME_"])
resp = self.delete_srv.call(del_req)
rospy.loginfo("Resp of delete: " + str(resp))
#!/usr/bin/python3
import rospy
from moveit_commander.move_group import MoveGroupCommander
from geometry_msgs.msg import Pose
from math import radians, cos, sin
from gazebo_msgs.srv import SpawnModel, DeleteModel, DeleteModelRequest
colors = ['Blue', 'Red', 'Green', 'Purple']
srv = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel)
req = DeleteModelRequest()
for x in colors:
for y in range(3):
req.model_name = "mp" + x + str(y)
resp = srv(req)
if resp.success:
print(resp.status_message, '(%s)' % x)
else:
print("failed to delete model [%s]" % x)
#! /usr/bin/env python
from gazebo_msgs.srv import DeleteModel, DeleteModelRequest
# Import the service message
import sys
import rospy
rospy.init_node('service_client')
# Initialise a ROS node with the name service_client
#Overview: call service --> create client variable --> create object variable -->...
#... assign object name --> call client variable with object variable as argument
rospy.wait_for_service('/gazebo/delete_model')
# Wait for the service client /gazebo/delete_model
delete_model_service = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel)
# Create the service client
kk = DeleteModelRequest()
# Create an object of type DeleteModelRequest
kk.model_name = "right_wall"
# Fill the variable model_name of this object with the desired value
result = delete_model_service(kk)
# Send through the connection the name of the object to the service node
print result