def start_node():
# Actually launch a node called "closed_loop_path_follower"
rospy.init_node('closed_loop_path_follower', anonymous=False)
# Create a Subscriber to the robot's current estimated position
# with a callback to "path_follow"
rospy.Subscriber('/robot_pose_estimated', Pose2D, path_follow)
# Create a Subscriber to the path the robot is trying to follow
# With a callback to "update_path"
rospy.Subscriber('/path_segment_spec', ME439PathSpecs, update_path)
# Subscriber to the "path_complete" topic
rospy.Subscriber('/path_complete', Bool, set_path_complete)
# Prevent the node from exiting
rospy.spin()
# When done or Errored, Zero the speeds
global pub_speeds
# Set up the message that will go on that topic.
msg_out = ME439WheelSpeeds()
msg_out.v_left = 0
msg_out.v_right = 0
pub_speeds.publish(msg_out)
rospy.loginfo(pub_speeds)
def talker():
# Actually launch a node called "set_desired_wheel_speeds"
rospy.init_node('set_desired_wheel_speeds', anonymous=False)
pub_speeds = rospy.Publisher('/wheel_speeds_desired',
ME439WheelSpeeds,
queue_size=10)
sub_distance = rospy.Subscriber('/sensors_data_raw', ME439SensorsRaw,
stagesettings)
msg_out = ME439WheelSpeeds()
msg_out.v_left = 0
msg_out.v_right = 0
r = rospy.Rate(10) # N Hz
try:
t_start = rospy.get_rostime()
while not rospy.is_shutdown():
msg_out.v_left = vel_left
msg_out.v_right = vel_right
pub_speeds.publish(msg_out)
r.sleep()
except Exception:
traceback.print_exc()
# When done or Errored, Zero the speeds
msg_out.v_left = 0
msg_out.v_right = 0
pub_speeds.publish(msg_out)
rospy.loginfo(pub_speeds)
pass
# When done or Errored, Zero the speeds
msg_out.v_left = 0
msg_out.v_right = 0
pub_speeds.publish(msg_out)
rospy.loginfo(pub_speeds)
def talker():
# Actually launch a node called "set_desired_wheel_speeds"
rospy.init_node('set_desired_wheel_speeds', anonymous=False)
# Create the publisher. Name the topic "sensors_data", with message type "Sensors"
pub_speeds = rospy.Publisher('/wheel_speeds_desired',
ME439WheelSpeeds,
queue_size=10)
# Declare the message that will go on that topic.
# Here we use one of the message name types we Imported, and add parentheses to call it as a function.
# We could also put data in it right away using .
msg_out = ME439WheelSpeeds()
msg_out.v_left = 0
msg_out.v_right = 0
# set up a rate basis to keep it on schedule.
r = rospy.Rate(100) # N Hz
try:
# start a loop
t_start = rospy.get_rostime()
while not rospy.is_shutdown():
future_stages = np.argwhere(
stage_settings_array[:, 0] >= (rospy.get_rostime() -
t_start).to_sec())
if len(future_stages) > 0:
stage = future_stages[0]
print stage
else:
break
msg_out.v_left = stage_settings_array[stage, 1]
msg_out.v_right = stage_settings_array[stage, 2]
# Actually publish the message
pub_speeds.publish(msg_out)
# Log the info (optional)
# rospy.loginfo(pub_speeds)
r.sleep()
# # Here step through the settings.
# for stage in range(0,len(stage_settings_array)): # len gets the length of the array (here the number of rows)
# # Set the desired speeds
# dur = stage_settings_array[stage,0]
# msg_out.v_left = stage_settings_array[stage,1]
# msg_out.v_right = stage_settings_array[stage,2]
# # Actually publish the message
# pub_speeds.publish(msg_out)
# # Log the info (optional)
# rospy.loginfo(pub_speeds)
#
# # Sleep for just long enough to reach the next command.
# sleep_dur = dur - (rospy.get_rostime()-t_start).to_sec()
# sleep_dur = max(sleep_dur, 0.) # In case there was a setting with zero duration, we will get a small negative time. Don't use negative time.
# rospy.sleep(sleep_dur)
except Exception:
traceback.print_exc()
# When done or Errored, Zero the speeds
msg_out.v_left = 0
msg_out.v_right = 0
pub_speeds.publish(msg_out)
rospy.loginfo(pub_speeds)
pass
# When done or Errored, Zero the speeds
msg_out.v_left = 0
msg_out.v_right = 0
pub_speeds.publish(msg_out)
rospy.loginfo(pub_speeds)
# Publish a message to the "/sensors_data_raw" topic.
pub_sensors.publish(pub_sensors_message)
# Publish a message to the "/motor_commands" topic.
pub_motorcommands.publish(pub_motorcommands_message)
# Publish a message to the "/robot_wheel_angles" topic
pub_robot_wheel_angles.publish(robot_wheel_angles_message)
# Publish a message to the "/robot_wheel_displacements" topic
pub_robot_wheel_displacements.publish(
robot_wheel_displacements_message)
# Log the info (optional)
rospy.loginfo(pub_sensors)
except Exception:
traceback.print_exc()
pass
if __name__ == '__main__':
try:
talker()
except rospy.ROSInterruptException:
traceback.print_exc()
# Stop the wheels if this crashes or otherwise ends.
stop_wheel_speeds_message = ME439WheelSpeeds()
stop_wheel_speeds_message.v_left = 0
stop_wheel_speeds_message.v_right = 0
set_wheel_speed_targets(stop_wheel_speeds_message,
[motors.motor1, motors.motor2])
pass
def path_follow(pose_msg_in):
# First assign the incoming message
global estimated_pose, path_segment_spec
estimated_pose = pose_msg_in
pose_xy = np.array([estimated_pose.x, estimated_pose.y])
pose_theta = np.array([estimated_pose.theta])
if np.isinf(path_segment_spec.Length):
return
# Set up global variables
global initialize_psi_world, estimated_pose_previous
global estimated_x_local_previous, estimated_theta_local_previous
global path_segment_curvature_previous, estimated_psi_world_previous
global estimated_segment_completion_fraction_previous
# Create a vector variable for the Origin of the path segment
path_segment_Origin = np.array(
[path_segment_spec.x0, path_segment_spec.y0])
# Forward distance relative to a Line path
# Therefore Define the Forward Axis:
path_segment_y0_vector = np.array(
[-np.sin(path_segment_spec.theta0),
np.cos(path_segment_spec.theta0)])
# local X is computed perpendicular to the segment.
# Therefore define the Perpendicular axis.
path_segment_x0_vector = np.array([
-np.sin(path_segment_spec.theta0 - np.pi / 2.0),
np.cos(path_segment_spec.theta0 - np.pi / 2.0)
])
# Define path curvature
path_segment_curvature = 1.0 / path_segment_spec.Radius
# =============================================================================
# First Compute the robot's position relative to the path (x, y, theta)
# and the local path properties (curvature 1/R and segment percentage)
# =============================================================================
# If it is a Line (Infinite radius)
if np.isinf(path_segment_spec.Radius):
path_segment_Origin + path_segment_spec.Length * path_segment_y0_vector
# compute position relative to Segment:
estimated_xy_rel_to_segment_origin = (
pose_xy - path_segment_Origin
) # XY vector from Origin of segment to current location of robot.
# Projection of vector from path origin to current position
estimated_segment_forward_pos = estimated_xy_rel_to_segment_origin.dot(
path_segment_y0_vector)
# The fraction completion can be estimated as the path length the robot
# has gone through, as a fraction of total path length on this segment
estimated_segment_completion_fraction = estimated_segment_forward_pos / \
path_segment_spec.Length
# Position of the robot to the Right of the segment Origin
estimated_segment_rightward_pos = estimated_xy_rel_to_segment_origin.dot(
path_segment_x0_vector)
# y_local = 0 by definition: Local coords are defined relative to the
# closest point on the path.
estimated_y_local = 0.0
estimated_x_local = estimated_segment_rightward_pos
estimated_theta_local = pose_theta - path_segment_spec.theta0
# Arc
else:
curve_sign = np.sign(path_segment_spec.Radius)
path_segment_circle_center = path_segment_Origin + \
path_segment_spec.Radius * -path_segment_x0_vector
# determine the angular displacement of this arc. SIGNED quantity!
path_segment_angular_displacement = path_segment_spec.Length / path_segment_spec.Radius
estimated_xy_rel_to_circle_center = (pose_xy -
path_segment_circle_center)
# Compute angle of a vector from circle center to Robot, in world frame
# Note how this definition affects the signs of the arguments to
# "arctan2"
estimated_psi_world = np.arctan2(-estimated_xy_rel_to_circle_center[0],
estimated_xy_rel_to_circle_center[1])
# unwrap the angular displacement
if initialize_psi_world:
estimated_psi_world_previous = estimated_psi_world
initialize_psi_world = False
# was negative, is now positive --> should be more negative.
while estimated_psi_world - estimated_psi_world_previous > np.pi:
estimated_psi_world += -2.0 * np.pi
# was positive and is now negative --> should be more positive.
while estimated_psi_world - estimated_psi_world_previous < -np.pi:
estimated_psi_world += 2.0 * np.pi
# update the "previous angle" memory.
estimated_psi_world_previous = estimated_psi_world
# The local path forward direction is perpendicular (clockwise) to this
# World frame origin-to-robot angle.
estimated_path_theta = estimated_psi_world + np.pi / 2.0 * curve_sign
# The fraction completion can be estimated as the path angle the robot
# has gone through, as a fraction of total angular displacement on this
# segment
estimated_segment_completion_fraction = (
estimated_path_theta -
path_segment_spec.theta0) / path_segment_angular_displacement
# x_local is positive Inside the circle for Right turns, and Outside
# the circle for Left turns
estimated_x_local = curve_sign * (
np.sqrt(np.sum(np.square(estimated_xy_rel_to_circle_center))) -
np.abs(path_segment_spec.Radius))
estimated_theta_local = pose_theta - estimated_path_theta
# Whether Line or Arc, update the "local" coordinate state and path
# properties:
estimated_theta_local = m439rbt.fix_angle_pi_to_neg_pi(
estimated_theta_local)
# Update the "previous" values
estimated_pose_previous = estimated_pose
estimated_x_local_previous = estimated_x_local
estimated_theta_local_previous = estimated_theta_local
path_segment_curvature_previous = path_segment_curvature
estimated_segment_completion_fraction_previous = estimated_segment_completion_fraction
# =============================================================================
# ### CONTROLLER for path tracking based on local position and curvature.
# # parameters for the controller are
# # Vmax: Maximum allowable speed,
# # and controller gains:
# # Beta (gain on lateral error, mapping to lateral speed)
# # gamma (gain on heading error, mapping to rotational speed)
# # and a control variable for the precision of turns,
# # angle_focus_factor
# =============================================================================
global Vmax, Beta, gamma, angle_focus_factor
# CODE HERE: Put in formulas for anything that is 0.0, and try the "TRY THIS" variations.
# First set the speed with which we want the robot to approach the path
xdot_local_desired = -Beta * estimated_x_local # Use formula from Lecture
# limit it to +-Vmax
xdot_local_desired = np.min([np.abs(xdot_local_desired),
abs(Vmax)]) * np.sign(xdot_local_desired)
# Next set the desired theta_local
theta_local_desired = -np.arcsin(
xdot_local_desired / Vmax) # Use formula from Lecture
# Next SET SPEED OF ROBOT CENTER.
# G. Cook 2011 says just use constant speed all the time,
# TRY THIS FIRST
Vc = Vmax
# But, that drives farther from the path at first if it is facing away.
# The parameter "angle_focus_factor" can make it even more restrictive
# Value of 1.0 uses a straight cosine of the angle.
# TRY WITH AND WITHOUT THIS
Vc = Vmax * np.cos(angle_focus_factor *
m439rbt.fix_angle_pi_to_neg_pi(theta_local_desired -
estimated_theta_local))
# Could also limit it to only forward.
# TRY WITH AND WITHOUT THIS
Vc = np.max([Vc, 0])
# Finally set the desired angular rate
estimated_theta_local_error = m439rbt.fix_angle_pi_to_neg_pi(
theta_local_desired - estimated_theta_local)
omega = gamma * estimated_theta_local_error + Vc * (
path_segment_curvature /
(1.0 + path_segment_curvature * estimated_x_local)
) * np.cos(estimated_theta_local)
# CODE END
# Finally, use the "robot" object created elsewhere (member of the
# me439_mobile_robot_xx class) to translate omega and Vc into wheel speeds
global robot
robot.set_wheel_speeds_from_robot_velocities(Vc, omega)
# Check if the overall path is complete
# If so, stop!
global path_is_complete
if path_is_complete:
robot.set_wheel_speeds(0., 0.)
# Now Publish the desired wheel speeds
global pub_speeds
# Set up the message that will go on that topic.
msg_speeds = ME439WheelSpeeds()
msg_speeds.v_left = robot.left_wheel_speed
msg_speeds.v_right = robot.right_wheel_speed
pub_speeds.publish(msg_speeds)
print(estimated_segment_completion_fraction)
# Finally, if the segment is complete, publish that fact
if estimated_segment_completion_fraction >= 1.0:
global pub_segment_complete
msg_seg_complete = Bool()
msg_seg_complete.data = True
pub_segment_complete.publish(msg_seg_complete)
# and clear the memory of angle wrapping:
initialize_psi_world = True