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 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())
def control_callback(event):
# rospy.Timer() callback trigered by Duration(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 thruster_1, thruster_2, thruster_3, thruster_4 # control forces
twist = Twist()
# Get forces in global frame using PD controller
if (goals_received == True and joy_button_L1 == 0 and joy_button_R1 == 0):
# print("x_acc_goal: ",x_acc_goal)
# print("y_acc_goal: ",y_acc_goal)
# PD body 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'])
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_goal, x_vel, x_vel_goal,x_acc_goal,param_model_x['kp'], param_model_x['kd'], param_model_x['lim'])
# ay = control.acc_ctrl(y, y_goal, y_vel, y_vel_goal,y_acc_goal,param_model_y['kp'], param_model_y['kd'], param_model_y['lim'])
# convert into body frame:
# 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)
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 slam_callback(msg):
global time_prev, x_prev, y_prev, psi_prev
global thruster_1, thruster_2, thruster_3, thruster_4
# ----- Goals recived? -----
if (goals_received == False):
thruster_1 = 0
thruster_2 = 0
thruster_3 = 0
thruster_4 = 0
pub_velocity.publish(thruster_ctrl_msg())
print("No goals received, idle. ")
# Turn off thrustrs and exit callback
return
# ----- Position and Velocity -----
# Get current position, orientation and time:
x = msg.pose.position.x
y = msg.pose.position.y
psi = tft.euler_from_quaternion([msg.pose.orientation.x,msg.pose.orientation.y,\
msg.pose.orientation.z,msg.pose.orientation.w])[2]
time = (msg.header.stamp.secs + msg.header.stamp.nsecs * 0.000000001)
# Derive velocities from slam data:
time_diff = time - time_prev
x_vel = control.get_velocity(x, x_prev, time_diff)
y_vel = control.get_velocity(y, y_prev, time_diff)
psi_vel = control.get_velocity(psi, psi_prev, time_diff)
# ----- Control -----
# Get forces in global frame using PD controller
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 into body frame:
x_body_ctrl = math.cos(psi) * x_global_ctrl + math.sin(psi) * y_global_ctrl
y_body_ctrl = -math.sin(psi) * x_global_ctrl + math.cos(
psi) * y_global_ctrl
# ----- Simulation -----
# vector forces scaled in body frame
x_sim = (x_body_ctrl) * linear_scale
y_sim = (y_body_ctrl) * linear_scale
psi_sim = (-psi_global_ctrl) * angular_scale
# thrust allocation
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
# Publish forces to simulation (joint_state_publisher message)
pub_velocity.publish(thruster_ctrl_msg())
# ----- Next iteratioon -----
# Before finishing, assign current values to previous ones
time_prev = time
x_prev = x
y_prev = y
psi_prev = psi
