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