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