def slam_callback(data, paramf):
global dtv, dxv, dyv, tp, xp, yp, qp, ed
global flag_first, flag_goal_met, flag_end, n_safe, n_goals, goals_received
global x_goal, y_goal, q_goal, t_goal, t_goal_psi, x0, y0, q0, t0, goal_array, psides
global back_and_forth, single_goal, counter
# if no goal positions exist, then exit this callback!!!
if len(goal_array) == 0:
data_to_send = Float64MultiArray(data=[goals_received, flag_goal_met])
pub_goal.publish(data_to_send)
return
q_now = [
data.pose.orientation.x, data.pose.orientation.y,
data.pose.orientation.z, data.pose.orientation.w
]
# for first run get current position and load first goals:
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 goal has been met assign new goals:
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
# --------------------------------------
# to go back nad forth between two goals
if (n_goals > 1 and back_and_forth):
n_goals = 0
elif (single_goal):
if (counter <= 100 and n_goals == 1):
if (counter % 10 == 0):
print("wait for 10 seconds; counting " + str(counter / 10))
n_goals = 0
counter += 1
# print("n_goals: " + str(n_goals))
else: # maitain the goal
if (n_goals == 2): print("GOAL REACHED")
n_goals = 1
counter = 0
# print("n_goals: " + str(n_goals))
# --------------------------------------
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("x0: ", x0, " x_goal: ", x_goal)
# work out time and distance 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'])
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)
# Test if goal has been met:
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 no more goals
print 'final goal met - holding position'
# --------- Publish goals ---------
# new code
array = [goals_received, flag_goal_met,\
x0,y0,q0[0],q0[1],q0[2],q0[3],\
x_goal,y_goal,q_goal[0],q_goal[1],q_goal[2],q_goal[3],\
t_goal,t_goal_psi,ed, \
param['t_ramp'],param['psivel']]
# end new code
data_to_send = Float64MultiArray(data=array)
pub_goal.publish(data_to_send)
def slam_callback(data, paramf):
global dtv, dxv, dyv, tp, xp, yp, qp, ed
global flag_first, flag_goal_met, flag_end, n_safe, n_goals, goals_received
global x_goal, y_goal, q_goal, t_goal, t_goal_psi, x0, y0, q0, t0, goal_array,psides
global back_and_forth,single_goal,counter
# if no goal positions exist, then exit this callback!!!
if len(goal_array) == 0:
data_to_send = Float64MultiArray(data = [goals_received])
pub_goal.publish(data_to_send)
return
q_now = [data.pose.orientation.x, data.pose.orientation.y, data.pose.orientation.z, data.pose.orientation.w]
# for first run get current position and load first goals:
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 goal has been met assign new goals:
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
# --------------------------------------
# to go back nad forth between two goals
if(n_goals > 1 and back_and_forth):
n_goals = 0
elif(single_goal):
if(counter <= 1000 and n_goals == 1):
if(counter % 100 == 0): print("wait for 10 seconds; counting " + str(counter/100))
n_goals = 0
counter += 1
# print("n_goals: " + str(n_goals))
else: # maitain the goal
if(n_goals == 2): print("GOAL REACHED")
n_goals = 1
counter = 0
# print("n_goals: " + str(n_goals))
# --------------------------------------
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 and distance 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'])
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)
# time since start of the goal:
t_now = (data.header.stamp.secs + data.header.stamp.nsecs * 0.000000001) - t0
# GOALS - get desired linear positions and velocities
xvelmax = abs(kguseful.safe_div((x_goal-x0), t_goal))
yvelmax = abs(kguseful.safe_div((y_goal-y0), t_goal))
name = "x"
# print(name, " velocity: ",xvelmax)
xdes, xveldes,ax = kglocal.velramp(t_now, xvelmax, x0, x_goal, param['t_ramp'],name)
name = "y"
# print(name, " velocity: ",yvelmax)
ydes, yveldes,ay = kglocal.velramp(t_now, yvelmax, y0, y_goal, param['t_ramp'],name)
# print("ax: ", ax, "xveldes: ",xveldes, " xdes: ",xdes)
# get desired angular positions and velocities
qdes = kglocal.despsi_fun(q_goal, t_goal_psi, q0, t_now)
# Angle (Psides) and angular velocity (psiveldes) in euler:
psides = tft.euler_from_quaternion(qdes) # Its a list: (roll,pitch,yaw)
psiveldes = kglocal.desvelpsi_fun(ed, t_goal_psi, t_now, paramf['psivel'])
# VELOCITIES - calculate velocities from current and previous positions
# 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))
# Test if goal has been met:
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 no more goals
print 'final goal met - holding position'
# --------- Publish goals ---------
# publish goal array
array = [goals_received, xdes,ydes,psides[2],\
xveldes,yveldes,psiveldes,ax,ay]
data_to_send = Float64MultiArray(data = array)
pub_goal.publish(data_to_send)
# 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'])
# pub_goal.publish(goals_stamped)
# PREVIOUS VALUES - 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
# ---- simulation----
global linear_scale, angular_scale, thruster_1, thruster_2, thruster_3, thruster_4, a_sim, b_sim
# ------------------
# 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
# ---------------------------------------- OBSTACLE removed
# 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
# ---------------------------------------- OBSTACLE removed
# 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
# ---------------------------------------- OBSTACLE removed
# if flag_goal_met and not flag_obstacle_close:
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
# 'nv' parameter comes into play here
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
# xvelmax = abs((x_goal-x0)/t_goal) # peak velocity in x and y
xvelmax = abs(kguseful.safe_div((x_goal - x0), t_goal))
yvelmax = abs(kguseful.safe_div((y_goal - y0), t_goal))
xdes, xveldes = kglocal.velramp(t_now, xvelmax, x0, x_goal,
param['t_ramp'])
ydes, yveldes = kglocal.velramp(t_now, yvelmax, y0, y_goal,
param['t_ramp'])
# xdes = kglocal.desxy_fun(x_goal, x0, t_goal, t_now)
# ydes = kglocal.desxy_fun(y_goal, y0, t_goal, t_now)
# xveldes = kglocal.desvel_fun(x_goal, x0, t_goal, t_now)
# yveldes = kglocal.desvel_fun(y_goal, y0, t_goal, t_now)
qdes = kglocal.despsi_fun(q_goal, t_goal_psi, q0, 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)
# ------- Simulation ---------
x_sim = (f_body[0]) * linear_scale
y_sim = (f_body[1]) * linear_scale
psi_sim = (-psif_nav) * angular_scale
thruster_1 = 0 + 0.5 * x_sim + a_sim * psi_sim
thruster_2 = 0 + 0.5 * x_sim - a_sim * psi_sim
thruster_3 = 0 - 0.5 * y_sim + b_sim * psi_sim
thruster_4 = 0 - 0.5 * y_sim - b_sim * psi_sim
# ------- end simulation -------------
# 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
# -------- Simulation --------
pub_velocity.publish(thruster_ctrl_msg())
# ------- end simulation -------------
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