set_pose.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2008, Willow Garage, Inc.
00005 # All rights reserved.
00006 #
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions
00009 # are met:
00010 #
00011 #  * Redistributions of source code must retain the above copyright
00012 #    notice, this list of conditions and the following disclaimer.
00013 #  * Redistributions in binary form must reproduce the above
00014 #    copyright notice, this list of conditions and the following
00015 #    disclaimer in the documentation and/or other materials provided
00016 #    with the distribution.
00017 #  * Neither the name of the Willow Garage nor the names of its
00018 #    contributors may be used to endorse or promote products derived
00019 #    from this software without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 #
00034 
00035 ## Gazebo send position topic or calls set pose service for ros_sim_iface consumption
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] # position
00084     self.target_q = [0,0,0] # quaternion
00085     self.target_e = [0,0,0] # euler pose
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     #self.p3d_p = [p3d.pose.pose.position.x, p3d.pose.pose.position.y, p3d.pose.pose.position.z]
00109     #self.p3d_q = [p3d.pose.pose.orientation.x, p3d.pose.pose.orientation.y, p3d.pose.pose.orientation.z, p3d.pose.pose.orientation.w]
00110     #self.p3d_e = tft.euler_from_quaternion(self.p3d_q)
00111     self.wait_topic_initialized = True
00112 
00113   def setPose(self):
00114     # get goal from commandline
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     # setup rospy
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     # wait for topic if user requests
00161     if self.wait_for_topic:
00162       while not self.wait_topic_initialized:
00163         time.sleep(0.1)
00164 
00165     # compoose goal message
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 = "" # what should this be?
00176     target_pose = Odometry(h,child_frame_id,pwc,twc)
00177 
00178     if self.use_service:
00179       success = self.setPoseService(target_pose)
00180       
00181     # publish topic if specified
00182     if self.use_topic:
00183       timeout_t = time.time() + self.timeout
00184       while not rospy.is_shutdown() and time.time() < timeout_t:
00185         # publish target pose
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     #print usage if not arguments
00211     if len(sys.argv) == 1:
00212       print_usage()
00213     else:
00214       sic = SimIfaceControl()
00215       sic.setPose()
00216 
00217 
00218 


gazebo_plugins
Author(s): John Hsu
autogenerated on Thu Feb 23 2017 03:43:23