def velramp(t, velabs, xy0, xyg, tr):
d = xyg - xy0
vel = velabs * kguseful.safe_div(d,
abs(d)) # avoid zero division stability
a = vel / tr
tv = kguseful.safe_div((d - tr * vel), vel)
if tv > 0:
if t <= tr:
veldes = t * a
xydes = 0.5 * veldes * t + xy0
elif t > tr and t < tr + tv:
veldes = vel
xydes = 0.5 * vel * tr + (t - tr) * vel + xy0
elif t > tr + tv and t < 2 * tr + tv:
veldes = vel - (t - tr - tv) * a
xydes = 0.5 * vel * tr + tv * vel + veldes * (
t - tr - tv) + 0.5 * (vel - veldes) * (t - tr - tv) + xy0
else:
veldes = 0
xydes = xyg
elif tv <= 0:
tg = math.sqrt((4 * d) / a)
if t <= 0.5 * tg:
veldes = a * t
xydes = 0.5 * veldes * t + xy0
elif t > 0.5 * tg and t < tg:
veldes = 0.5 * tg * a - (t - 0.5 * tg) * a
vm2 = 0.5 * tg * a
xydes = vm2 * 0.5 * tg - 0.5 * (tg - t) * vm2 + xy0
else:
veldes = 0
xydes = xyg
return xydes, veldes
def velramp(t, velabs, xy0, xyg, tr,name):
print_results = False # set to True if you want to print
d = xyg-xy0
vel = velabs*kguseful.safe_div(d, abs(d)) # avoid zero division stability
a = vel/tr
# Can it reach the user-specified velocity? Shape: ramp or triangle.
tv = kguseful.safe_div((d-tr*vel), vel)
if tv > 0:
# where(timewise) robot is on ramp
if t <= tr:
ades = a
veldes = t*a
xydes = 0.5*veldes*t + xy0
if (name == "x" and print_results) : print(name + "-ramp acc is +a")
elif t > tr and t < tr+tv:
ades = 0
veldes = vel
xydes = 0.5*vel*tr + (t-tr)*vel + xy0
if (name == "x" and print_results) : print(name + "-ramp acc is 0")
elif t > tr + tv and t < 2*tr + tv:
ades = -a
veldes = vel-(t-tr-tv)*a
xydes = 0.5*vel*tr + tv*vel + veldes*(t-tr-tv) + 0.5*(vel-veldes)*(t-tr-tv) + xy0
if (name == "x" and print_results) : print(name + "-ramp acc is -a")
else:
ades = 0
veldes = 0
xydes = xyg
if (name == "x" and print_results) : print(name + "-ramp. Outside of ramp, velocity and acc is 0!")
elif tv <= 0:
# calculate tg insteadof tr. Fixes the error when Mallard traverses sidways, y-direction for instance.
# Thenk velocity, acceleration and distance is in x-direction is 0.
if (a == 0):
tg = 0
# tg = math.sqrt((4*d)/0.001)
if (name == "x" and print_results) : print (name + "-triangle. Distance and acceleration is 0!")
else:
tg = math.sqrt((4*d)/a)
if (name == "x" and print_results) : print(name + "-triangle. Standard computation")
# where(timewise) robot is on triangle ramp
if t <= 0.5*tg:
ades = a
veldes = a*t
xydes = 0.5*veldes*t + xy0
if (name == "x" and print_results) : print(name + "-triangle acc is +a")
elif t > 0.5*tg and t < tg:
ades = -a
veldes = 0.5*tg*a - (t - 0.5*tg)*a
vm2 = 0.5*tg*a
xydes = vm2*0.5*tg - 0.5*(tg-t)*vm2 + xy0
if (name == "x" and print_results) : print(name + "-triangle acc is -a")
else:
ades = 0
veldes = 0
xydes = xyg
if (name == "x" and print_results) : print(name + "-tirnagle. Outside of ramp, velocity and acc is 0!")
return xydes, veldes,ades
def despsi_fun(goal, t_gpsi, q0f, t_nowf):
if t_nowf < t_gpsi:
ratio = kguseful.safe_div(t_nowf, t_gpsi)
des = tft.quaternion_slerp(q0f, goal, ratio)
else:
des = goal
return des
def cont_fun(xy, des, vel, veldes, kp, kd, lim):
fp = (des - xy) * kp
fd = (veldes - vel) * kd
f_nav = fp + fd
if abs(f_nav) > lim:
f_nav = kguseful.safe_div(f_nav, abs(f_nav)) * lim
print('xy limit hit')
return f_nav
def contpsi_fun(q, des, vel, veldes, kp, kd, lim):
err_psi = kguseful.err_psi_fun(q, des)
fp = err_psi * kp
fd = (veldes - vel) * kd
f_nav = fp + fd
if abs(f_nav) > lim:
f_nav = kguseful.safe_div(f_nav, abs(f_nav)) * lim
print('psi limit hit')
return f_nav
def callback(data, paramf):
global dtv, dxv, dyv, tp, xp, yp, qp, ed
global flag_first, flag_goal_met, flag_end, n_safe, n_goals
global x_goal, y_goal, q_goal, t_goal, t_goal_psi, x0, y0, q0, t0, goal_array
pub = rospy.Publisher('/mallard/cmd_vel', Twist, queue_size=10)
twist = Twist()
q_now = [data.pose.orientation.x, data.pose.orientation.y, data.pose.orientation.z, data.pose.orientation.w]
# if it's the first run then zero is current position
if flag_first:
x0 = data.pose.position.x
y0 = data.pose.position.y
q0 = q_now
x_goal = goal_array[n_goals, 0]
y_goal = goal_array[n_goals, 1]
q_goal = tft.quaternion_from_euler(0, 0, goal_array[n_goals, 2])
# if a goal has been met then increment the goal
if flag_goal_met:
x0 = goal_array[n_goals, 0]
y0 = goal_array[n_goals, 1]
q0 = tft.quaternion_from_euler(0, 0, goal_array[n_goals, 2])
n_goals = n_goals + 1
x_goal = goal_array[n_goals, 0]
y_goal = goal_array[n_goals, 1]
q_goal = tft.quaternion_from_euler(0, 0, goal_array[n_goals, 2])
# work out time it will take to get to new goal, xy and psi
if flag_first or flag_goal_met:
t0 = data.header.stamp.secs + data.header.stamp.nsecs * 0.000000001
dist = math.sqrt(pow((x_goal - x0), 2) + pow((y_goal - y0), 2))
t_goal = kguseful.safe_div(dist, paramf['vel']) # avoid zero division stability
ed = kguseful.err_psi_fun(q0, q_goal)
t_goal_psi = abs(kguseful.safe_div(ed, paramf['psivel']))
flag_first = False
flag_goal_met = False
rospy.loginfo("t_goal_psi: %s, t_goal: %s", t_goal_psi, t_goal)
rospy.loginfo("x: %s, y: %s", data.pose.position.x, data.pose.position.y)
rospy.loginfo("xg: %s, yg: %s", x_goal, y_goal)
rospy.loginfo("goal number: %s, end goal: %s", n_goals + 1, goal_array.shape[0])
# build difference history buffers and calculate velocities
t_now = (data.header.stamp.secs + data.header.stamp.nsecs * 0.000000001) - t0 # time since start of goal
dtv.appendleft(t_now-tp) # time difference vector
dxv.appendleft(data.pose.position.x-xp) # x difference vector
dyv.appendleft(data.pose.position.y-yp) # y difference vector
dpsi = kguseful.err_psi_fun(qp, q_now)
dpsiv.appendleft(dpsi) # psi difference vector
xvel = kglocal.vel_fun(list(dxv), list(dtv)) # velocity vectors x, y and psi
yvel = kglocal.vel_fun(list(dyv), list(dtv))
psivel = kglocal.vel_fun(list(dpsiv), list(dtv))
# get current desired positions and velocities
xdes = kglocal.desxy_fun(x_goal, x0, t_goal, t_now)
ydes = kglocal.desxy_fun(y_goal, y0, t_goal, t_now)
qdes = kglocal.despsi_fun(q_goal, t_goal_psi, q0, t_now)
xveldes = kglocal.desvel_fun(x_goal, x0, t_goal, t_now)
yveldes = kglocal.desvel_fun(y_goal, y0, t_goal, t_now)
psiveldes = kglocal.desvelpsi_fun(ed, t_goal_psi, t_now, paramf['psivel'])
# Get forces in nav frame using PD controller
xf_nav = kglocal.cont_fun(data.pose.position.x, xdes, xvel, xveldes, paramf['kp'], paramf['kd'], paramf['lim'])
yf_nav = kglocal.cont_fun(data.pose.position.y, ydes, yvel, yveldes, paramf['kp'], paramf['kd'], paramf['lim'])
psif_nav = kglocal.contpsi_fun(q_now, qdes, psivel, psiveldes, paramf['kp_psi'], paramf['kd_psi'], paramf['lim_psi'])
# put xy forces into body frame
f_body = kguseful.quat_rot([xf_nav, yf_nav, 0], [-q_now[0], -q_now[1], -q_now[2], q_now[3]])
# put forces into twist structure and publish
twist.linear.x = f_body[0]
twist.linear.y = f_body[1]
twist.angular.z = -psif_nav # minus is a fix to account for wrong direction on el_mal
if n_safe > paramf['nv'] + 5: # stop output while deque buffers are filling
pub.publish(twist) # publish twist command
n_safe = n_safe + 1
# if goal is met then move to next goal
if abs(x_goal - data.pose.position.x) <= paramf['goal_tol']:
if abs(y_goal - data.pose.position.y) <= paramf['goal_tol']:
e_psi = kguseful.err_psi_fun(q_now, q_goal)
if abs(e_psi) <= paramf['goal_tol_psi']:
if goal_array.shape[0] != n_goals + 1: # if there are more goals
print 'goal met'
flag_goal_met = True # set flag to move to next goal
if goal_array.shape[0] == n_goals + 1: # if there are more goals
print 'final goal met - holding position'
# change current to previous values
tp = t_now
xp = data.pose.position.x
yp = data.pose.position.y
qp = q_now
def callback(data, paramf):
global dtv, dxv, dyv, tp, xp, yp, qp, ed
global flag_first, flag_goal_met, flag_end, n_safe, n_goals
global x_goal, y_goal, q_goal, t_goal, t_goal_psi, x0, y0, q0, t0, goal_array
global flag_obstacle_close, flag_obstacle_prev, fobs, t_stall
# THIS CODE ADDED FOR RVIZ COVERAGE SELECTION
# ------------------
# if no goal positions exist in goal_array, then exit this callback by using return
# no further computation is undertaken
if len(goal_array) == 0:
return
# ------------------
# pub = rospy.Publisher('/mallard/cmd_vel', Twist, queue_size=10)
twist = Twist()
q_now = [
data.pose.orientation.x, data.pose.orientation.y,
data.pose.orientation.z, data.pose.orientation.w
]
##### code added for obstacle avoidance
global param_obs, lidar_data
if lidar_data is not None:
flag_obstacle_close, fobs = detect_obstacle(lidar_data, param_obs)
# rospy.loginfo("obstacle avoidance forces: %s", fobs)
# rospy.loginfo("obstacle close flag: %s", flag_obstacle_close)
else:
flag_obstacle_close = False
fobs = (0, 0)
# Force obstacle avoidance to be null
# ---------------------------------
# Remove these two lines to use obstacle avoidance
# flag_obstacle_close = False
fobs = (0, 0)
# ----------------------------------
# if it's the first run then zero is current position
if flag_first:
x0 = data.pose.position.x
y0 = data.pose.position.y
q0 = q_now
x_goal = goal_array[n_goals, 0]
y_goal = goal_array[n_goals, 1]
q_goal = tft.quaternion_from_euler(0, 0, goal_array[n_goals, 2])
# if a obstacle is to close flag is set
if flag_obstacle_close != flag_obstacle_prev:
if flag_obstacle_close:
t_stall = data.header.stamp.secs
x0 = data.pose.position.x + 1e-9
y0 = data.pose.position.y + 1e-9
q0 = q_now
x_goal = data.pose.position.x
y_goal = data.pose.position.y
q_goal = q_now
print 'STOPPING!'
else:
t_stall = data.header.stamp.secs - t_stall
x0 = data.pose.position.x
y0 = data.pose.position.y
q0 = q_now
x_goal = goal_array[n_goals, 0]
y_goal = goal_array[n_goals, 1]
q_goal = tft.quaternion_from_euler(0, 0, goal_array[n_goals, 2])
print 'RESUMING'
# if a goal has been met then increment the goal
if flag_goal_met and not flag_obstacle_close:
x0 = goal_array[n_goals, 0]
y0 = goal_array[n_goals, 1]
q0 = tft.quaternion_from_euler(0, 0, goal_array[n_goals, 2])
n_goals = n_goals + 1
x_goal = goal_array[n_goals, 0]
y_goal = goal_array[n_goals, 1]
q_goal = tft.quaternion_from_euler(0, 0, goal_array[n_goals, 2])
# work out time it will take to get to new goal, xy and psi
if flag_first or flag_goal_met:
t0 = data.header.stamp.secs + data.header.stamp.nsecs * 0.000000001
dist = math.sqrt(pow((x_goal - x0), 2) + pow((y_goal - y0), 2))
t_goal = kguseful.safe_div(
dist, paramf['vel']) # avoid zero division stability
ed = kguseful.err_psi_fun(q0, q_goal)
t_goal_psi = abs(kguseful.safe_div(ed, paramf['psivel']))
flag_first = False
flag_goal_met = False
rospy.loginfo("t_goal_psi: %s, t_goal: %s", t_goal_psi, t_goal)
rospy.loginfo("x: %s, y: %s", data.pose.position.x,
data.pose.position.y)
rospy.loginfo("xg: %s, yg: %s", x_goal, y_goal)
rospy.loginfo("goal number: %s, end goal: %s", n_goals + 1,
goal_array.shape[0])
# build difference history buffers and calculate velocities
t_now = (data.header.stamp.secs + data.header.stamp.nsecs *
0.000000001) - t0 # time since start of goal
dtv.appendleft(t_now - tp) # time difference vector
dxv.appendleft(data.pose.position.x - xp) # x difference vector
dyv.appendleft(data.pose.position.y - yp) # y difference vector
dpsi = kguseful.err_psi_fun(qp, q_now)
dpsiv.appendleft(dpsi) # psi difference vector
xvel = kglocal.vel_fun(list(dxv),
list(dtv)) # velocity vectors x, y and psi
yvel = kglocal.vel_fun(list(dyv), list(dtv))
psivel = kglocal.vel_fun(list(dpsiv), list(dtv))
# get current desired positions and velocities
xdes = kglocal.desxy_fun(x_goal, x0, t_goal, t_now)
ydes = kglocal.desxy_fun(y_goal, y0, t_goal, t_now)
qdes = kglocal.despsi_fun(q_goal, t_goal_psi, q0, t_now)
xveldes = kglocal.desvel_fun(x_goal, x0, t_goal, t_now)
yveldes = kglocal.desvel_fun(y_goal, y0, t_goal, t_now)
psiveldes = kglocal.desvelpsi_fun(ed, t_goal_psi, t_now, paramf['psivel'])
# Get forces in nav frame using PD controller
xf_nav = kglocal.cont_fun(data.pose.position.x, xdes, xvel, xveldes,
paramf['kp'], paramf['kd'], paramf['lim'])
yf_nav = kglocal.cont_fun(data.pose.position.y, ydes, yvel, yveldes,
paramf['kp'], paramf['kd'], paramf['lim'])
psif_nav = kglocal.contpsi_fun(q_now, qdes, psivel, psiveldes,
paramf['kp_psi'], paramf['kd_psi'],
paramf['lim_psi'])
# put xy forces into body frame
f_body = kguseful.quat_rot([xf_nav, yf_nav, 0],
[-q_now[0], -q_now[1], -q_now[2], q_now[3]])
# print flag_obstacle_close, np.round(fobs,4)
# put forces into twist structure and publish
twist.linear.x = f_body[0] + fobs[0]
twist.linear.y = f_body[1] + fobs[1]
twist.angular.z = -psif_nav # minus is a fix to account for wrong direction on el_mal
if n_safe > paramf['nv'] + 5: # stop output while deque buffers are filling
pub.publish(twist) # publish twist command
n_safe = n_safe + 1
# if goal is met then move to next goal
if not flag_obstacle_close:
if abs(x_goal - data.pose.position.x) <= paramf['goal_tol']:
if abs(y_goal - data.pose.position.y) <= paramf['goal_tol']:
e_psi = kguseful.err_psi_fun(q_now, q_goal)
if abs(e_psi) <= paramf['goal_tol_psi']:
if goal_array.shape[
0] != n_goals + 1: # if there are more goals
print 'goal met'
flag_goal_met = True # set flag to move to next goal
if goal_array.shape[
0] == n_goals + 1: # if there are more goals
print 'final goal met - holding position'
# change current to previous values
tp = t_now
xp = data.pose.position.x
yp = data.pose.position.y
qp = q_now
flag_obstacle_prev = flag_obstacle_close
def vel_fun(dv, dt):
dvdt = [kguseful.safe_div(i, j) for i, j in zip(dv, dt)]
vel = kguseful.safe_div(sum(dvdt), float(len(dvdt)))
return vel
def desvel_fun(goal, zero, t_goalf, t_nowf):
if t_nowf < t_goalf:
des = kguseful.safe_div((goal - zero), t_goalf)
else:
des = 0
return des
def desxy_fun(goal, zero, t_goalf, t_nowf):
if t_nowf < t_goalf:
des = zero + (goal - zero) * (kguseful.safe_div(t_nowf, t_goalf))
else:
des = goal
return des
