4 Copyright (c) 2020, Ubiquity Robotics 6 Redistribution and use in source and binary forms, with or without 7 modification, are permitted provided that the following conditions are met: 8 * Redistributions of source code must retain the above copyright notice, this 9 list of conditions and the following disclaimer. 10 * Redistributions in binary form must reproduce the above copyright notice, 11 this list of conditions and the following disclaimer in the documentation 12 and/or other materials provided with the distribution. 13 * Neither the name of display_node nor the names of its 14 contributors may be used to endorse or promote products derived from 15 this software without specific prior written permission. 16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 Example client program for sending multiple move_basic commands 36 python3 =
True if sys.hexversion > 0x03000000
else False 40 import geometry_msgs.msg
42 from move_base_msgs.msg
import MoveBaseAction, MoveBaseGoal
51 print """-h --help - This help menu 52 -c --continue - Continuous operation, no prompts to continue 53 -w --waypoints - Select a waypoint list by name as below 56 octagon An octagon pattern 57 figure8 A figure 8 dual octagon 58 -s --scale - A different scale for the pattern 59 -x --offsetX - An offset for X map placement of the pattern 60 -y --offsetY - An offset for Y map placement of the pattern """ 75 [ 0.00, 0.00, 0.000,
"MOVE: Leg A" ],
76 [ 0.50, 0.00, 0.000,
"MOVE: Leg B" ]
81 [ 0.00, 0.00, 0.000,
"MOVE: Leg A" ],
82 [ 0.50, 0.00, 1.570,
"MOVE: Leg B" ],
83 [ 0.50, 0.50, 3.140,
"MOVE: Leg C" ],
84 [ 0.00, 0.50, -1.570,
"MOVE: Leg D" ]
90 [ 0.40, 0.30, 0.000,
"MOVE: Leg A" ],
91 [ 0.60, 0.10, -0.785,
"MOVE: Leg B" ],
92 [ 0.60, -0.10, -1.571,
"MOVE: Leg C" ],
93 [ 0.40, -0.30, -2.356,
"MOVE: Leg D" ],
94 [ 0.20, -0.30, -3.141,
"Move: Leg E" ],
95 [ 0.00, -0.10, 2.356,
"Move: Leg F" ],
96 [ 0.00, 0.10, 1.571,
"Move: Leg G" ],
97 [ 0.20, 0.30, 0.785,
"Move: Leg H" ]
102 [ 0.40, 0.30, 0.000,
"MOVE: Leg A" ],
103 [ 0.60, 0.10, -0.785,
"MOVE: Leg B" ],
104 [ 0.60, -0.10, -1.571,
"MOVE: Leg C" ],
105 [ 0.40, -0.30, -2.356,
"MOVE: Leg D" ],
106 [ 0.20, -0.30, -3.141,
"Move: Leg E" ],
107 [ 0.00, -0.10, 2.356,
"Move: Leg F" ],
108 [ 0.00, 0.10, 1.571,
"Move: Leg G" ],
109 [-0.20, 0.30, 2.356,
"Move: Leg H" ],
110 [-0.40, 0.30, 3.141,
"Move: Leg I" ],
111 [-0.60, 0.10, -2.356,
"Move: Leg J" ],
112 [-0.60, -0.10, -1.571,
"Move: Leg K" ],
113 [-0.40, -0.30, -0.785,
"Move: Leg L" ],
114 [-0.20, -0.30, 0.000,
"Move: Leg M" ],
115 [ 0.00, -0.10, 0.785,
"Move: Leg N" ],
116 [ 0.00, 0.10, 1.571,
"Move: Leg O" ],
117 [ 0.20, 0.30, 0.785,
"Move: Leg P" ]
124 Constructor for our class 128 rospy.init_node(
'controller')
145 opts, args = getopt.getopt(sys.argv[1:],
'hcw:s:x:y:h', \
146 [
'help',
'continue',
'waypoints=',
'scale=',
'offsetX=',
'offsetY='])
147 except getopt.GetoptError
as err:
149 print "Error in recognized options" 155 if o
in (
"-h",
"--help"):
156 print (
"displaying help")
159 elif o
in (
"-c",
"--continue"):
162 elif o
in (
"-w",
"--waypoints"):
169 elif (a ==
"figure8"):
171 elif (a ==
"octagon"):
174 print (
"Invalid choice of waypoint list")
176 print "Waypoint list will be '%s'", waypointList
177 elif o
in (
"-s",
"--scaleX"):
180 elif o
in (
"-x",
"--offsetX"):
182 elif o
in (
"-y",
"--offsetY"):
185 print (
"WaypointsName: %s scaleX %f scaleY %f offsetX %f offsetY %f" \
188 print "A total of %d waypoints is in the list " % (len(waypointList))
199 goal = MoveBaseGoal()
200 goal.target_pose.header.frame_id =
"map" 201 goal.target_pose.header.stamp = rospy.Time.now()
202 goal.target_pose.pose.position.x = x
203 goal.target_pose.pose.position.y = y
206 x , y, z, w = tf.transformations.quaternion_from_euler(0, 0, yaw)
207 goal.target_pose.pose.orientation.x = x
208 goal.target_pose.pose.orientation.y = y
209 goal.target_pose.pose.orientation.z = z
210 goal.target_pose.pose.orientation.w = w
211 now = rospy.get_rostime()
212 print "[%i.%i] PubMove: %s x,y,z,w of %f %f %f %f yaw %f" \
213 % (now.secs,now.nsecs,comment,x,y,z,w,yaw)
216 client.wait_for_server()
219 client.send_goal(goal)
221 now = rospy.get_rostime()
222 print "[%i.%i] Waiting for result ..." % (now.secs, now.nsecs)
223 wait = client.wait_for_result()
225 rospy.logerr(
"Action server not available!")
226 rospy.signal_shutdown(
"Action server not available!")
228 now = rospy.get_rostime()
229 print "[%i.%i] Received result" % (now.secs, now.nsecs)
230 return client.get_result()
237 print "ROS publisher publishing goals to move basic" 243 for waypoint
in waypointList:
244 x,y,yaw,comment = waypoint
247 now = rospy.get_rostime()
248 print "[%i.%i] Waypoint: %s X %f Y %f yaw %f will be published now" \
249 % (now.secs,now.nsecs,comment,x, y, yaw)
253 if moveResult ==
True:
254 print "ERROR: MoveBasic Bad Status! for Waypoint: X %f Y %f yaw %f " % (x, y, yaw)
256 print "[%i.%i] Waypoint: %s X %f Y %f yaw %f has been reached" \
257 % (now.secs,now.nsecs,comment,x, y, yaw)
261 raw_input(
"Hit ENTER to go to next waypoint ... ")
263 if __name__ ==
"__main__":
list figureLine
Define lists of waypoints in terms of X,Y values in meters and robot angular yaw pose The entries on ...
def publishMoveBaseGoalWaitForReply(self, x, y, yaw, comment)