set_pose.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # Software License Agreement (BSD License)
3 #
4 # Copyright (c) 2008, Willow Garage, Inc.
5 # All rights reserved.
6 #
7 # Redistribution and use in source and binary forms, with or without
8 # modification, are permitted provided that the following conditions
9 # are met:
10 #
11 # * Redistributions of source code must retain the above copyright
12 # notice, this list of conditions and the following disclaimer.
13 # * Redistributions in binary form must reproduce the above
14 # copyright notice, this list of conditions and the following
15 # disclaimer in the documentation and/or other materials provided
16 # with the distribution.
17 # * Neither the name of the Willow Garage nor the names of its
18 # contributors may be used to endorse or promote products derived
19 # from this software without specific prior written permission.
20 #
21 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 # POSSIBILITY OF SUCH DAMAGE.
33 #
34 
35 ## Gazebo send position topic or calls set pose service for ros_sim_iface consumption
36 
37 PKG = 'gazebo_plugins'
38 NAME = 'set_pose'
39 
40 import math
41 import roslib
42 roslib.load_manifest(PKG)
43 
44 import sys, unittest
45 import os, os.path, threading, time
46 import rospy, rostest
47 
48 from gazebo_plugins.srv import SetPose
49 
50 from std_msgs.msg import String
51 from geometry_msgs.msg import Pose,Quaternion,Point, PoseStamped, PoseWithCovariance, TwistWithCovariance, Twist, Vector3
52 from nav_msgs.msg import Odometry
53 import tf.transformations as tft
54 from numpy import float64
55 
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) ]
62 
63 
65  return math.fmod(math.fmod(angle, 2*math.pi) + 2*math.pi, 2*math.pi)
66 
67 def normalize_angle(angle):
68  anorm = normalize_angle_positive(angle)
69  if anorm > math.pi:
70  anorm -= 2*math.pi
71  return anorm
72 
73 def shortest_angular_distance(angle_from, angle_to):
74  angle_diff = normalize_angle_positive(angle_to) - normalize_angle_positive(angle_from)
75  if angle_diff > math.pi:
76  angle_diff = -(2*math.pi - angle_diff)
77  return normalize_angle(angle_diff)
78 
80  def __init__(self):
81  self.update_rate=10
82  self.timeout=1
83  self.target_p = [0,0,0] # position
84  self.target_q = [0,0,0] # quaternion
85  self.target_e = [0,0,0] # euler pose
87  self.frame_id = "world"
88  self.service_name = "set_pose_service"
89  self.topic_name = "set_pose_topic"
90  self.use_topic = False;
91  self.use_service = False;
92  self.wait_topic_name = "clock"
93  self.wait_for_topic = False;
94 
95  rospy.init_node(NAME, anonymous=True)
96 
97  def setPoseService(self,pose_msg):
98  print "waiting for service to set pose"
99  rospy.wait_for_service(self.service_name);
100  try:
101  set_pose = rospy.ServiceProxy(self.service_name, SetPose)
102  resp1 = set_pose(pose_msg)
103  return resp1.success
104  except rospy.ServiceException, e:
105  print "service call failed: %s"%e
106 
107  def waitTopicInput(self,p3d):
108  #self.p3d_p = [p3d.pose.pose.position.x, p3d.pose.pose.position.y, p3d.pose.pose.position.z]
109  #self.p3d_q = [p3d.pose.pose.orientation.x, p3d.pose.pose.orientation.y, p3d.pose.pose.orientation.z, p3d.pose.pose.orientation.w]
110  #self.p3d_e = tft.euler_from_quaternion(self.p3d_q)
111  self.wait_topic_initialized = True
112 
113  def setPose(self):
114  # get goal from commandline
115  for i in range(0,len(sys.argv)):
116  if sys.argv[i] == '-update_rate':
117  if len(sys.argv) > i+1:
118  self.update_rate = float(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:
142  self.frame_id = sys.argv[i+1]
143  if sys.argv[i] == '-s':
144  if len(sys.argv) > i+1:
145  self.service_name = sys.argv[i+1]
146  self.use_service = True;
147  if sys.argv[i] == '-t':
148  if len(sys.argv) > i+1:
149  self.topic_name = sys.argv[i+1]
150  self.use_topic = True;
151  if sys.argv[i] == '-p':
152  if len(sys.argv) > i+1:
153  self.wait_topic_name = sys.argv[i+1]
154  self.wait_for_topic = True;
155 
156  # setup rospy
157  self.pub_set_pose_topic = rospy.Publisher(self.topic_name, Odometry)
158  rospy.Subscriber(self.wait_topic_name, rospy.AnyMsg, self.waitTopicInput)
159 
160  # wait for topic if user requests
161  if self.wait_for_topic:
162  while not self.wait_topic_initialized:
163  time.sleep(0.1)
164 
165  # compoose goal message
166  h = rospy.Header()
167  h.stamp = rospy.get_rostime()
168  h.frame_id = self.frame_id
169  p = Point(self.target_p[0],self.target_p[1],self.target_p[2])
170  tmpq = tft.quaternion_from_euler(self.target_e[0],self.target_e[1],self.target_e[2])
171  q = Quaternion(tmpq[0],tmpq[1],tmpq[2],tmpq[3])
172  pose = Pose(p,q)
173  pwc = PoseWithCovariance(pose,COV)
174  twc = TwistWithCovariance(Twist(Vector3(),Vector3()),COV)
175  child_frame_id = "" # what should this be?
176  target_pose = Odometry(h,child_frame_id,pwc,twc)
177 
178  if self.use_service:
179  success = self.setPoseService(target_pose)
180 
181  # publish topic if specified
182  if self.use_topic:
183  timeout_t = time.time() + self.timeout
184  while not rospy.is_shutdown() and time.time() < timeout_t:
185  # publish target pose
186  self.pub_set_pose_topic.publish(target_pose)
187 
188  if self.update_rate > 0:
189  time.sleep(1.0/self.update_rate)
190  else:
191  time.sleep(0.001)
192 
193 def print_usage(exit_code = 0):
194  print '''Commands:
195  -update_rate <Hz> - update rate, default to 10 Hz
196  -timeout <seconds> - test timeout in seconds. default to 1 seconds
197  -x <x in meters>
198  -y <y in meters>
199  -z <z in meters>
200  -R <roll in radians>
201  -P <pitch in radians>
202  -Y <yaw in radians>
203  -f target frame_id
204  -s set pose service name
205  -t set pose topic name
206  -p wait for this ros topic to be published first
207 '''
208 
209 if __name__ == '__main__':
210  #print usage if not arguments
211  if len(sys.argv) == 1:
212  print_usage()
213  else:
215  sic.setPose()
216 
217 
218 
def print_usage(exit_code=0)
Definition: set_pose.py:193
def normalize_angle(angle)
Definition: set_pose.py:67
def shortest_angular_distance(angle_from, angle_to)
Definition: set_pose.py:73
def setPoseService(self, pose_msg)
Definition: set_pose.py:97
def normalize_angle_positive(angle)
Definition: set_pose.py:64
def waitTopicInput(self, p3d)
Definition: set_pose.py:107


gazebo_plugins
Author(s): John Hsu
autogenerated on Tue Apr 6 2021 02:19:39