def update(self, state, a, delta):
#this is looping until there is nothing infront of us
r = rospy.Rate(1)
pose = self.odom.pose.pose
twist = self.odom.twist.twist
current_spd = math.sqrt(twist.linear.x**2 + twist.linear.y**2)
msg = VelAngle()
msg.vel = a
msg.angle = (delta * 180) / math.pi
msg.vel_curr = current_spd
self.motion_pub.publish(msg)
state.x = pose.position.x
state.y = pose.position.y
quat = (pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w)
angles = tf.euler_from_quaternion(quat)
state.yaw = angles[2]
state.v = math.sqrt(twist.linear.x**2 + twist.linear.y**2)
'''dt = 0.1
L = 2.9
state.x = state.x + state.v * math.cos(state.yaw) * dt
state.y = state.y + state.v * math.sin(state.yaw) * dt
state.yaw = state.yaw + state.v / L * math.tan(delta) * dt
state.v = state.v + a * dt'''
return state
def __init__(self):
global hertz
global tolerance
self.odom = None
self.tolerance = tolerance #allowed imprecision for reaching a waypoint
self.current_goal = None #current goal from list of goals
self.vel_curr = 0.0 #current velocity
self.angle_curr = 0.0 #current wheel angle
self.kill = False
self.vel_angle = VelAngle()
#util classes
self.waypoints = waypoint_handler.WaypointHandler(self.tolerance)
self.angle_pid_controller = pid.PID(1,0,0,0,0)
self.vel_pid_controller = pid.PID(1,0,0,0,0)
#publishers
self.vel_angle_p = rospy.Publisher('/vel_angle', VelAngle, queue_size=10)
self.xyz_waypoint_pub = rospy.Publisher('/xyz_waypoints', PointStamped, queue_size=10, latch = True)
#subscribers
self.odom_s = rospy.Subscriber('/pose_and_speed', Odometry, self.odom_callback, queue_size=10)
self.point_cloud_s = rospy.Subscriber('/2d_point_cloud', PointCloud2, self.point_cloud_callback, queue_size=10)
self.killswitch_s = rospy.Subscriber('/stop', EmergencyStop, self.killswitch_callback, queue_size=10)
self.waypoints_s = rospy.Subscriber('/waypoints', WaypointsArray, self.waypoints_callback, queue_size=10)
rospy.init_node('the_overmind', anonymous=False)
self.control()
def __init__(self):
rospy.init_node('teleop')
self.msg = VelAngle()
self.motion_pub = rospy.Publisher('/nav_cmd', VelAngle, queue_size=10)
#self.vel_sub = rospy.Subscriber('/pose_and_speed', Odometry, self.vel_callback, queue_size = 10)
self.prev_key = 1
self.cur_vel = 0.0
curses.wrapper(self.get_input)
def update(self, state, a, delta):
pose = self.global_pose
twist = self.global_twist
current_spd = twist.linear.x
msg = VelAngle()
if self.debug:
self.delay_print -= 1
if self.delay_print <= 0:
self.delay_print = 50
rospy.loginfo("Target Speed: " + str(a))
rospy.loginfo("Current Speed: " + str(current_spd))
msg.vel = a #Speed we want from pure pursuit controller
msg.angle = (delta * 180) / math.pi
msg.vel_curr = current_spd
# Check if any node wants us to stop
stop_msg = Bool()
if any(x == True for x in self.stop_requests.values()):
stop_msg.data = True
else:
stop_msg.data = False
self.motion_pub.publish(msg)
self.stop_pub.publish(stop_msg)
state.x = pose.position.x
state.y = pose.position.y
quat = (pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w)
angles = tf.euler_from_quaternion(quat)
state.yaw = angles[2]
state.v = twist.linear.x
return state
def waypoints_callback(self, msg):
"""
Waypoints callback does way too much right now. All of the path stuff should be handled in a helper method
"""
google_points = []
#Reads each point in the waypoint topic into google_points
for gps_point in msg.waypoints:
point = gps_util.get_point(gps_point)
google_points.append(point)
print len(google_points)
#Adds more points between the google points
google_points_plus = gps_util.add_intermediate_points(
google_points, 15.0)
print len(google_points_plus)
ax = []
ay = []
extra_points = Path()
extra_points.header = Header()
extra_points.header.frame_id = '/map'
#Puts the x's and y's
for p in google_points_plus:
extra_points.poses.append(self.create_poseStamped(p))
ax.append(p.x)
ay.append(p.y)
self.points_pub.publish(extra_points)
#calculate the spline
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax,
ay,
ds=0.1)
path = Path()
path.header = Header()
path.header.frame_id = '/map'
for i in range(0, len(cx)):
curve_point = Point()
curve_point.x = cx[i]
curve_point.y = cy[i]
path.poses.append(self.create_poseStamped(curve_point))
self.path_pub.publish(path)
#================================================ pure persuit copy/pase ===============================================
k = 0.1 # look forward gain
Lfc = 3.5 # look-ahead distance
Kp = 1.0 # speed propotional gain
dt = 0.1 # [s]
L = 2.9 # [m] wheel base of vehicle
target_speed = 10.0 / 3.6 # [m/s]
T = 100.0 # max simulation time
# initial state
pose = self.odom.pose.pose
twist = self.odom.twist.twist
quat = (pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w)
angles = tf.euler_from_quaternion(quat)
initial_v = math.sqrt(twist.linear.x**2 + twist.linear.y**2)
state = State(x=pose.position.x,
y=pose.position.y,
yaw=angles[2],
v=initial_v) #TODO this has to be where we start
lastIndex = len(cx) - 1
time = 0.0
x = [state.x]
y = [state.y]
yaw = [state.yaw]
v = [state.v]
t = [0.0]
target_ind = pure_pursuit.calc_target_index(state, cx, cy)
while lastIndex > target_ind:
ai = pure_pursuit.PIDControl(target_speed, state.v)
di, target_ind = pure_pursuit.pure_pursuit_control(
state, cx, cy, target_ind)
#publish where we want to be
mkr = self.create_marker(cx[target_ind], cy[target_ind], '/map')
self.target_pub.publish(mkr)
#publish an arrow with our twist
arrow = self.create_marker(0, 0, '/base_link')
arrow.type = 0 #arrow
arrow.scale.x = 2.0
arrow.scale.y = 1.0
arrow.scale.z = 1.0
arrow.color.r = 1.0
arrow.color.g = 0.0
arrow.color.b = 0.0
#TODO di might be in radians so that might be causing the error
quater = tf.quaternion_from_euler(0, 0, di)
arrow.pose.orientation.x = quater[0]
arrow.pose.orientation.y = quater[1]
arrow.pose.orientation.z = quater[2]
arrow.pose.orientation.w = quater[3]
self.target_twist_pub.publish(arrow)
#go back to pure persuit
state = self.update(state, ai, di)
#time = time + dt
x.append(state.x)
y.append(state.y)
yaw.append(state.yaw)
v.append(state.v)
t.append(time)
# Test
assert lastIndex >= target_ind, "Cannot goal"
rospy.logerr("Done navigating")
msg = VelAngle()
msg.vel = 0
msg.angle = 0
msg.vel_curr = 0
self.motion_pub.publish(msg)
def create_path(self):
# Increase the "resolution" of the path with 15 intermediate points
local_points_plus = self.local_points # geometry_util.add_intermediate_points(self.local_points, 15.0)
ax = []
ay = []
# Create a Path object for displaying the raw path (no spline) in RViz
display_points = Path()
display_points.header = Header()
display_points.header.frame_id = '/map'
# Set the beginning of the navigation the first point
last_index = 0
target_ind = 0
# Creates a list of the x's and y's to be used when calculating the spline
for p in local_points_plus:
display_points.poses.append(create_pose_stamped(p))
ax.append(p.x)
ay.append(p.y)
self.points_pub.publish(display_points)
# If the path doesn't have any successive points to navigate through, don't try
if len(ax) > 2:
# Create a cubic spline from the raw path
cx, cy, cyaw, ck, cs = cubic_spline_planner.calc_spline_course(
ax, ay, ds=0.1)
# Create Path object which displays the cubic spline in RViz
path = Path()
path.header = Header()
path.header.frame_id = '/map'
# Add cubic spline points to path
for i in range(0, len(cx)):
curve_point = Point()
curve_point.x = cx[i]
curve_point.y = cy[i]
path.poses.append(create_pose_stamped(curve_point))
self.path_pub.publish(path)
# Set the current state of the cart to navigating
self.current_state = VehicleState()
self.current_state.is_navigating = True
self.vehicle_state_pub.publish(self.current_state)
target_speed = self.global_speed
# initial state
pose = self.global_pose
twist = self.global_twist
quat = (pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w)
angles = tf.euler_from_quaternion(quat)
initial_v = twist.linear.x
#TODO state has to be where we start
state = State(x=pose.position.x,
y=pose.position.y,
yaw=angles[2],
v=initial_v)
# last_index represents the last point in the cubic spline, the destination
last_index = len(cx) - 1
time = 0.0
x = [state.x]
y = [state.y]
yaw = [state.yaw]
v = [state.v]
t = [0.0]
target_ind = pure_pursuit.calc_target_index(state, cx, cy, 0)
# Publish the ETA to the destination before we get started
self.calc_eta(None)
# Continue to loop while we have not hit the target destination, and the path is still valid
while last_index > target_ind and self.path_valid and not rospy.is_shutdown(
):
target_speed = self.global_speed
ai = target_speed #pure_pursuit.PIDControl(target_speed, state.v)
di, target_ind = pure_pursuit.pure_pursuit_control(
state, cx, cy, target_ind)
#publish our desired position
mkr = create_marker(cx[target_ind], cy[target_ind], '/map')
self.target_pub.publish(mkr)
# Arrow that represents steering angle
arrow = create_marker(0, 0, '/base_link')
arrow.type = 0 #arrow
arrow.scale.x = 2.0
arrow.scale.y = 1.0
arrow.scale.z = 1.0
arrow.color.r = 1.0
arrow.color.g = 0.0
arrow.color.b = 0.0
quater = tf.quaternion_from_euler(0, 0, di)
arrow.pose.orientation.x = quater[0]
arrow.pose.orientation.y = quater[1]
arrow.pose.orientation.z = quater[2]
arrow.pose.orientation.w = quater[3]
self.target_twist_pub.publish(arrow)
state = self.update(state, ai, di)
x.append(state.x)
y.append(state.y)
yaw.append(state.yaw)
v.append(state.v)
t.append(time)
else:
self.path_valid = False
rospy.logwarn("It appears the cart is already at the destination")
#Check if we've reached the destination, if so we should change the cart state to finished
rospy.loginfo("Done navigating")
self.current_state = VehicleState()
self.current_state.is_navigating = False
self.current_state.reached_destination = True
notify_server = String()
# Let operator know why current path has stopped
if self.path_valid:
rospy.loginfo(
"Reached Destination succesfully without interruption")
self.arrived_pub.publish(notify_server)
else:
rospy.loginfo(
"Already at destination, or there may be no path to get to the destination or navigation was interrupted."
)
# Update the internal state of the vehicle
self.vehicle_state_pub.publish(self.current_state)
msg = VelAngle()
msg.vel = 0
msg.angle = 0
msg.vel_curr = 0
self.motion_pub.publish(msg)