
Public Member Functions | |
| def | __init__ (self, robot_id, linear_velocity=0, angular_velocity=0, obstacle_velocity=0.5) |
| def | angular_velocity (self) |
| def | angular_velocity (self, value) |
| def | avoid_obstacle (self, limit, linear_vel, angular_vel) |
| def | get_accelerometer_data (self, key) |
| def | get_encoder_data (self, key) |
| def | get_gyroscope_data (self, key) |
| def | get_ir_data (self, key) |
| def | get_magnetometer_data (self, key) |
| def | is_ir_sensor_triggered (self, key, limit) |
| def | is_obstacle_detected (self, limit) |
| def | linear_velocity (self) |
| def | linear_velocity (self, value) |
| def | obstacle_velocity (self) |
| def | obstacle_velocity (self, value) |
| def | publish_cmd_vel (self, twist_msg) |
Static Public Member Functions | |
| def | random_turn () |
Public Attributes | |
| pheeno_id | |
Private Member Functions | |
| def | __callback_accelerometer (self, msg) |
| def | __callback_encoder (self, msg, location) |
| def | __callback_gyroscope (self, msg) |
| def | __callback_ir_sensor (self, msg, location) |
| def | __callback_magnetometer (self, msg) |
Private Attributes | |
| _pub | |
| _sensor_data | |
| _velocity | |
PheenoRobot Class
Provides a easier way of generating all the necessary attributes, ROS
publishers and subscribers, and useful methods for running a Pheeno robot.
Just instantiate the class to a variable within a script and all the
necessary ROS calls are generated automatically. Many of the attributes
of the Pheeno are hidden from users (except velocity ones) and require
getters to get them.
Parameters
----------
robot_id : str
ID number of the pheeno.
linear : int/float
Initial linear velocity of the robot.
angular : int/float
Initial angular velocity of the robot.
obstacle_velocity : int/float
Velocity for obstacle avoidance. In terms of angular velocity, may be
faster or slower than the current value of angular velocity.
Attributes
----------
pheeno_id : str
The ID of the pheeno that is used as the namespace for pheeno ROS
publishers and subscribers.
linear_velocity : float (default 0)
The linear velocity of the robot.
angular_velocity : float (default 0)
The angular velocity of the robot.
obstacle_velocity : float (default 0.5)
If a robot approaces an obstacle, this is the angular velocity used for
obstacle avoidance.
Methods
-------
get_ir_data
Returns the IR data for a specific sensor. If the "all" option is
used, then all the data is returned as a list.
Options: "center", "back", "right", "left", "cr", "cl", "all"
get_encoder_data
Returns encoder data for a specific encoder. If the "all" option is
used, then all the data is returned as a list.
Options: "LL", "LR", "RL", "RR", "all"
get_magnetometer_data
Returns magnetometer data for a specific coordinate. If the "all" option
is used, then all the data is returned as a list.
Options: "x", "y", "z", "all"
get_gyroscope_data
Returns gyroscope data for a specific coordinate. If "all" option is
used, then all the data is returned as a list.
Options: "x", "y", "z", "all"
get_accelerometer_data
Returns accelerometer data for a specific coordinate. If "all" option
is used, then all the data is returned as a list.
Options: "x", "y", "z", "all"
avoid_obstacle
Obstacle avoidance algorithm based on IR sensors. Call when wanting to
plan motion while still trying to avoid obstacles.
is_obstacle_detected
Bool return if any IR sensor has been tripped.
is_ir_sensor_triggered
Bool return if a SPECIFIC IR sensor has been tripped.
Examples
--------
>>> # If you have more than 10 robots add a `0` to the first
>>> # 9 numbers.
>>> pheeno = PheenoRobot("01", linear_vel=0.8, angular_vel=0)
Definition at line 22 of file pheeno_robot.py.
| def pheeno_ros.pheeno_robot.PheenoRobot.__init__ | ( | self, | |
| robot_id, | |||
linear_velocity = 0, |
|||
angular_velocity = 0, |
|||
obstacle_velocity = 0.5 |
|||
| ) |
Definition at line 96 of file pheeno_robot.py.
|
private |
Callback for Accelerometer sensor subscriber
Definition at line 189 of file pheeno_robot.py.
|
private |
Callback for Encoder sensors subscriber
Definition at line 173 of file pheeno_robot.py.
|
private |
Callback for Gyroscope sensor subscriber
Definition at line 183 of file pheeno_robot.py.
|
private |
Callback for IR sensors subscriber
Definition at line 169 of file pheeno_robot.py.
|
private |
Callback for Magnetometer sensor subscriber
Definition at line 177 of file pheeno_robot.py.
| def pheeno_ros.pheeno_robot.PheenoRobot.angular_velocity | ( | self | ) |
Angular velocity property
Definition at line 210 of file pheeno_robot.py.
| def pheeno_ros.pheeno_robot.PheenoRobot.angular_velocity | ( | self, | |
| value | |||
| ) |
Angular velocity setter property
Definition at line 215 of file pheeno_robot.py.
| def pheeno_ros.pheeno_robot.PheenoRobot.avoid_obstacle | ( | self, | |
| limit, | |||
| linear_vel, | |||
| angular_vel | |||
| ) |
Obstacle avoidance algorithm
Given a desired linear and angular velocity, the algorithm will set
those values as the robots linear and angular velocity if no obstacle
is in the way. If there is, the linear velocity of the robot is set to
zero, and the angular velocity is set of a positive or negative version
of the obstacle velocity robot attribute depending on certain
conditions.
Arguments
---------
limit : int/float
The ir sensor limit.
linear_vel : float
The desired linear velocity a user would like to set if there are
no obstacles in the way.
angular_vel : float
The desired angular velocity a user would like to set if there are
no obstacles in the way.
Examples
-------
For a robot moving along a linear path, the method may be called as such:
>>> ir_limit = 15 # in cm
>>> desired_linear = 0.7
>>> desired_angular = 0 # b/c we don't want it to turn
>>> pheeno.avoid_obstacle(ir_limit, desired_linear, desired_angular)
If no obstacle is in the path of the robot, the attributes
`linear_velocity` and `angular_velocity` are set to 0.7 and 0,
respectively. If not, the values are set based on the internal logic of
the algorithm. Assign these values to the proper places in a
`geometry_msg.Twist()` msg and call the `publish_cmd_vel(twist_msg)`
method to enact the changes.
Definition at line 296 of file pheeno_robot.py.
| def pheeno_ros.pheeno_robot.PheenoRobot.get_accelerometer_data | ( | self, | |
| key | |||
| ) |
Getter for Accelerometer data
Definition at line 281 of file pheeno_robot.py.
| def pheeno_ros.pheeno_robot.PheenoRobot.get_encoder_data | ( | self, | |
| key | |||
| ) |
Getter for Encoder data
Definition at line 248 of file pheeno_robot.py.
| def pheeno_ros.pheeno_robot.PheenoRobot.get_gyroscope_data | ( | self, | |
| key | |||
| ) |
Getter for Gyroscope data
Definition at line 270 of file pheeno_robot.py.
| def pheeno_ros.pheeno_robot.PheenoRobot.get_ir_data | ( | self, | |
| key | |||
| ) |
Getter for IR Sensor data
Definition at line 237 of file pheeno_robot.py.
| def pheeno_ros.pheeno_robot.PheenoRobot.get_magnetometer_data | ( | self, | |
| key | |||
| ) |
Getter for Magnetometer data
Definition at line 259 of file pheeno_robot.py.
| def pheeno_ros.pheeno_robot.PheenoRobot.is_ir_sensor_triggered | ( | self, | |
| key, | |||
| limit | |||
| ) |
Returns bool if the specific IR sensor is triggered
Definition at line 375 of file pheeno_robot.py.
| def pheeno_ros.pheeno_robot.PheenoRobot.is_obstacle_detected | ( | self, | |
| limit | |||
| ) |
Returns bool if an obstacle is in the way of any IR sensor
Definition at line 367 of file pheeno_robot.py.
| def pheeno_ros.pheeno_robot.PheenoRobot.linear_velocity | ( | self | ) |
Linear velocity property
Definition at line 196 of file pheeno_robot.py.
| def pheeno_ros.pheeno_robot.PheenoRobot.linear_velocity | ( | self, | |
| value | |||
| ) |
Linear velocity setter property
Definition at line 201 of file pheeno_robot.py.
| def pheeno_ros.pheeno_robot.PheenoRobot.obstacle_velocity | ( | self | ) |
Obstacle velocity property
Definition at line 224 of file pheeno_robot.py.
| def pheeno_ros.pheeno_robot.PheenoRobot.obstacle_velocity | ( | self, | |
| value | |||
| ) |
Obstacle velocity setter property
Definition at line 229 of file pheeno_robot.py.
| def pheeno_ros.pheeno_robot.PheenoRobot.publish_cmd_vel | ( | self, | |
| twist_msg | |||
| ) |
Publish an movement velocity on the `cmd_vel` topic
Definition at line 292 of file pheeno_robot.py.
|
static |
Given a velocity, generate a random turn direction
Definition at line 383 of file pheeno_robot.py.
|
private |
Definition at line 133 of file pheeno_robot.py.
|
private |
Definition at line 108 of file pheeno_robot.py.
|
private |
Definition at line 103 of file pheeno_robot.py.
| pheeno_ros.pheeno_robot.PheenoRobot.pheeno_id |
Definition at line 98 of file pheeno_robot.py.