37 PKG =
'gazebo_plugins' 42 roslib.load_manifest(PKG)
45 import os, os.path, threading, time
48 from gazebo_plugins.srv
import SetPose
50 from std_msgs.msg
import String
51 from geometry_msgs.msg
import Pose,Quaternion,Point, PoseStamped, PoseWithCovariance, TwistWithCovariance, Twist, Vector3
53 import tf.transformations
as tft
54 from numpy
import float64
56 COV = [float64(0),float64(0),float64(0),float64(0),float64(0),float64(0), \
57 float64(0),float64(0),float64(0),float64(0),float64(0),float64(0), \
58 float64(0),float64(0),float64(0),float64(0),float64(0),float64(0), \
59 float64(0),float64(0),float64(0),float64(0),float64(0),float64(0), \
60 float64(0),float64(0),float64(0),float64(0),float64(0),float64(0), \
61 float64(0),float64(0),float64(0),float64(0),float64(0),float64(0) ]
65 return math.fmod(math.fmod(angle, 2*math.pi) + 2*math.pi, 2*math.pi)
75 if angle_diff > math.pi:
76 angle_diff = -(2*math.pi - angle_diff)
95 rospy.init_node(NAME, anonymous=
True)
98 print "waiting for service to set pose" 101 set_pose = rospy.ServiceProxy(self.
service_name, SetPose)
104 except rospy.ServiceException, e:
105 print "service call failed: %s"%e
115 for i
in range(0,len(sys.argv)):
116 if sys.argv[i] ==
'-update_rate':
117 if len(sys.argv) > i+1:
119 if sys.argv[i] ==
'-timeout':
120 if len(sys.argv) > i+1:
121 self.
timeout = float(sys.argv[i+1])
122 if sys.argv[i] ==
'-x':
123 if len(sys.argv) > i+1:
124 self.
target_p[0] = float(sys.argv[i+1])
125 if sys.argv[i] ==
'-y':
126 if len(sys.argv) > i+1:
127 self.
target_p[1] = float(sys.argv[i+1])
128 if sys.argv[i] ==
'-z':
129 if len(sys.argv) > i+1:
130 self.
target_p[2] = float(sys.argv[i+1])
131 if sys.argv[i] ==
'-R':
132 if len(sys.argv) > i+1:
133 self.
target_e[0] = float(sys.argv[i+1])
134 if sys.argv[i] ==
'-P':
135 if len(sys.argv) > i+1:
136 self.
target_e[1] = float(sys.argv[i+1])
137 if sys.argv[i] ==
'-Y':
138 if len(sys.argv) > i+1:
139 self.
target_e[2] = float(sys.argv[i+1])
140 if sys.argv[i] ==
'-f':
141 if len(sys.argv) > i+1:
143 if sys.argv[i] ==
'-s':
144 if len(sys.argv) > i+1:
147 if sys.argv[i] ==
'-t':
148 if len(sys.argv) > i+1:
151 if sys.argv[i] ==
'-p':
152 if len(sys.argv) > i+1:
167 h.stamp = rospy.get_rostime()
171 q = Quaternion(tmpq[0],tmpq[1],tmpq[2],tmpq[3])
173 pwc = PoseWithCovariance(pose,COV)
174 twc = TwistWithCovariance(Twist(Vector3(),Vector3()),COV)
176 target_pose = Odometry(h,child_frame_id,pwc,twc)
183 timeout_t = time.time() + self.
timeout 184 while not rospy.is_shutdown()
and time.time() < timeout_t:
186 self.pub_set_pose_topic.publish(target_pose)
195 -update_rate <Hz> - update rate, default to 10 Hz 196 -timeout <seconds> - test timeout in seconds. default to 1 seconds 201 -P <pitch in radians> 204 -s set pose service name 205 -t set pose topic name 206 -p wait for this ros topic to be published first 209 if __name__ ==
'__main__':
211 if len(sys.argv) == 1:
def print_usage(exit_code=0)
def normalize_angle(angle)
def shortest_angular_distance(angle_from, angle_to)
def setPoseService(self, pose_msg)
def normalize_angle_positive(angle)
def waitTopicInput(self, p3d)