pheeno_robot.py
Go to the documentation of this file.
00001 """
00002 Pheeno Robot Python Class files for coding
00003 
00004 Notes
00005 -----
00006 Contains files directly related to Pheeno Robot movement, sensors, etc. This
00007 object class can be used in other python scripts to give one direct control
00008 of the Pheeno without needing to write extra attributes, callbacks, etc.
00009 
00010 Written by: Zahi Kakish (zmk5)
00011 License: BSD 3-Clause
00012 
00013 """
00014 import random
00015 import rospy
00016 from geometry_msgs.msg import Twist
00017 from geometry_msgs.msg import Vector3
00018 from std_msgs.msg import Float32
00019 from std_msgs.msg import Int16
00020 
00021 
00022 class PheenoRobot(object):
00023     """
00024     PheenoRobot Class
00025 
00026     Provides a easier way of generating all the necessary attributes, ROS
00027     publishers and subscribers, and useful methods for running a Pheeno robot.
00028     Just instantiate the class to a variable within a script and all the
00029     necessary ROS calls are generated automatically. Many of the attributes
00030     of the Pheeno are hidden from users (except velocity ones) and require
00031     getters to get them.
00032 
00033     Parameters
00034     ----------
00035     robot_id : str
00036         ID number of the pheeno.
00037     linear : int/float
00038         Initial linear velocity of the robot.
00039     angular : int/float
00040         Initial angular velocity of the robot.
00041     obstacle_velocity : int/float
00042         Velocity for obstacle avoidance. In terms of angular velocity, may be faster or
00043         slower than the current value of angular velocity.
00044 
00045     Attributes
00046     ----------
00047     pheeno_id : str
00048         The ID of the pheeno that is used as the namespace for pheeno ROS publishers and
00049         subscribers.
00050     linear_velocity : float (default 0)
00051         The linear velocity of the robot.
00052     angular_velocity : float (default 0)
00053         The angular velocity of the robot.
00054     obstacle_velocity : float (default 0.5)
00055         If a robot approaces an obstacle, this is the angular velocity used for obstacle
00056         avoidance.
00057 
00058     Methods
00059     -------
00060     get_ir_data
00061         Returns the IR data for a specific sensor. If the "all" option is
00062         used, then all the data is returned as a list.
00063         Options: "center", "back", "right", "left", "cr", "cl", "all"
00064     get_encoder_data
00065         Returns encoder data for a specific encoder. If the "all" option is
00066         used, then all the data is returned as a list.
00067         Options: "LL", "LR", "RL", "RR", "all"
00068     get_magnetometer_data
00069         Returns magnetometer data for a specific coordinate. If the "all" option
00070         is used, then all the data is returned as a list.
00071         Options: "x", "y", "z", "all"
00072     get_gyroscope_data
00073         Returns gyroscope data for a specific coordinate. If "all" option is
00074         used, then all the data is returned as a list.
00075         Options: "x", "y", "z", "all"
00076     get_accelerometer_data
00077         Returns accelerometer data for a specific coordinate. If "all" option
00078         is used, then all the data is returned as a list.
00079         Options: "x", "y", "z", "all"
00080     avoid_obstacle
00081         Obstacle avoidance algorithm based on IR sensors. Call when wanting to
00082         plan motion while still trying to avoid obstacles.
00083     is_obstacle_detected
00084         Bool return if any IR sensor has been tripped.
00085     is_ir_sensor_triggered
00086         Bool return if a SPECIFIC IR sensor has been tripped.
00087 
00088     Examples
00089     --------
00090     >>> # If you have more than 10 robots add a `0` to the first
00091     >>> # 9 numbers.
00092     >>> pheeno = PheenoRobot("01", linear_vel=0.8, angular_vel=0)
00093 
00094     """
00095     def __init__(self, robot_id, linear_velocity=0, angular_velocity=0, obstacle_velocity=0.5):
00096         if robot_id == "":
00097             self.pheeno_id = robot_id
00098 
00099         else:
00100             self.pheeno_id = "/pheeno_" + str(robot_id)
00101 
00102         self._velocity = {"linear": float(linear_velocity),
00103                           "angular": float(angular_velocity),
00104                           "obstacle": float(obstacle_velocity)}
00105 
00106         # Sensor Values
00107         self._sensor_data = {"ir": {"center": 0.0,
00108                                     "back": 0.0,
00109                                     "right": 0.0,
00110                                     "left": 0.0,
00111                                     "cr": 0.0,
00112                                     "cl": 0.0},
00113                              "odom": {"position": 0.0,
00114                                       "orient": 0.0,
00115                                       "linear": 0.0,
00116                                       "angular": 0.0},
00117                              "encoder": {"LL": 0,
00118                                          "LR": 0,
00119                                          "RL": 0,
00120                                          "RR": 0},
00121                              "magnetometer": {"x": 0.0,
00122                                               "y": 0.0,
00123                                               "z": 0.0},
00124                              "gyroscope": {"x": 0.0,
00125                                            "y": 0.0,
00126                                            "z": 0.0},
00127                              "accelerometer": {"x": 0.0,
00128                                                "y": 0.0,
00129                                                "z": 0.0}}
00130 
00131         # cmd_vel Publisher
00132         self._pub = rospy.Publisher(self.pheeno_id + "/cmd_vel",
00133                                     Twist,
00134                                     queue_size=100)
00135 
00136         # IR Sensor Subscribers
00137         rospy.Subscriber(self.pheeno_id + "/scan_center", Float32,
00138                          self.__callback_ir_sensor, callback_args="center")
00139         rospy.Subscriber(self.pheeno_id + "/scan_back", Float32,
00140                          self.__callback_ir_sensor, callback_args="back")
00141         rospy.Subscriber(self.pheeno_id + "/scan_right", Float32,
00142                          self.__callback_ir_sensor, callback_args="right")
00143         rospy.Subscriber(self.pheeno_id + "/scan_left", Float32,
00144                          self.__callback_ir_sensor, callback_args="left")
00145         rospy.Subscriber(self.pheeno_id + "/scan_cr", Float32,
00146                          self.__callback_ir_sensor, callback_args="cr")
00147         rospy.Subscriber(self.pheeno_id + "/scan_cl", Float32,
00148                          self.__callback_ir_sensor, callback_args="cl")
00149 
00150         # Encoder Subscribers
00151         rospy.Subscriber(self.pheeno_id + "/encoder_LL", Int16,
00152                          self.__callback_encoder, callback_args="LL")
00153         rospy.Subscriber(self.pheeno_id + "/encoder_LR", Int16,
00154                          self.__callback_encoder, callback_args="LR")
00155         rospy.Subscriber(self.pheeno_id + "/encoder_RL", Int16,
00156                          self.__callback_encoder, callback_args="RL")
00157         rospy.Subscriber(self.pheeno_id + "/encoder_RR", Int16,
00158                          self.__callback_encoder, callback_args="RR")
00159 
00160         # Magnetometer, Gyroscope, Accelerometer Subscriber
00161         rospy.Subscriber(self.pheeno_id + "/magnetometer", Vector3,
00162                          self.__callback_magnetometer)
00163         rospy.Subscriber(self.pheeno_id + "/gyroscope", Vector3,
00164                          self.__callback_gyroscope)
00165         rospy.Subscriber(self.pheeno_id + "/accelerometer", Vector3,
00166                          self.__callback_accelerometer)
00167 
00168     def __callback_ir_sensor(self, msg, location):
00169         """ Callback for IR sensors subscriber """
00170         self._sensor_data["ir"][location] = msg.data
00171 
00172     def __callback_encoder(self, msg, location):
00173         """ Callback for Encoder sensors subscriber """
00174         self._sensor_data["encoder"][location] = msg.data
00175 
00176     def __callback_magnetometer(self, msg):
00177         """ Callback for Magnetometer sensor subscriber """
00178         self._sensor_data["magnetometer"]["x"] = msg.x
00179         self._sensor_data["magnetometer"]["y"] = msg.y
00180         self._sensor_data["magnetometer"]["z"] = msg.z
00181 
00182     def __callback_gyroscope(self, msg):
00183         """ Callback for Gyroscope sensor subscriber """
00184         self._sensor_data["gyroscope"]["x"] = msg.x
00185         self._sensor_data["gyroscope"]["y"] = msg.y
00186         self._sensor_data["gyroscope"]["z"] = msg.z
00187 
00188     def __callback_accelerometer(self, msg):
00189         """ Callback for Accelerometer sensor subscriber """
00190         self._sensor_data["accelerometer"]["x"] = msg.x
00191         self._sensor_data["accelerometer"]["y"] = msg.y
00192         self._sensor_data["accelerometer"]["z"] = msg.z
00193 
00194     @property
00195     def linear_velocity(self):
00196         """ Linear velocity property """
00197         return self._velocity["linear"]
00198 
00199     @linear_velocity.setter
00200     def linear_velocity(self, value):
00201         """ Linear velocity setter property """
00202         if -1.0 < value < 1.0:
00203             self._velocity["linear"] = value
00204 
00205         else:
00206             raise ValueError("Motor values must be between -1 and 1.")
00207 
00208     @property
00209     def angular_velocity(self):
00210         """ Angular velocity property """
00211         return self._velocity["angular"]
00212 
00213     @angular_velocity.setter
00214     def angular_velocity(self, value):
00215         """ Angular velocity setter property """
00216         if -1.0 < value < 1.0:
00217             self._velocity["angular"] = value
00218 
00219         else:
00220             raise ValueError("Motor values must be between -1 and 1.")
00221 
00222     @property
00223     def obstacle_velocity(self):
00224         """ Obstacle velocity property """
00225         return self._velocity["obstacle"]
00226 
00227     @obstacle_velocity.setter
00228     def obstacle_velocity(self, value):
00229         """ Obstacle velocity setter property """
00230         if 0 < value < 1.0:
00231             self._velocity["obstacle"] = value
00232 
00233         else:
00234             raise ValueError("Obstacle velocity should be an positive and between (0 < v < 1)")
00235 
00236     def get_ir_data(self, key):
00237         """ Getter for IR Sensor data """
00238         if key == "all":
00239             values = []
00240             for sensor in ["center", "back", "right", "left", "cr", "cl"]:
00241                 values.append(self._sensor_data["ir"][sensor])
00242 
00243             return values
00244 
00245         return self._sensor_data["ir"][key]
00246 
00247     def get_encoder_data(self, key):
00248         """ Getter for Encoder data """
00249         if key == "all":
00250             values = []
00251             for sensor in ["LL", "LR", "RL", "RR"]:
00252                 values.append(self._sensor_data["encoder"][sensor])
00253 
00254             return values
00255 
00256         return self._sensor_data["encoder"][key]
00257 
00258     def get_magnetometer_data(self, key):
00259         """ Getter for Magnetometer data """
00260         if key == "all":
00261             values = []
00262             for sensor in ["x", "y", "z"]:
00263                 values.append(self._sensor_data["magnetometer"][sensor])
00264 
00265             return values
00266 
00267         return self._sensor_data["magnetometer"][key]
00268 
00269     def get_gyroscope_data(self, key):
00270         """ Getter for Gyroscope data """
00271         if key == "all":
00272             values = []
00273             for sensor in ["x", "y", "z"]:
00274                 values.append(self._sensor_data["gyroscope"][sensor])
00275 
00276             return values
00277 
00278         return self._sensor_data["gyroscope"][key]
00279 
00280     def get_accelerometer_data(self, key):
00281         """ Getter for Accelerometer data """
00282         if key == "all":
00283             values = []
00284             for sensor in ["x", "y", "z"]:
00285                 values.append(self._sensor_data["accelerometer"][sensor])
00286 
00287             return values
00288 
00289         return self._sensor_data["accelerometer"][key]
00290 
00291     def publish_cmd_vel(self, twist_msg):
00292         """ Publish an movement  velocity on the `cmd_vel` topic """
00293         self._pub.publish(twist_msg)
00294 
00295     def avoid_obstacle(self, limit, linear_vel, angular_vel):
00296         """
00297         Obstacle avoidance algorithm
00298 
00299         Given a desired linear and angular velocity, the algorithm will set those
00300         values as the robots linear and angular velocity if no obstacle is in
00301         the way. If there is, the linear velocity of the robot is set to zero,
00302         and the angular velocity is set of a positive or negative version of the
00303         obstacle velocity robot attribute depending on certain conditions.
00304 
00305         Arguments
00306         ---------
00307         limit : int/float
00308             The ir sensor limit.
00309         linear_vel : float
00310             The desired linear velocity a user would like to set if there are
00311             no obstacles in the way.
00312         angular_vel : float
00313             The desired angular velocity a user would like to set if there are
00314             no obstacles in the way.
00315 
00316         Examples
00317         -------
00318         For a robot moving along a linear path, the method may be called as such:
00319         >>> ir_limit = 15  # in cm
00320         >>> desired_linear = 0.7
00321         >>> desired_angular = 0  # b/c we don't want it to turn
00322         >>> pheeno.avoid_obstacle(ir_limit, desired_linear, desired_angular)
00323 
00324         If no obstacle is in the path of the robot, the attributes `linear_velocity`
00325         and `angular_velocity` are set to 0.7 and 0, respectively. If not, the
00326         values are set based on the internal logic of the algorithm. Assign these
00327         values to the proper places in a `geometry_msg.Twist()` msg and call the
00328         `publish_cmd_vel(twist_msg)` method to enact the changes.
00329 
00330 
00331         """
00332         if self.is_obstacle_detected(limit):
00333             if self.is_ir_sensor_triggered("center", limit):
00334                 if (self._sensor_data["ir"]["right"] < 10 and
00335                         self._sensor_data["ir"]["left"] < 10):
00336                     angular = self.random_turn() * self._velocity["obstacle"]
00337 
00338                 if self._sensor_data["ir"]["right"] < self._sensor_data["ir"]["left"]:
00339                     angular = -1 * self._velocity["obstacle"]  # Turn Left
00340 
00341                 else:
00342                     angular = self._velocity["obstacle"]  # Turn Right
00343 
00344             elif (self.is_ir_sensor_triggered("cl", limit) and
00345                   self.is_ir_sensor_triggered("cr", limit)):
00346                 angular = self.random_turn() * self._velocity["obstacle"]
00347 
00348             # If individual IR sensors are triggered.
00349             else:
00350                 angular = self._velocity["obstacle"]
00351 
00352             # Set linear velocity to 0, so only angular velocity will occur.
00353             linear = 0
00354 
00355         # If no obstacle detected, use the default terms.
00356         else:
00357             angular = angular_vel
00358             linear = linear_vel
00359 
00360         # Set new velocities
00361         self._velocity["linear"] = linear
00362         self._velocity["angular"] = angular
00363 
00364     def is_obstacle_detected(self, limit):
00365         """ Returns bool if an obstacle is in the way of any IR sensor """
00366         for key in self._sensor_data["ir"]:
00367             if self._sensor_data["ir"][key] <= limit:
00368                 return True
00369 
00370         return False
00371 
00372     def is_ir_sensor_triggered(self, key, limit):
00373         """ Returns bool if the specific IR sensor is triggered """
00374         if self._sensor_data["ir"][key] <= limit:
00375             return True
00376 
00377         return False
00378 
00379     @staticmethod
00380     def random_turn():
00381         """ Given a velocity, generate a random turn direction """
00382         if random.random() < 0.5:
00383             rospy.loginfo("Turning Left")
00384             return -1.0
00385 
00386         rospy.loginfo("Turning Right")
00387         return 1.0


pheeno_ros
Author(s): Zahi Kakish, Sean Wilson
autogenerated on Thu Jun 6 2019 21:26:16