Skip to content
Snippets Groups Projects
Commit 6db80280 authored by Davis, Duane T's avatar Davis, Duane T
Browse files

PROJECT: Removed project_robot.py (it's just a copy of another version)

parent c167ec8a
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
minesearch_project/src/minesearch_project/project_robot.py deleted 100644 → 0
+ 0
− 268
View file @ c167ec8a
#!/usr/bin/env python
# Standard python imports
import math
# ROS imports
import rospy
import tf
import geometry_msgs.msg as geometry_msgs
import nav_msgs.msg as nav_msgs
import sensor_msgs.msg as sensor_msgs
import geometry_msgs.msg as geometry_msgs
# CS4313 library imports
import cs4313_utilities.robot as robot
import cs4313_utilities.robot_math as rm
import cs4313_utilities.shared_robot_classes as src
import cs4313_utilities.quaternion_math as quat
# Constants for use throughout the class
WALL_FOLLOW_TRANSITION_STEPS = 20
class SensingRobot(robot.SimpleRobot):
''' Class extending the SimpleRobot class to provide additional capability.
and control modes (potential field and bug2) based on laser scan data.
Member variables:
navMode: enumeration for which navigation mode to use
mLine: directed line used for line following
sideOfMLine: enumeration for what side of the m-line the vehicle is on
wallFollowCtr: delay counter before testing m-line distance
turnAccumulator: accumulated turn during wall following
thetaLast: used to integrate turn accumulator
Member variables (inherited from SimpleRobot):
state: current state containing pose, velocity, and odometry values
laserData: current laser data
controlMode: enumeration for which control mode to use
navMode: enumeration for which navigation mode to use
controllers: dictionary of control functions
waypoint: Cartesian coordinates of the current waypoint
waypointDistance: distance from the current waypoint
mLine: directed line used for line following
cmd_vel: ROS message object for velocity commands
cmdPub: ROS publisher object for velocity commands
isAtWaypoint: Boolean indicator for reaching a waypoint
sideOfMLine: enumeration for what side of the m-line the vehicle is on
wallFollowCtr: delay counter before testing m-line distance
turnAccumulator: accumulated turn during wall following
thetaLast: used to integrate turn accumulator
baseToOdomTF: ROS broadcaster to publish odometry transforms
mapToOdomTF: ROS listener for map transforms
timer: ROS timer for control loop iteration
Class methods (inherited from SimpleRobot):
setControlConstants: override of parent class method for additional constants
pfWaypointControl: uses a potential field to compute a command to a waypoint
bug2WaypointControl: uses the Bug 2 algorithm to compute a command to a waypoint
wallFollowingControl: computes a command to follow (parallel) a detected wall
lineTrackingControl: computes a command to follow a line
Class methods (inherited from SimpleRobot):
spinOnce: runs a single iteration of the vehicle's control loop
setWaypoint: sets the current waypoint to a new location
waypointCaptured: Boolean test for reaching the waypoint
processLaser: ROS callback for updating laser scan data
processGroundTruth: ROS callback for updating state based on ground truth data
processOdometry: ROS callback for updating state based on odometry data
clampedMaxXDotCommand: computes a max fwd speed command to account for heading error
setVelocityCommands: sets the velocity command based on heading and distance error
pdThetaCommand: computes a proportional-derivative turn rate command
pdXDotCommand: computes a proportional-derivative forward speed command
pdWaypointControl: computes a proportional-derivative command directly to a waypoint
'''
def __init__(self, odomTopic="odom", laserTopic="scan", \
cmdTopic="cmd_vel", tfTopic="tf"):
''' Initializes the object member variables, registers the object
subscriptions and publishers with ROS
@param odomTopic: topic name to subscribe to for odometry data
@param laserTopic: topic name to subscribe to tfor laser scan data
@param cmdTopic: topic name to publish velocity commands to
@param tfTopic: topic to which the robot will publish its odom to map tf
'''
robot.SimpleRobot.__init__(self, odomTopic, laserTopic, cmdTopic, tfTopic)
# Vehicle controllers
self.controlMode = src.BUG2_CONTROL # current control mode
self.navMode = src.LINE_FOLLOWING # Current navigation mode
self.controllers[src.PF_CONTROL] = self.pfWaypointControl
self.controllers[src.BUG2_CONTROL] = self.bug2WaypointControl
# Command info (current commands)
self.mLine = src.DirectedLine((0.0, 0.0), (0.0, 0.0))
self.sideOfMLine = 0 # Which side of the m-line for Bug2
self.wallFollowCtr = 0 # Delay before testing m-line distance
self.turnAccumulator = 0.0 # Accumulated turn during wall following
self.thetaLast = 0.0 # Used to integrate turn accumulator
def setControlConstants(self, newCaptureDist=1.5, newWallFollowDist=1.5, \
newInterceptDist=2.0, newMaxXDot=1.0, newMaxThetaDot=0.77, \
newXCmdC1=2.0, newXCmdC2=1.0, newPDThetaC1=3.0, \
newPDThetaC2=1.0, newPFZeta=7.0, newPFEta=0.2):
'''Overrides parent class method to include additional control constants
for potential field and bug algorithm implementations
@param newCaptureDist: updated distance to consider a waypoint "achieved"
@param newWallFollowDist: updated wall following distance
@param newInterceptDist: distance along the line to intercept when tracking
@param newMaxXDot: updated max forward commanded velocity
@param newMaxThetaDot: updated max commanded angular (radian) velocity
@param newXDotCmdC: updated coefficient for forward speed command
@param newPDThetaC1: updated turn speed PD control error multiple
@param newPDThetaC2: updated turn speed PD control rate (damping) multiple
@param newPFZeta: updated potential field zeta (attractive) coefficient
@param newPFEta: updated potential field eta (repulsive) coefficient
'''
robot.SimpleRobot.setControlConstants(self, newCaptureDist, newMaxXDot, \
newMaxThetaDot, newXCmdC1, newXCmdC2, \
newPDThetaC1, newPDThetaC2)
self.BUG_FOLLOW = src.LEFT # side of vehicle to wall-follow in bug2
self.WALL_FOLLOW_DISTANCE = abs(newWallFollowDist)
self.INTERCEPT_DISTANCE = abs(newInterceptDist)
self.PF_ZETA = abs(newPFZeta)
self.PF_ETA = abs(newPFEta)
def lineTrackingControl(self, line):
''' Computes waypoint orders that will track along a line (the m-line)
towards the waypoint. If the vehicle is beyond the waypoint or closer
than the intercept distance, it will drive straight there
@param line: directed line that the vehicle is tracking
'''
if self.waypointDistance < self.INTERCEPT_DISTANCE:
self.pdWaypointControl()
else:
thetaErr = rm.normalize_pi(line.interceptHeading(self.state.pose, \
self.INTERCEPT_DISTANCE) - \
self.state.pose[2])
self.setVelocityCommands(thetaErr, self.waypointDistance)
def wallFollowingControl(self, direction):
''' Computes control orders that will drive the vehicle along a wall
detected by the laser. Following will be on the side of the vehicle
specified by the argument
@param direction: side of the vehicle (left or right) to keep the wall
'''
followed = []
distance = self.laserData.maxRange
# Update the turn accumulator (reset if we just started)
if self.wallFollowCtr <= 10:
self.thetaLast = self.state.pose[2]
self.turnAccumulator = 0.0
else:
self.turnAccumulator += \
rm.normalize_pi(self.state.pose[2] - self.thetaLast)
self.thetaLast = self.state.pose[2]
# Account for losing track of the wall (i.e., no intervals)
if len(self.laserData.intervals) == 0:
thetaErr = -direction * math.pi/2.0
self.setVelocityCommands(thetaErr, self.waypointDistance)
return # probably a corner--just turn towards the wall
for interval in self.laserData.intervals:
if self.laserData.ranges[interval[2]] < distance:
followed = interval
distance = self.laserData.ranges[interval[2]]
# These will be used later
trackSegment = 0
right = self.laserData.rayReturn(followed[0])
left = self.laserData.rayReturn(followed[1])
close = self.laserData.rayReturn(followed[2])
if ((interval[0] == interval[2]) or (interval[1] == interval[2])):
if self.BUG_FOLLOW == src.LEFT:
trackSegment = src.DirectedLine(left, right)
else:
trackSegment = src.DirectedLine(right, left)
else:
if self.BUG_FOLLOW == src.LEFT:
trackSegment = src.DirectedLine(close, right)
else:
trackSegment = src.DirectedLine(close, left)
trackSegment.rho -= self.WALL_FOLLOW_DISTANCE
thetaErr = rm.normalize_pi(trackSegment.interceptHeading((0.0, 0.0), self.INTERCEPT_DISTANCE))
self.setVelocityCommands(thetaErr, self.waypointDistance)
def bug2WaypointControl(self):
''' Computes waypoint control orders that will drive the robot
towards a commanded waypoint using the Bug2 algorithm
'''
if not self.waypointCaptured():
if (self.navMode == src.LINE_FOLLOWING and \
((self.laserData.obstacleToFront() < (self.WALL_FOLLOW_DISTANCE + 0.5)) or \
((self.laserData.closestRange < self.WALL_FOLLOW_DISTANCE - 0.25) and \
(rm.signum(self.laserData.closestBearing) == \
rm.signum(rm.normalize_pi(self.mLine.theta - self.state.pose[2])))))):
self.navMode = src.WALL_FOLLOWING
self.wallFollowCtr = 0
self.turnAccumulator = 0.0
sideOfMLine = rm.signum(self.mLine.distanceFromLine(self.state.pose))
if self.navMode == src.WALL_FOLLOWING:
# If we went in a circle, conclude that we can't get there and move on
if abs(self.turnAccumulator) > (2.0 * math.pi):
self.cmd_vel.linear.x = 0.0
self.cmd_vel.angular.z = 0.0
self.isAtWaypoint = True
return
d = rm.cartesian_distance(self.state.pose, self.waypoint)
if self.wallFollowCtr > WALL_FOLLOW_TRANSITION_STEPS:
if ((sideOfMLine != self.sideOfMLine) and \
(d < self.mLine.distance)):
self.navMode = src.LINE_FOLLOWING
self.mLine.updateStart(self.state.pose)
elif self.wallFollowCtr == WALL_FOLLOW_TRANSITION_STEPS:
self.mLine.updateStart(self.state.pose)
if self.navMode == src.LINE_FOLLOWING:
self.lineTrackingControl(self.mLine)
else:
self.wallFollowingControl(self.BUG_FOLLOW)
self.sideOfMLine = sideOfMLine
self.wallFollowCtr += 1
def pfWaypointControl(self):
''' Computes waypoint control orders that will drive the robot
towards a commanded waypoint using potential field navigation
'''
if not self.waypointCaptured():
# Compute the attractive force to the goal (linear increase with d)
hdgTo = rm.normalize_pi(rm.bearing_to(self.state.pose, \
self.waypoint) - \
self.state.pose[2])
f = self.PF_ZETA * self.waypointDistance
fx = math.cos(hdgTo) * f
fy = math.sin(hdgTo) * f
# Compute the repulsive force for each laser return
for rayNum in range(0, len(self.laserData.ranges)):
if self.laserData.ranges[rayNum] > self.laserData.minRange and \
self.laserData.ranges[rayNum] < (self.laserData.maxRange - 0.05):
angle = self.laserData.minAngle + \
self.laserData.increment * rayNum
f = self.PF_ETA * (1.0 / self.laserData.maxRange - \
1.0 / self.laserData.ranges[rayNum]) * \
1.0 / (self.laserData.ranges[rayNum] * \
self.laserData.ranges[rayNum])
fx += math.cos(angle) * f
fy += math.sin(angle) * f
# Compute actual commands based on computed force vectors
thetaErr = rm.normalize_pi(math.atan2(fy, fx))
self.setVelocityCommands(thetaErr, self.waypointDistance)
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment