/
gazebo_interface.py
52 lines (44 loc) · 2.54 KB
/
gazebo_interface.py
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
#! /usr/bin/env python
# Wrappers around the services provided by rosified gazebo
import sys
import rospy
import os
import time
from gazebo_msgs.msg import *
from gazebo_msgs.srv import *
from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Wrench
def spawn_sdf_model_client(model_name, model_xml, robot_namespace, initial_pose, reference_frame, gazebo_namespace):
rospy.loginfo("Waiting for service %s/spawn_sdf_model"%gazebo_namespace)
rospy.wait_for_service(gazebo_namespace+'/spawn_sdf_model')
try:
spawn_sdf_model = rospy.ServiceProxy(gazebo_namespace+'/spawn_sdf_model', SpawnModel)
rospy.loginfo("Calling service %s/spawn_sdf_model"%gazebo_namespace)
resp = spawn_sdf_model(model_name, model_xml, robot_namespace, initial_pose, reference_frame)
rospy.loginfo("Spawn status: %s"%resp.status_message)
return resp.success
except rospy.ServiceException as e:
print("Service call failed: %s" % e)
def spawn_urdf_model_client(model_name, model_xml, robot_namespace, initial_pose, reference_frame, gazebo_namespace):
rospy.loginfo("Waiting for service %s/spawn_urdf_model"%gazebo_namespace)
rospy.wait_for_service(gazebo_namespace+'/spawn_urdf_model')
try:
spawn_urdf_model = rospy.ServiceProxy(gazebo_namespace+'/spawn_urdf_model', SpawnModel)
rospy.loginfo("Calling service %s/spawn_urdf_model"%gazebo_namespace)
resp = spawn_urdf_model(model_name, model_xml, robot_namespace, initial_pose, reference_frame)
rospy.loginfo("Spawn status: %s"%resp.status_message)
return resp.success
except rospy.ServiceException as e:
print("Service call failed: %s" % e)
def set_model_configuration_client(model_name, model_param_name, joint_names, joint_positions, gazebo_namespace):
rospy.loginfo("Waiting for service %s/set_model_configuration"%gazebo_namespace)
rospy.wait_for_service(gazebo_namespace+'/set_model_configuration')
rospy.loginfo("temporary hack to **fix** the -J joint position option (issue #93), sleeping for 1 second to avoid race condition.");
time.sleep(1)
try:
set_model_configuration = rospy.ServiceProxy(gazebo_namespace+'/set_model_configuration', SetModelConfiguration)
rospy.loginfo("Calling service %s/set_model_configuration"%gazebo_namespace)
resp = set_model_configuration(model_name, model_param_name, joint_names, joint_positions)
rospy.loginfo("Set model configuration status: %s"%resp.status_message)
return resp.success
except rospy.ServiceException as e:
print("Service call failed: %s" % e)