def control_callback(event):
# Actual control is done here. The 'event' is the rospy.Timer() duration period, can be used
# for trubleshooting. To test how often is executed use: $ rostopic hz /mallard/thruster_commands.
global x0,y0,q0,x_goal,y_goal,q_goal,ed
global time_elapsed,t_goal,t_goal_psi
global t_ramp,psivel
twist = Twist()
# Get forces in global frame using PD controller
if(goals_received == True and joy_button_L1 == 0 and joy_button_R1 == 0):
# new code - velocity ramp
# get current time
tn = rospy.Time.now()
time_now = tn.secs + (tn.nsecs * 0.000000001)
# time elapsed form the start of tracking the goal (equal to t_now in goal selector)
time_elapsed = time_now - time_begin
# get max velocities
xvelmax = abs(kguseful.safe_div((x_goal-x0),t_goal))
yvelmax = abs(kguseful.safe_div((y_goal-y0),t_goal))
# get desired position,velocity and acceleration from velocity ramp
x_des, x_vel_des, x_acc_des = kglocal.velramp(time_elapsed, xvelmax, x0, x_goal, t_ramp,name="x")
y_des, y_vel_des, y_acc_des = kglocal.velramp(time_elapsed, yvelmax, y0, y_goal, t_ramp,name="y")
# get desired angle:
qdes = kglocal.despsi_fun(q_goal, t_goal_psi, q0, time_elapsed)
psi_des = tft.euler_from_quaternion(qdes)[2] # Its a list: (roll,pitch,yaw)
# psi_des = psides[2]
psi_vel_des = kglocal.desvelpsi_fun(ed, t_goal_psi, time_elapsed,psivel)
# end new code
# PD control
# x_global_ctrl = control.proportional(x, x_goal, x_vel, x_vel_goal, param['kp'], param['kd'], param['lim'])
# y_global_ctrl = control.proportional(y, y_goal, y_vel, y_vel_goal, param['kp'], param['kd'], param['lim'])
psi_global_ctrl = control.proportional_angle(psi, psi_des,psi_vel,psi_vel_des, param['kp_psi'], param['kd_psi'], param['lim_psi'])
# PD body control
# x_PD_body = math.cos(psi)*x_global_ctrl + math.sin(psi)*y_global_ctrl
# y_PD_body =-math.sin(psi)*x_global_ctrl + math.cos(psi)*y_global_ctrl
# ----- Model control -----
ax = control.acc_ctrl(x, x_des, x_vel, x_vel_des,x_acc_des,param_model_x['kp'], param_model_x['kd'], param_model_x['lim'])
ay = control.acc_ctrl(y, y_des, y_vel, y_vel_des,y_acc_des,param_model_y['kp'], param_model_y['kd'], param_model_y['lim'])
# Model body control:
# aqx = Rt * [ax,ay]t
# vqx = Rt * [x_vel,y_vel]t
aqx = math.cos(psi)*ax + math.sin(psi)*ay
aqy = -math.sin(psi)*ax + math.cos(psi)*ay
vqx = math.cos(psi)*x_vel + math.sin(psi)*y_vel
vqy = -math.sin(psi)*x_vel + math.cos(psi)*y_vel
# print("X-velocity: " + str(round(vqx,4)))
x_body_model_ctrl = Mx*aqx + R1_x*vqx + R2_x*(vqx*abs(vqx))
# x_body_model_ctrl = Mx*aqx + R2_x*(vqx*abs(vqx))
y_body_model_ctrl = My*aqy + R1_y*vqy + R2_y*(vqy*abs(vqy))
# y_body_model_ctrl = My*aqy + R2_y*(vqy*abs(vqy))
# twist.angular.y = data.buttons[4]
# vector forces scaled in body frame
twist.linear.x = x_body_model_ctrl
# twist.linear.x = x_PD_body
twist.linear.y = y_body_model_ctrl
# twist.linear.y = y_PD_body
twist.angular.z = -psi_global_ctrl
# Test uniformity of the timer
now = rospy.Time.now()
# twist.angular.x = now.secs
# twist.angular.y = now.nsecs
# Publish forces to simulation (joint_state_publisher message)
pub_velocity.publish(twist)
# send [time,position,velocity,goal_position,goal_velocity,control input]
array_data = [now.secs,now.nsecs,\
x,y,psi,x_vel,y_vel,psi_vel,\
x_des,y_des,psi_des,x_vel_des,y_vel_des,psi_vel_des,\
x_body_model_ctrl,y_body_model_ctrl,psi_global_ctrl]
data_to_send = Float64MultiArray(data = array_data)
pub_data.publish(data_to_send)
# goals_received, xdes,ydes,psides[2],\
# xveldes,yveldes,psiveldes,ax,ay]
elif(joy_button_L1 == 1 or joy_button_R1 == 1):
twist.linear.x = joy_x
twist.linear.y = joy_y
twist.angular.z = joy_z
pub_velocity.publish(twist)
else:
# ----- idle if no goals -----
pub_velocity.publish(Twist())
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
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 control_callback(event):
# Actual control is done here. The 'event' is the rospy.Timer() duration period, can be used
# for trubleshooting. To test how often is executed use: $ rostopic hz /mallard/thruster_commands.
global x0,y0,q0,x_goal,y_goal,q_goal,ed
global time_elapsed,t_goal,t_goal_psi
global t_ramp,psivel
twist = Twist()
# Get forces in global frame using PD controller
if(goals_received == True and joy_button_L1 == 0 and joy_button_R1 == 0):
# new code - velocity ramp
# get current time
tn = rospy.Time.now()
time_now = tn.secs + (tn.nsecs * 0.000000001)
# time elapsed form the start of tracking the goal (equal to t_now in goal selector)
time_elapsed = time_now - time_begin
# get max velocities
xvelmax = abs(kguseful.safe_div((x_goal-x0),t_goal))
yvelmax = abs(kguseful.safe_div((y_goal-y0),t_goal))
# get desired position,velocity and acceleration from velocity ramp
x_des, x_vel_des, x_acc_des = kglocal.velramp(time_elapsed, xvelmax, x0, x_goal, t_ramp,name="x")
y_des, y_vel_des, y_acc_des = kglocal.velramp(time_elapsed, yvelmax, y0, y_goal, t_ramp,name="y")
# get desired angle:
qdes = kglocal.despsi_fun(q_goal, t_goal_psi, q0, time_elapsed)
psi_des = tft.euler_from_quaternion(qdes)[2] # Its a list: (roll,pitch,yaw)
psi_vel_des = kglocal.desvelpsi_fun(ed, t_goal_psi, time_elapsed,psivel)
# end new code
# PD global control
x_global_ctrl = control.proportional(x, x_goal, x_vel, x_vel_goal, param['kp'], param['kd'], param['lim'])
y_global_ctrl = control.proportional(y, y_goal, y_vel, y_vel_goal, param['kp'], param['kd'], param['lim'])
psi_global_ctrl = control.proportional_angle(psi, psi_goal,psi_vel,psi_vel_goal, param['kp_psi'], param['kd_psi'], param['lim_psi'])
# Convert to body coordiante
x_PD_body = math.cos(psi)*x_global_ctrl + math.sin(psi)*y_global_ctrl
y_PD_body =-math.sin(psi)*x_global_ctrl + math.cos(psi)*y_global_ctrl
# Publish to twist velocitycommand
twist.linear.x = x_PD_body
twist.linear.y = y_PD_body
twist.angular.z = -psi_global_ctrl
# Generate time instance
now = rospy.Time.now()
twist.angular.x = now.secs
twist.angular.y = now.nsecs
# Publish forces to simulation (joint_state_publisher message)
pub_velocity.publish(twist)
# send [time,position,velocity,goal_position,goal_velocity,control input]
array_data = [now.secs,now.nsecs,\
x,y,psi,x_vel,y_vel,psi_vel,\
x_des,y_des,psi_des,x_vel_des,y_vel_des,psi_vel_des,\
x_body_model_ctrl,y_body_model_ctrl,psi_global_ctrl]
data_to_send = Float64MultiArray(data = array_data)
pub_data.publish(data_to_send)
elif(joy_button_L1 == 1 or joy_button_R1 == 1):
twist.linear.x = joy_x
twist.linear.y = joy_y
twist.angular.z = joy_z
pub_velocity.publish(twist)
else:
# ----- idle if no goals -----
pub_velocity.publish(Twist())