random_walk.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 """
3 Random Walk 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  Random Walk Pheeno
36
37  In this demo task, the Pheeno robot undergoes 3 count turn in a
38  specific direction followed by a 12 count linear motion. At 15
39  count the direction, the count is reset and a new direction is
40  chosen.
41
42  """
43  # Get arguments from argument parser.
44  input_args = get_args()
45  if input_args.number == "":
46  pheeno_number = ""
47
48  else:
49  pheeno_number = str(input_args.number)
50
51  # Initialize Node
52  rospy.init_node("pheeno_random_walk")
53
54  # Create PheenoRobot Object
55  pheeno = PheenoRobot(pheeno_number, 0.05, 0)
56
57  # Other Important Variables
58  count = 0
59  cmd_vel_msg = Twist()
60  linear_vel = 0.05
61  angular_vel = 0.07
62
63  rate = rospy.Rate(15)
64
65  while not rospy.is_shutdown():
66  if count == 0:
67  cmd_vel_msg.linear.x = 0
68  cmd_vel_msg.angular.z = pheeno.random_turn() * angular_vel
69  count += 1
70
71  elif count == 3:
72  cmd_vel_msg.linear.x = linear_vel
73  cmd_vel_msg.angular.z = 0
74  count += 1
75
76  elif count == 15:
77  count = 0
78
79  else:
80  count += 1
81
82  pheeno.publish_cmd_vel(cmd_vel_msg)
83  rate.sleep()
84
85
86
87 if __name__ == "__main__":
88  try:
89  main()
90
91  except rospy.ROSInterruptException:
92  pass
93
94  finally: