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)
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
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:
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: