obstacle_avoidance.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 """
00003 Obstacle Avoidance Example Code
00004 
00005 Written by: Zahi Kakish (zmk5)
00006 License: BSD 3-Clause
00007 
00008 """
00009 import argparse
00010 import rospy
00011 from geometry_msgs.msg import Twist
00012 from pheeno_robot import PheenoRobot
00013 
00014 
00015 def get_args():
00016     """ Get arguments from rosrun for individual deployment. """
00017     parser = argparse.ArgumentParser(
00018         description="Obstacle avoidance python script."
00019     )
00020 
00021     # Required arguments
00022     parser.add_argument("-n", "--number",
00023                         action="store",
00024                         required=False,
00025                         help="Add a pheeno number namespace.",
00026                         default="")
00027 
00028     # The rationale behind rospy.myargv()[1:] is provided here:
00029     # https://groups.google.com/a/rethinkrobotics.com/forum/#!topic/brr-users/ErXVWhRmtNA
00030     return parser.parse_args(rospy.myargv()[1:])
00031 
00032 
00033 def main():
00034     """
00035     Obstacle Avoidance Pheeno
00036 
00037     In this demo task, the Pheeno robot does a random walk, but avoids
00038     obstacles that are a certain distance away from the Pheeno. Those
00039     sensor limits are shown in `sensor_limits`.
00040 
00041     """
00042     # Get arguments from argument parser.
00043     input_args = get_args()
00044     if input_args.number == "":
00045         pheeno_number = ""
00046 
00047     else:
00048         pheeno_number = str(input_args.number)
00049 
00050     # Initialize Node
00051     rospy.init_node("pheeno_obstacle_avoidance")
00052 
00053     # Create PheenoRobot Object
00054     pheeno = PheenoRobot(pheeno_number, 0.05, 0)
00055 
00056     # Other important variables
00057     rate = rospy.Rate(10)
00058     saved_time = rospy.get_rostime().secs
00059     current_duration = 0
00060     linear_vel = 0.05
00061     angular_vel = 0.07
00062     cmd_vel_msg = Twist()
00063     ir_limit = 20
00064 
00065     while not rospy.is_shutdown():
00066         # Find current duration
00067         current_duration = rospy.get_rostime().secs - saved_time
00068 
00069         if current_duration <= 10:
00070             pheeno.avoid_obstacle(ir_limit, 0, angular_vel)
00071             cmd_vel_msg.linear.x = pheeno.linear_velocity
00072             cmd_vel_msg.angular.z = pheeno.angular_velocity
00073 
00074             # To avoid contradicting turns with obstacle avoidance, this
00075             # logic statement prevents the pheeno from going back and forth.
00076             if angular_vel != pheeno.angular_velocity:
00077                 angular_vel = pheeno.angular_velocity
00078 
00079         elif current_duration < 20:
00080             pheeno.avoid_obstacle(ir_limit, linear_vel, 0)
00081             cmd_vel_msg.linear.x = pheeno.linear_velocity
00082             cmd_vel_msg.angular.z = pheeno.angular_velocity
00083 
00084         else:
00085             # Reset variables
00086             saved_time = rospy.get_rostime().secs
00087             angular_vel = pheeno.random_turn() * angular_vel
00088 
00089         # Publish to cmd_vel Topic and sleep
00090         pheeno.publish_cmd_vel(cmd_vel_msg)
00091         rate.sleep()
00092 
00093 
00094 if __name__ == "__main__":
00095     try:
00096         main()
00097 
00098     except rospy.ROSInterruptException:
00099         pass
00100 
00101     finally:
00102         rospy.loginfo("Exiting 'pheeno_obstacle_avoidance' node.")


pheeno_ros_sim
Author(s): Zahi Kakish
autogenerated on Thu Jun 6 2019 19:59:41