gazebo_ros: gazebo_interface.py Source File

Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 # Wrappers around the services provided by rosified gazebo
00003 
00004 import sys
00005 import rospy
00006 import os
00007 import time
00008 
00009 from gazebo_msgs.msg import *
00010 from gazebo_msgs.srv import *
00011 from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Wrench
00012 
00013 
00014 def spawn_sdf_model_client(model_name, model_xml, robot_namespace, initial_pose, reference_frame, gazebo_namespace):
00015     rospy.loginfo("Waiting for service %s/spawn_sdf_model"%gazebo_namespace)
00016     rospy.wait_for_service(gazebo_namespace+'/spawn_sdf_model')
00017     try:
00018       spawn_sdf_model = rospy.ServiceProxy(gazebo_namespace+'/spawn_sdf_model', SpawnModel)
00019       rospy.loginfo("Calling service %s/spawn_sdf_model"%gazebo_namespace)
00020       resp = spawn_sdf_model(model_name, model_xml, robot_namespace, initial_pose, reference_frame)
00021       rospy.loginfo("Spawn status: %s"%resp.status_message)
00022       return resp.success
00023     except rospy.ServiceException, e:
00024       print "Service call failed: %s"%e
00025 
00026 def spawn_urdf_model_client(model_name, model_xml, robot_namespace, initial_pose, reference_frame, gazebo_namespace):
00027     rospy.loginfo("Waiting for service %s/spawn_urdf_model"%gazebo_namespace)
00028     rospy.wait_for_service(gazebo_namespace+'/spawn_urdf_model')
00029     try:
00030       spawn_urdf_model = rospy.ServiceProxy(gazebo_namespace+'/spawn_urdf_model', SpawnModel)
00031       rospy.loginfo("Calling service %s/spawn_urdf_model"%gazebo_namespace)
00032       resp = spawn_urdf_model(model_name, model_xml, robot_namespace, initial_pose, reference_frame)
00033       rospy.loginfo("Spawn status: %s"%resp.status_message)
00034       return resp.success
00035     except rospy.ServiceException, e:
00036       print "Service call failed: %s"%e
00037 
00038 def set_model_configuration_client(model_name, model_param_name, joint_names, joint_positions, gazebo_namespace):
00039     rospy.loginfo("Waiting for service %s/set_model_configuration"%gazebo_namespace)
00040     rospy.wait_for_service(gazebo_namespace+'/set_model_configuration')
00041     rospy.loginfo("temporary hack to **fix** the -J joint position option (issue #93), sleeping for 1 second to avoid race condition.");
00042     time.sleep(1)
00043     try:
00044       set_model_configuration = rospy.ServiceProxy(gazebo_namespace+'/set_model_configuration', SetModelConfiguration)
00045       rospy.loginfo("Calling service %s/set_model_configuration"%gazebo_namespace)
00046       resp = set_model_configuration(model_name, model_param_name, joint_names, joint_positions)
00047       rospy.loginfo("Set model configuration status: %s"%resp.status_message)
00048 
00049       return resp.success
00050     except rospy.ServiceException, e:
00051       print "Service call failed: %s"%e
00052