00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 PKG = 'gazebo_plugins'
00038 NAME = 'set_pose'
00039
00040 import math
00041 import roslib
00042 roslib.load_manifest(PKG)
00043
00044 import sys, unittest
00045 import os, os.path, threading, time
00046 import rospy, rostest
00047
00048 from gazebo_plugins.srv import SetPose
00049
00050 from std_msgs.msg import String
00051 from geometry_msgs.msg import Pose,Quaternion,Point, PoseStamped, PoseWithCovariance, TwistWithCovariance, Twist, Vector3
00052 from nav_msgs.msg import Odometry
00053 import tf.transformations as tft
00054 from numpy import float64
00055
00056 COV = [float64(0),float64(0),float64(0),float64(0),float64(0),float64(0), \
00057 float64(0),float64(0),float64(0),float64(0),float64(0),float64(0), \
00058 float64(0),float64(0),float64(0),float64(0),float64(0),float64(0), \
00059 float64(0),float64(0),float64(0),float64(0),float64(0),float64(0), \
00060 float64(0),float64(0),float64(0),float64(0),float64(0),float64(0), \
00061 float64(0),float64(0),float64(0),float64(0),float64(0),float64(0) ]
00062
00063
00064 def normalize_angle_positive(angle):
00065 return math.fmod(math.fmod(angle, 2*math.pi) + 2*math.pi, 2*math.pi)
00066
00067 def normalize_angle(angle):
00068 anorm = normalize_angle_positive(angle)
00069 if anorm > math.pi:
00070 anorm -= 2*math.pi
00071 return anorm
00072
00073 def shortest_angular_distance(angle_from, angle_to):
00074 angle_diff = normalize_angle_positive(angle_to) - normalize_angle_positive(angle_from)
00075 if angle_diff > math.pi:
00076 angle_diff = -(2*math.pi - angle_diff)
00077 return normalize_angle(angle_diff)
00078
00079 class SimIfaceControl():
00080 def __init__(self):
00081 self.update_rate=10
00082 self.timeout=1
00083 self.target_p = [0,0,0]
00084 self.target_q = [0,0,0]
00085 self.target_e = [0,0,0]
00086 self.wait_topic_initialized = False
00087 self.frame_id = "world"
00088 self.service_name = "set_pose_service"
00089 self.topic_name = "set_pose_topic"
00090 self.use_topic = False;
00091 self.use_service = False;
00092 self.wait_topic_name = "clock"
00093 self.wait_for_topic = False;
00094
00095 rospy.init_node(NAME, anonymous=True)
00096
00097 def setPoseService(self,pose_msg):
00098 print "waiting for service to set pose"
00099 rospy.wait_for_service(self.service_name);
00100 try:
00101 set_pose = rospy.ServiceProxy(self.service_name, SetPose)
00102 resp1 = set_pose(pose_msg)
00103 return resp1.success
00104 except rospy.ServiceException, e:
00105 print "service call failed: %s"%e
00106
00107 def waitTopicInput(self,p3d):
00108
00109
00110
00111 self.wait_topic_initialized = True
00112
00113 def setPose(self):
00114
00115 for i in range(0,len(sys.argv)):
00116 if sys.argv[i] == '-update_rate':
00117 if len(sys.argv) > i+1:
00118 self.update_rate = float(sys.argv[i+1])
00119 if sys.argv[i] == '-timeout':
00120 if len(sys.argv) > i+1:
00121 self.timeout = float(sys.argv[i+1])
00122 if sys.argv[i] == '-x':
00123 if len(sys.argv) > i+1:
00124 self.target_p[0] = float(sys.argv[i+1])
00125 if sys.argv[i] == '-y':
00126 if len(sys.argv) > i+1:
00127 self.target_p[1] = float(sys.argv[i+1])
00128 if sys.argv[i] == '-z':
00129 if len(sys.argv) > i+1:
00130 self.target_p[2] = float(sys.argv[i+1])
00131 if sys.argv[i] == '-R':
00132 if len(sys.argv) > i+1:
00133 self.target_e[0] = float(sys.argv[i+1])
00134 if sys.argv[i] == '-P':
00135 if len(sys.argv) > i+1:
00136 self.target_e[1] = float(sys.argv[i+1])
00137 if sys.argv[i] == '-Y':
00138 if len(sys.argv) > i+1:
00139 self.target_e[2] = float(sys.argv[i+1])
00140 if sys.argv[i] == '-f':
00141 if len(sys.argv) > i+1:
00142 self.frame_id = sys.argv[i+1]
00143 if sys.argv[i] == '-s':
00144 if len(sys.argv) > i+1:
00145 self.service_name = sys.argv[i+1]
00146 self.use_service = True;
00147 if sys.argv[i] == '-t':
00148 if len(sys.argv) > i+1:
00149 self.topic_name = sys.argv[i+1]
00150 self.use_topic = True;
00151 if sys.argv[i] == '-p':
00152 if len(sys.argv) > i+1:
00153 self.wait_topic_name = sys.argv[i+1]
00154 self.wait_for_topic = True;
00155
00156
00157 self.pub_set_pose_topic = rospy.Publisher(self.topic_name, Odometry)
00158 rospy.Subscriber(self.wait_topic_name, rospy.AnyMsg, self.waitTopicInput)
00159
00160
00161 if self.wait_for_topic:
00162 while not self.wait_topic_initialized:
00163 time.sleep(0.1)
00164
00165
00166 h = rospy.Header()
00167 h.stamp = rospy.get_rostime()
00168 h.frame_id = self.frame_id
00169 p = Point(self.target_p[0],self.target_p[1],self.target_p[2])
00170 tmpq = tft.quaternion_from_euler(self.target_e[0],self.target_e[1],self.target_e[2])
00171 q = Quaternion(tmpq[0],tmpq[1],tmpq[2],tmpq[3])
00172 pose = Pose(p,q)
00173 pwc = PoseWithCovariance(pose,COV)
00174 twc = TwistWithCovariance(Twist(Vector3(),Vector3()),COV)
00175 child_frame_id = ""
00176 target_pose = Odometry(h,child_frame_id,pwc,twc)
00177
00178 if self.use_service:
00179 success = self.setPoseService(target_pose)
00180
00181
00182 if self.use_topic:
00183 timeout_t = time.time() + self.timeout
00184 while not rospy.is_shutdown() and time.time() < timeout_t:
00185
00186 self.pub_set_pose_topic.publish(target_pose)
00187
00188 if self.update_rate > 0:
00189 time.sleep(1.0/self.update_rate)
00190 else:
00191 time.sleep(0.001)
00192
00193 def print_usage(exit_code = 0):
00194 print '''Commands:
00195 -update_rate <Hz> - update rate, default to 10 Hz
00196 -timeout <seconds> - test timeout in seconds. default to 1 seconds
00197 -x <x in meters>
00198 -y <y in meters>
00199 -z <z in meters>
00200 -R <roll in radians>
00201 -P <pitch in radians>
00202 -Y <yaw in radians>
00203 -f target frame_id
00204 -s set pose service name
00205 -t set pose topic name
00206 -p wait for this ros topic to be published first
00207 '''
00208
00209 if __name__ == '__main__':
00210
00211 if len(sys.argv) == 1:
00212 print_usage()
00213 else:
00214 sic = SimIfaceControl()
00215 sic.setPose()
00216
00217
00218
