def __init__(self):
# Housekeeping - Initialization, logging info and shutdown callback
rospy.init_node("Controller")
rospy.loginfo("Starting up the simplebot controller!")
rospy.on_shutdown(self.shutdown)
# Get parameters from the parameter server
self.get_parameters()
# Fire up subscribers for navigation data
self.scan = LaserScan()
rospy.Subscriber("/base_scan", LaserScan, self.get_scan)
self.odom = Odometry()
rospy.Subscriber("/odom", Odometry, self.get_odom)
self.tran = tf.TransformListener()
# Sleep for a wee bit to ensure that we've received telemetry.
rospy.sleep(1)
# Setting controller publishing parameters and operating rate
self.vel = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
update_rate = rospy.Rate(10)
# Set up motion data in for publishing
self.motion = Twist()
# Set up PID controller(s)
rospy.loginfo("Setting up PID(s)")
turn_min_output = -self.turn
turn_max_output = self.turn
turn_kp = 2.5
turn_ki = 0.0
turn_kd = 0.0
turn_delta_time_ms = (1.0/10.0)*1000.0
turn_direct = False
self.turn_PID = ROSPID(0.0, turn_min_output, turn_max_output,
turn_kp, turn_ki, turn_kd, turn_delta_time_ms,
turn_direct)
rospy.loginfo("Done with setting up PID(s)")
# Set initial state for system and the initial distance to goal
self.blocked = False
self.can_leave = False
self.min_dist_goal = self.dist_goal()
# Variables to keep track of us being able to actually reach the goal
# after a set number of iterations to make sure that the distance has
# changed at least a bit since the start position
self.goal_unreachable = False
self.bug_boundary_count = 0
while not rospy.is_shutdown():
if self.goal_unreachable is False:
if self.blocked is False:
if self.at_goal():
self.full_stop()
else:
if self.is_blocked():
self.blocked = True
self.can_leave = False
self.bug_start = [self.x_r, self.y_r]
print(self.bug_start)
self.bug_boundary_count = 0
if self.dist_goal() < self.min_dist_goal:
self.min_dist_goal = self.dist_goal()
self.be_bug()
else:
self.move_to_goal()
else:
self.be_bug()
else:
# Do nothing. Robot being unable to reach goal has already been
# logged when we entered this state.
pass
self.vel.publish(self.motion)
update_rate.sleep()
class Controller():
def __init__(self):
# Housekeeping - Initialization, logging info and shutdown callback
rospy.init_node("Controller")
rospy.loginfo("Starting up the simplebot controller!")
rospy.on_shutdown(self.shutdown)
# Get parameters from the parameter server
self.get_parameters()
# Fire up subscribers for navigation data
self.scan = LaserScan()
rospy.Subscriber("/base_scan", LaserScan, self.get_scan)
self.odom = Odometry()
rospy.Subscriber("/odom", Odometry, self.get_odom)
self.tran = tf.TransformListener()
# Sleep for a wee bit to ensure that we've received telemetry.
rospy.sleep(1)
# Setting controller publishing parameters and operating rate
self.vel = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
update_rate = rospy.Rate(10)
# Set up motion data in for publishing
self.motion = Twist()
# Set up PID controller(s)
rospy.loginfo("Setting up PID(s)")
turn_min_output = -self.turn
turn_max_output = self.turn
turn_kp = 2.5
turn_ki = 0.0
turn_kd = 0.0
turn_delta_time_ms = (1.0/10.0)*1000.0
turn_direct = False
self.turn_PID = ROSPID(0.0, turn_min_output, turn_max_output,
turn_kp, turn_ki, turn_kd, turn_delta_time_ms,
turn_direct)
rospy.loginfo("Done with setting up PID(s)")
# Set initial state for system and the initial distance to goal
self.blocked = False
self.can_leave = False
self.min_dist_goal = self.dist_goal()
# Variables to keep track of us being able to actually reach the goal
# after a set number of iterations to make sure that the distance has
# changed at least a bit since the start position
self.goal_unreachable = False
self.bug_boundary_count = 0
while not rospy.is_shutdown():
if self.goal_unreachable is False:
if self.blocked is False:
if self.at_goal():
self.full_stop()
else:
if self.is_blocked():
self.blocked = True
self.can_leave = False
self.bug_start = [self.x_r, self.y_r]
print(self.bug_start)
self.bug_boundary_count = 0
if self.dist_goal() < self.min_dist_goal:
self.min_dist_goal = self.dist_goal()
self.be_bug()
else:
self.move_to_goal()
else:
self.be_bug()
else:
# Do nothing. Robot being unable to reach goal has already been
# logged when we entered this state.
pass
self.vel.publish(self.motion)
update_rate.sleep()
def be_bug(self):
# In some cases there are no valid ranges in scan. Check if this occurs
# If it does set self.theta_goal as the travel angle..?
no_scan = min(self.scan.ranges) == self.scan.range_max
if no_scan:
theta = self.theta_goal
# Assume the worst so we want to slow down in this case
wall_dist = self.dist
else:
(theta, wall_dist) = self.find_tanget()
turn_speed = self.turn_PID.get_output_value(theta)
self.set_turn_speed(turn_speed)
print("turn_speed: " + str(turn_speed))
# If we get dangerously close to the wall reduce the speed
if wall_dist < self.dist/4.0:
self.set_x_speed(0.3)
elif wall_dist < self.dist:
self.set_x_speed(0.6)
else:
self.set_x_speed(self.speed)
# Check that we have been traveling for at least a minimum amount of
# "time". If we have and arrived relatively close to the starting point
# of the boundary we're following, give up and stop doing things.
self.bug_boundary_count += 1
if (self.bug_boundary_count > 100):
d_x = self.bug_start[0] - self.x_r
d_y = self.bug_start[1] - self.y_r
dist_start = math.sqrt(d_x**2.0 + d_y**2.0)
if (dist_start < 0.5):
self.full_stop()
self.goal_unreachable = True
# We're allowed to leave if we reach a point which is closer to the
# goal than previously and currently not blocked in the direction of
# travel.
if ((self.dist_goal() < self.min_dist_goal) and
(not self.is_blocked())):
self.full_stop()
self.min_dist_goal = self.dist_goal()
self.can_leave = True
self.blocked = False
def find_tanget(self):
# DO A LOT OF STUFF
# Find the point of discontinuity that leaves the lowest total
# distance to goal.
(best_index, best_angle, best_dist) = self.disc_point()
print("best_index (disc_point):" + str(best_index))
print("best_angle (disc_point):" + str(best_angle*180.0/math.pi))
print("best_dist (disc_point):" + str(best_dist))
(second_angle, second_dist) = self.second_point(best_index, best_angle)
print("second_angle:" + str(second_angle*180.0/math.pi))
print("second_dist:" + str(second_dist))
# Now we need to find the vectors representing the points 1 and 2
# 1 will be denoting the best and 2 will be denoting the second point
# PS: This assumption is likely to be the source of "odd" behavior at
# certain points.
vec_1 = [best_dist*math.cos(best_angle),
best_dist*math.sin(best_angle)]
vec_2 = [second_dist*math.cos(second_angle),
second_dist*math.sin(second_angle)]
# Now we want to find the vector representing the wall.
# This is given vec_wall = vec_2 - vec_1
vec_wall = [vec_2[0] - vec_1[0], vec_2[1] - vec_1[1]]
# Now we need the vector perpendicular to the wall
vec_perp = [vec_2[1] - vec_1[1], -(vec_2[0] - vec_1[0])]
# Now the distance can be found...
vec_perp_factor = np.linalg.norm(vec_perp)
vec_perp_norm = [vec_perp[0]/vec_perp_factor,
vec_perp[1]/vec_perp_factor]
wall_dist = abs(vec_perp_norm[0]*vec_1[0] + vec_perp_norm[1]*vec_1[1])
# Now we need to find the vector we want to travel towards
# First find normalized wall vector!
vec_wall_factor = np.linalg.norm(vec_wall)
vec_wall_norm = [vec_wall[0]/vec_wall_factor,
vec_wall[1]/vec_wall_factor]
# Then calculate the ideal vector to follow
desired_x = self.dist*vec_wall_norm[0] + \
(wall_dist - self.dist)*vec_perp_norm[0]
desired_y = self.dist*vec_wall_norm[1] + \
(wall_dist - self.dist)*vec_perp_norm[1]
# Finally calculate the angle we want to follow
desired_theta = math.atan(desired_y / desired_x)
print("desired_theta: " + str(desired_theta*180.0/math.pi))
return desired_theta, wall_dist
def second_point(self, best_index, best_angle):
# Here we want to find the point closest of two points on each side of
# the best point. This point will then be used to calculate the slope
# of the wall in be_bug()
index_min_1 = best_index - 1
angle_min_1 = best_angle - self.scan_delta
index_plus_1 = best_index + 1
angle_plus_1 = best_angle + self.scan_delta
# Want to make sure we do not go out of range for the indexes
min_index = 0
if index_min_1 >= min_index:
dist_min_1 = self.scan.ranges[index_min_1]
else:
# Assign it a large number so that it will not be chosen as the
# second point
dist_min_1 = 9000
max_index = len(self.scan.ranges) - 1
if index_plus_1 <= max_index:
dist_plus_1 = self.scan.ranges[index_plus_1]
else:
# Assign it a large number so that it will not be chosen as the
# second point
dist_plus_1 = 9000
if dist_min_1 < dist_plus_1:
return angle_min_1, dist_min_1
else:
return angle_plus_1, dist_plus_1
def disc_point(self):
# Find the "best" point of discontinuity available
# Find all disc_points outside scan range where point-1 or point+1
# have a finite distance.
# Then check the total goal distance for each point of discontinuity
# and pick the one with the lowest distance as the chosen point.
index = 1
angle = self.scan_min + self.scan_delta
best_index = -1
best_angle = -1.0
best_goal_dist = 99999
best_dist = 99999
while index < len(self.scan.ranges) - 1:
# Make sure that we do not exceed the list range
if index == 0:
check_min_1 = False
else:
check_min_1 = True
if index == len(self.scan.ranges) - 1:
check_plus_1 = False
else:
check_plus_1 = True
disc_index_min_1 = False
disc_index_plus_1 = False
# Check if the point is a disc point
dist = self.scan.ranges[index]
# If distance is equal to the maximum range we have a point that
# may be a discontinuous point
if dist >= self.scan.range_max:
# Now check if this is indeed a discontinuous point
# This happens if the range for point-1 or point+1 is not max
disc_index_min_1 = False
disc_index_plus_1 = False
if check_min_1:
if self.scan.ranges[index - 1] < self.scan.range_max:
disc_index_min_1 = True
point_index = index - 1
point_angle = angle - self.scan_delta
point_dist = self.scan.ranges[index - 1]
if check_plus_1:
if self.scan.ranges[index + 1] < self.scan.range_max:
disc_index_plus_1 = True
point_index = index + 1
point_angle = angle + self.scan_delta
point_dist = self.scan.ranges[index + 1]
# If we found a disc point we want to check if the distance to
# goal is lower than previously found distances
if disc_index_min_1 or disc_index_plus_1:
# Calculate the total distance from point to point goal
x = dist * math.cos(angle)
y = dist * math.sin(angle)
dist_goal = math.sqrt((x - self.robot_goal.point.x)**2.0 +
(y - self.robot_goal.point.y)**2.0)
dist_total = dist + dist_goal
# Check if this is a better point than previously found
if dist_total < best_goal_dist:
best_index = point_index
best_angle = point_angle
best_dist = point_dist
best_goal_dist = dist_total
# Increment index and angle for next loop
index += 1
angle += self.scan_delta
return best_index, best_angle, best_dist
def best_point(self):
# Loop through all scan points and find the point that minimize the
# distance traveled towards the goal.
# This distance would be the distance to the point plus the distance
# from the point to the goal.
# For ease of calculations all is done in the robots frame of reference
best_index = 0
best_angle = 0.0
best_dist = 90000
best_total_dist = 90000
angle = self.scan_min
index = 0
while index < len(self.scan.ranges):
dist_p = self.scan.ranges[index]
if dist_p < self.scan.range_max:
x_p = dist_p * math.cos(angle)
y_p = dist_p * math.sin(angle)
dist_p_goal = math.sqrt((x_p-self.robot_goal.point.x)**2.0 +
(y_p-self.robot_goal.point.y)**2.0)
# print("dist_p_goal: " + str(dist_p_goal))
dist_total = dist_p + dist_p_goal
if dist_total < best_total_dist:
best_index = index
best_angle = angle
best_dist = dist_p
best_total_dist = dist_total
angle += self.scan_delta
index += 1
return best_index, best_angle, best_dist
def move_to_goal(self):
if self.goal_in_scan_range():
dist_goal = self.dist_goal()
if dist_goal < 1.0:
self.set_x_speed(0.1)
elif dist_goal < 2.0:
self.set_x_speed(0.3)
else:
self.set_x_speed(self.speed)
else:
self.set_x_speed(0.0)
self.set_turn_speed(self.turn_PID.get_output_value(self.theta_goal))
def goal_in_scan_range(self):
if (self.theta_goal < self.scan_max) and \
(self.theta_goal > self.scan_min):
return True
else:
return False
def is_blocked(self):
"""
Get all laser scan values that are inside the max range and convert
them to points. If the difference in y is less than the minimum
distance we will "collide" or at least be very close to collision and
should return True
Note we have to consider the delta_y sideways when the total distance
is less than the minimum distance to a target. I think.
This works. So I'll run with it.
"""
angle = self.scan_min
index = 0
cutoff_distance = min(2.0*self.dist, self.scan.range_max/2.0)
while angle < self.scan_max:
dist = self.scan.ranges[index]
if dist < cutoff_distance:
y = dist * math.sin(angle)
if abs(y) < self.dist:
return True
angle += self.scan_delta
index += 1
return False
def set_x_speed(self, speed):
self.motion.linear.x = speed
def set_turn_speed(self, speed):
self.motion.angular.z = speed
def current_theta_r(self):
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion(
[self.odom.pose.pose.orientation.x,
self.odom.pose.pose.orientation.y,
self.odom.pose.pose.orientation.z,
self.odom.pose.pose.orientation.w])
return yaw
def get_parameters(self):
self.dist = rospy.get_param("/simplebot/dist", 1.2)
self.speed = rospy.get_param("/simplebot/speed", 2.0)
self.turn = rospy.get_param("/simplebot/turn", 5.0)
self.x_target = rospy.get_param("x_target", 11.0)
self.y_target = rospy.get_param("y_target", -27.0)
def get_odom(self, odom):
self.odom = odom
self.x_r = self.odom.pose.pose.position.x
self.y_r = self.odom.pose.pose.position.y
self.goal = PointStamped()
self.goal.header.frame_id = "/odom"
self.goal.point.x = self.x_target
self.goal.point.y = self.y_target
self.goal.point.z = 0.0
try:
recent_time = self.tran.getLatestCommonTime("/odom", "/base_link")
self.goal.header.stamp = recent_time
self.robot_goal = self.tran.transformPoint("/base_link", self.goal)
self.theta_goal = math.atan2(self.robot_goal.point.y,
self.robot_goal.point.x)
except:
pass
def get_scan(self, scan):
self.scan = scan
self.scan_min = self.scan.angle_min
self.scan_max = self.scan.angle_max
self.scan_delta = self.scan.angle_increment
def dist_goal(self):
dist = math.sqrt(self.robot_goal.point.x**2.0 +
self.robot_goal.point.y**2.0)
return dist
def at_goal(self):
if self.dist_goal() < 0.1:
return True
else:
return False
def full_stop(self):
self.set_x_speed(0.0)
self.set_turn_speed(0.0)
def shutdown(self):
# Stop motion via default initialized Twist() and sleep before exiting
self.vel.publish(Twist())
rospy.sleep(1)