def slam_callback(msg):
# Publishing node: hector_slam, topic: /slam_out_pose
global x, y, psi, x_vel, y_vel, psi_vel # slam position and derived velocity
global time_prev, x_prev, y_prev, psi_prev # variables for next iteration
# ----- 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 place holder if timer_callback inactive
# ----- for next iteratioon -----
time_prev = time
x_prev = x
y_prev = y
psi_prev = psi
def slam_callback(msg):
# Publishing node: hector_slam, topic: /slam_out_pose
global x,y,psi,x_vel,y_vel,psi_vel # slam position and derived velocity
global time_prev,x_prev,y_prev,psi_prev # variables for next iteration
# ----- 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)
# Transmit vicon data toa topic
p = PoseStamped()
# Write to PoseStamp positions:
p.header.stamp= rospy.Time.now()
p.pose.position.x = x
p.pose.position.y = y
# Angle
p.pose.position.z = psi
pub_vicon_test.publish(p)
# ----- for next iteratioon -----
time_prev = time
x_prev = x
y_prev = y
psi_prev = psi
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
