obstacle_avoidance.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 """
3 Obstacle Avoidance Example Code
4 
5 Written by: Zahi Kakish (zmk5)
6 License: BSD 3-Clause
7 
8 """
9 import argparse
10 import rospy
11 from geometry_msgs.msg import Twist
12 from pheeno_ros.pheeno_robot import PheenoRobot
13 
14 
15 def get_args():
16  """ Get arguments from rosrun for individual deployment. """
17  parser = argparse.ArgumentParser(
18  description="Obstacle avoidance python script."
19  )
20 
21  # Required arguments
22  parser.add_argument("-n", "--number",
23  action="store",
24  required=False,
25  help="Add a pheeno number namespace.",
26  default="")
27 
28  # The rationale behind rospy.myargv()[1:] is provided here:
29  # https://groups.google.com/a/rethinkrobotics.com/forum/#!topic/brr-users/ErXVWhRmtNA
30  return parser.parse_args(rospy.myargv()[1:])
31 
32 
33 def main():
34  """
35  Obstacle Avoidance Pheeno
36 
37  In this demo task, the Pheeno robot does a random walk, but avoids
38  obstacles that are a certain distance away from the Pheeno. Those
39  sensor limits are shown in `sensor_limits`.
40 
41  """
42  # Get arguments from argument parser.
43  input_args = get_args()
44  if input_args.number == "":
45  pheeno_number = ""
46 
47  else:
48  pheeno_number = str(input_args.number)
49 
50  # Initialize Node
51  rospy.init_node("pheeno_obstacle_avoidance")
52 
53  # Create PheenoRobot Object
54  pheeno = PheenoRobot(pheeno_number, 0.05, 0)
55 
56  # Other important variables
57  rate = rospy.Rate(10)
58  saved_time = rospy.get_rostime().secs
59  current_duration = 0
60  linear_vel = 0.05
61  angular_vel = 0.07
62  cmd_vel_msg = Twist()
63  ir_limit = 20
64 
65  while not rospy.is_shutdown():
66  # Find current duration
67  current_duration = rospy.get_rostime().secs - saved_time
68 
69  if current_duration <= 10:
70  pheeno.avoid_obstacle(ir_limit, 0, angular_vel)
71  cmd_vel_msg.linear.x = pheeno.linear_velocity
72  cmd_vel_msg.angular.z = pheeno.angular_velocity
73 
74  # To avoid contradicting turns with obstacle avoidance, this
75  # logic statement prevents the pheeno from going back and forth.
76  if angular_vel != pheeno.angular_velocity:
77  angular_vel = pheeno.angular_velocity
78 
79  elif current_duration < 20:
80  pheeno.avoid_obstacle(ir_limit, linear_vel, 0)
81  cmd_vel_msg.linear.x = pheeno.linear_velocity
82  cmd_vel_msg.angular.z = pheeno.angular_velocity
83 
84  else:
85  # Reset variables
86  saved_time = rospy.get_rostime().secs
87  angular_vel = pheeno.random_turn() * angular_vel
88 
89  # Publish to cmd_vel Topic and sleep
90  pheeno.publish_cmd_vel(cmd_vel_msg)
91  rate.sleep()
92 
93 
94 if __name__ == "__main__":
95  try:
96  main()
97 
98  except rospy.ROSInterruptException:
99  pass
100 
101  finally:
102  rospy.loginfo("Exiting 'pheeno_obstacle_avoidance' node.")


pheeno_ros
Author(s): Zahi Kakish, Sean Wilson
autogenerated on Mon Jun 10 2019 14:10:48