def main(): rospy.init_node('actuator_controller', anonymous=True) rospy.Subscriber("/release_signal", String, callback) pub = rospy.Publisher('/mavros/actuator_control', ActuatorControl, queue_size=10) rate = rospy.Duration(1/5.0) msg_out = ActuatorControl() msg_out.group_mix = 2 # Use group 2 (auxilary controls) msg_out.controls = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] while not rospy.is_shutdown(): if blue_open: act_index = blue_servo - 1 msg_out.controls[act_index] = 1.0 else: act_index = blue_servo - 1 msg_out.controls[act_index] = -1.0 if orange_open: act_index = orange_servo - 1 msg_out.controls[act_index] = 1.0 else: act_index = orange_servo - 1 msg_out.controls[act_index] = -1.0 msg_out.header.stamp = rospy.Time.now() pub.publish(msg_out) rospy.sleep(rate)
def talker(): rospy.init_node('actuator_controller', anonymous=True) sub_trig = rospy.Subscriber('~imagery_trigger', Empty, talker) pub = rospy.Publisher('/mavros/actuator_control', ActuatorControl, queue_size=10) rate = rospy.Rate(1) msg_out = ActuatorControl() msg_out.group_mix = 1 # Use group 1 (auxilary controls) msg_out.controls = [0.0, 0.0, 0.0, 0.0, -1.0, -1.0, -1.0, -1.0] is_low = True if not rospy.is_shutdown(): is_low = not is_low if is_low: msg_out.controls = [0.0, 0.0, 0.0, 0.0, -1.0, -1.0, -1.0, -1.0] rospy.loginfo("Set servos low") else: msg_out.controls = [0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 1.0] rospy.loginfo("Set servos high") msg_out.header.stamp = rospy.Time.now() pub.publish(msg_out) rate.sleep()
def __init__(self): self.message_pub = rospy.Publisher('/mavros/actuator_control', ActuatorControl, queue_size=1) self.actuator_control_message = ActuatorControl() self.seq = 0 self.start_offboard() self.start_throttle() self.start_stearing()
def pitch_camera(self): pitch_actuator = ActuatorControl() pitch_actuator.group_mix = 2 pitch_actuator.controls[1] = -0.7 import pdb pdb.set_trace() for jj in range(20): self.uav_actuator.publish(pitch_actuator) self.uav_set_mode(base_mode=0, custom_mode="OFFBOARD") self.uav_arming(True) self.r.sleep()
def __init__(self): magnetmsg = ActuatorControl('header', 'group.mix', 3) magnetmsg.controls[5] = 1.0 publish = rospy.Publisher('/mavros/actuator_controls', ActuatorControl, queue_size=10) self.rate = rospy.Rate(10) # 10Hz publish.publish(magnetmsg) self.rate.sleep()
def my_code(): act = ActuatorControl() act.header = Header() act.controls[0] = 0.0 #ros_roll act.controls[1] = 0.0 #ros_pitch act.controls[2] = 1 #ros_yaw act.controls[3] = 0.5 #ros_throttle act.controls[4] = 0.0 act.controls[5] = 0.0 act.controls[6] = 0.0 act.controls[7] = 0.0 while not rospy.is_shutdown(): pub_act.publish(act)
def __init__(self, gui_ready): self.gladefile = "gui.glade" self.builder = Gtk.Builder() self.builder.add_from_file(self.gladefile) self.builder.connect_signals(self) self.window = self.builder.get_object("main_window") self.window.show() self.lbl_airspeed = self.builder.get_object("lbl_airspeed") self.lbl_groundspeed = self.builder.get_object("lbl_groundspeed") self.lbl_heading = self.builder.get_object("lbl_heading") self.lbl_throttle = self.builder.get_object("lbl_throttle") self.lbl_altitude = self.builder.get_object("lbl_altitude") self.lbl_climb = self.builder.get_object("lbl_climb") self.lbl_landing_length = self.builder.get_object("lbl_landing_length") self.lbl_rssi = self.builder.get_object("lbl_rssi") self.lbl_bat_percent = self.builder.get_object("lbl_bat_percent") self.lbl_payload0_info = self.builder.get_object("lbl_payload0_info") self.lbl_payload1_info = self.builder.get_object("lbl_payload1_info") self.btn_record_telemetry = self.builder.get_object("btn_record_telemetry") self.btn_drop_payload0 = self.builder.get_object("btn_drop_payload0") self.btn_drop_payload1 = self.builder.get_object("btn_drop_payload1") self.btn_reset_das = self.builder.get_object("btn_reset_das") self.btn_record_telemetry.set_label("Record Telemetry") self.btn_video_window = self.builder.get_object("btn_video_window") self.btn_video_window.set_label("Open FPV Window") self.btn_record_video = self.builder.get_object("btn_record_video") self.btn_record_video.set_label("Record Video") self.reset_dialog = self.builder.get_object("reset_dialog") self.reset_dialog.set_transient_for(self.window) self.btn_reset_dialog_cncl = self.builder.get_object("btn_reset_dialog_cncl") self.btn_reset_dialog_ok = self.builder.get_object("btn_reset_dialog_ok") self.payload_dialog = self.builder.get_object("payload_dialog") self.payload_dialog.set_transient_for(self.window) self.lbl_payload_dialog = self.builder.get_object("lbl_payload_dialog") self.btn_payload_dialog_cncl = self.builder.get_object("btn_payload_dialog_cncl") self.btn_payload_dialog_ok = self.builder.get_object("lbl_payload_dialog_ok") self.gui_ready = gui_ready self.btn_tilt_up = self.builder.get_object("btn_tilt_up") self.btn_tilt_down = self.builder.get_object("btn_tilt_down") self.pub = rospy.Publisher('/mavros/actuator_control', ActuatorControl, queue_size=3) self.msg = ActuatorControl() self.msg.header.frame_id = "payload_servo" self.msg.group_mix = 3
def motr(a): rospy.init_node('motor', anonymous=True) pub = rospy.Publisher("/mavros/actuator_control", ActuatorControl, queue_size=10) rate = rospy.Rate(10) while not rospy.is_shutdown(): msg = ActuatorControl() msg.header.frame_id = "motorcontrol" msg.controls = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] msg.group_mix = 3 time.sleep(3) print 1 pub.publish(msg) time.sleep(3) rospy.spin()
def __init__(self): self.pub_sp = rospy.Publisher('/mavros/actuator_control', ActuatorControl, queue_size=10) rospy.init_node('motorControl', anonymous=True) rate = rospy.Rate(60) # 10hz rospy.wait_for_service('mavros/set_mode') change_mode = rospy.ServiceProxy('mavros/set_mode', SetMode) result_mode = change_mode(0, "MANUAL") act = ActuatorControl() while not rospy.is_shutdown(): act.controls = [1800, 1800, 1800, 1800, 0, 0, 0, 0] rospy.loginfo(str(act.controls)) self.pub_sp.publish(act) rate.sleep()
def img_callback(img_msg): # publish actuator steering with inference result if in OFFBOARD MODE global offboard_mode_is_active if offboard_mode_is_active: global model global graph global pub_act_ctl # preprocess img as in ML pipeline with PIL # img = np.array(Image.frombytes('RGB',(160,120),io.BytesIO(img_msg.data),'raw')) img = np.array(Image.open(io.BytesIO(img_msg.data))) # img_arr = np.frombuffer(img_msg.data, dtype='int8').reshape(160, 120, 3) # img inference #deb = rospy.Time.now() with graph.as_default(): #probs = model.predict(img_arr.reshape((1,) + img_arr.shape)) probs = model.predict(img.reshape((1, ) + img.shape)) #print(rospy.Time.now()-deb) # process classes to output [-1,1] from 15 classes array bucket_no = np.argmax(probs) act_val = (bucket_no / 7) - 1. # publish value in actuator_output_msg.channels[1] and fix power in actuator_output_msg.channels[1] actuator_control = ActuatorControl() actuator_control.header.stamp = rospy.Time.now() actuator_control.group_mix = 0 actuator_control.controls[0] = 0.0 actuator_control.controls[1] = 0.0 actuator_control.controls[2] = act_val actuator_control.controls[3] = 0.2 # Valeur entre 0 et 1 actuator_control.controls[4] = 0.0 actuator_control.controls[5] = 0.0 actuator_control.controls[6] = 0.0 actuator_control.controls[7] = 0.0 pub_act_ctl.publish(actuator_control)
def actuator_control(U): actuator_publisher = rospy.Publisher('/mavros/actuator_control', ActuatorControl, queue_size=10) act_msg = ActuatorControl() rate = rospy.Rate(100) # 10hz act_msg.controls[0] = 0.0 act_msg.controls[1] = 0.0 act_msg.controls[2] = 0.0 act_msg.controls[3] = 0.0 act_msg.controls[4] = 0.0 act_msg.controls[5] = 0.0 act_msg.controls[6] = 0.0 act_msg.controls[7] = 0.0 if U[0] > 0.7: U[0] = 0.7 act_msg.header.stamp = rospy.Time.now() act_msg.group_mix = 0 if not U[0] == 0 or not U[1] == 0: act_msg.controls[0] = U[1] - 0.2 act_msg.controls[3] = U[0] else: act_msg.controls[0] = 0.0 act_msg.controls[3] = 0.0 act_msg.controls[1] = 0.0 act_msg.controls[2] = 0.0 #act_msg.controls[3] = 0.5 act_msg.controls[4] = 0.0 act_msg.controls[5] = 0.0 act_msg.controls[6] = 0.0 act_msg.controls[7] = 0.0 actuator_publisher.publish(act_msg) rate.sleep()
def deploy_blue(self): pub = rospy.Publisher('/mavros/actuator_control', ActuatorControl, queue_size=10) rate = rospy.Rate(1) msg_out = ActuatorControl() msg_out.group_mix = 1 # Use group 1 (auxilary controls) msg_out.controls = [0.0, 0.0, 0.0, 0.0, -1.0, -1.0, -1.0, -1.0] is_low = True if not rospy.is_shutdown(): is_low = not is_low if is_low: msg_out.controls = [0.0, 0.0, 0.0, 0.0, -1.0, -1.0, -1.0, -1.0] rospy.loginfo("Set servos low") else: msg_out.controls = [0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 1.0, 1.0] rospy.loginfo("Set servos high") msg_out.header.stamp = rospy.Time.now() pub.publish(msg_out)
def __init__( self, roll_degs, pitch_degs, yaw_degs, ): self.message_pub = rospy.Publisher("/mavros/actuator_control", ActuatorControl, queue_size=10) self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose', PoseStamped, self.local_position_callback) self.actuator_control_message = ActuatorControl() self.seq = 0 self.local_position = PoseStamped() self.heading = 0.0 self.height = 0.0 self.roll_rads = np.deg2rad(roll_degs) self.pitch_rads = -0.5 #np.derad(-.0) #np.deg2rad(pitch_degs) self.yaw_rads = 0.0 # np.deg2rad(0.0) #np.deg2rad(yaw_degs)
def listener(): # In ROS, nodes are uniquely named. If two nodes with the same # node are launched, the previous one is kicked off. The # anonymous=True flag means that rospy will choose a unique # name for our 'listener' node so that multiple listeners can # run simultaneously. rospy.init_node('listener', anonymous=True) uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) roslaunch.configure_logging(uuid) launch = roslaunch.parent.ROSLaunchParent( uuid, ["/home/indigo/src/Firmware/launch/px4_posix_sitl.launch"]) launch.start() pub = rospy.Publisher('/mavros/actuator_control', ActuatorControl, queue_size=10) rate = rospy.Rate(200) # 10hz set_offboard_mode() # next_step() # next_step() delay() arm_quad() # next_step() # next_step() while not rospy.is_shutdown(): # delay() actuatorControl = ActuatorControl() actuatorControl.group_mix = actuatorControl.PX4_MIX_MANUAL_PASSTHROUGH actuatorControl.controls = [0, 0, 0, 1, 0, 0, 0, 0] pub.publish(actuatorControl) # next_step() rate.sleep()
def set_act(self, group, output, freq): """ Offboard method that sets the values of the mixers and/or actuators of the vehicle. Parameters ---------- group: int Desired control group. output : list Desired output values for the mixers and/or actuators of the drone. freq : float Topic publishing frequency, in Hz. """ pub = rospy.Publisher(self.info.drone_ns + '/mavros/actuator_control', ActuatorControl, queue_size=1) msg = ActuatorControl() msg.header = Header() msg.header.stamp = rospy.Time.now() msg.group_mix = group msg.controls = output pub.publish(msg) rate = rospy.Rate(freq) rate.sleep()
def __init__(self): self.node_name = rospy.get_name() # Publicaiton self.cmd_pub = rospy.Publisher('/mavros/actuator_control', ActuatorControl, queue_size=1) self.servo_state = 0 if __name__ == '__main__': rospy.init_node('release_servo_test_node', anonymous=False) rate = rospy.Rate(1) # 10hz n = release_test() n.servo_state = 0 cmd = ActuatorControl() while not rospy.is_shutdown(): if n.servo_state == 0: cmd.group_mix = 1 cmd.controls[4] = 1 cmd.controls[5] = 1 n.cmd_pub.publish(cmd) n.servo_state = 1 else: cmd.group_mix = 1 cmd.controls[4] = -1 cmd.controls[5] = -1 n.cmd_pub.publish(cmd)
def control(): # sp = np.load("xy_sin.npy") state_sub = rospy.Subscriber("/mavros/state", State, state_cb, queue_size=10) rospy.wait_for_service('mavros/cmd/arming') arming_client = rospy.ServiceProxy('mavros/cmd/arming', CommandBool) rospy.wait_for_service('mavros/set_mode') set_mode_client = rospy.ServiceProxy('mavros/set_mode', SetMode) acutator_control_pub = rospy.Publisher("/mavros/actuator_control", ActuatorControl, queue_size=10) local_pos_pub = rospy.Publisher("/mavros/setpoint_position/local", PoseStamped, queue_size=10) mocap_pos_pub = rospy.Publisher("/mavros/mocap/pose", PoseStamped, queue_size=10) imu_sub = rospy.Subscriber("/mavros/imu/data", Imu, imu_cb, queue_size=10) local_pos_sub = rospy.Subscriber("/mavros/local_position/pose", PoseStamped, lp_cb, queue_size=10) local_vel_sub = rospy.Subscriber("/mavros/local_position/velocity", TwistStamped, lv_cb, queue_size=10) act_control_sub = rospy.Subscriber("/mavros/act_control/act_control_pub", ActuatorControl, act_cb, queue_size=10) rospy.init_node('control', anonymous=True) rate = rospy.Rate(50.0) # print("*"*80) while not rospy.is_shutdown() and not current_state.connected: rate.sleep() rospy.loginfo(current_state.connected) # print("#"*80) pose = PoseStamped() # mocap_pose = PoseStamped() offb_set_mode = SetModeRequest() offb_set_mode.custom_mode = "OFFBOARD" arm_cmd = CommandBoolRequest() arm_cmd.value = True last_request = rospy.Time.now() i = 0 act = ActuatorControl() flag1 = False flag2 = False prev_imu_data = Imu() prev_time = rospy.Time.now() # prev_x = 0 # prev_y = 0 # prev_z = 0 # prev_vx = 0 # prev_vy = 0 # prev_vz = 0 prev_omega_x = 0 prev_omega_y = 0 prev_omega_z = 0 prev_vx = 0 prev_vy = 0 prev_vz = 0 delta_t = 0.02 count = 0 theta = 0.0 #rospy.loginfo("Outside") r = np.random.rand(100000, 1) * 10 x_trgt = [] y_trgt = [] row = 0 theta = 0.0 x_step = 0.0 y_step = 0.0 for t in xrange(0, 100000): val = r[t, 0] for p in xrange(0, 50): x_trgt.append(x_step) y_trgt.append(r[t, 0] * mt.sin(theta) + y_step) x_step += 0.01 y_step += 0.01 theta += 1.0 / 100 alpha = 0.01 temp = 0.0 y_new = [] for r_n in range(0, len(y_trgt)): temp = temp + alpha * (y_trgt[r_n] - temp) y_new.append(temp) while not rospy.is_shutdown(): if current_state.mode != "OFFBOARD" and ( rospy.Time.now() - last_request > rospy.Duration(5.0)): offb_set_mode_response = set_mode_client(offb_set_mode) if offb_set_mode_response.mode_sent: rospy.loginfo("Offboard enabled") flag1 = True last_request = rospy.Time.now() else: if current_state.armed == False: arm_cmd_response = arming_client(arm_cmd) if arm_cmd_response.success: rospy.loginfo("Vehicle armed") flag2 = True last_request = rospy.Time.now() # rospy.loginfo("Inside") curr_time = rospy.Time.now() if flag1 and flag2: x_f.append(float(local_position.pose.position.x)) y_f.append(float(local_position.pose.position.y)) z_f.append(float(local_position.pose.position.z)) vx_f.append(float(local_velocity.twist.linear.x)) vy_f.append(float(local_velocity.twist.linear.y)) vz_f.append(float(local_velocity.twist.linear.z)) # print(local_velocity.twist.linear.x) orientation = [ imu_data.orientation.w, imu_data.orientation.x, imu_data.orientation.y, imu_data.orientation.z ] (roll, pitch, yaw) = quaternion_to_euler_angle( imu_data.orientation.w, imu_data.orientation.x, imu_data.orientation.y, imu_data.orientation.z) r_f.append(float(mt.radians(roll))) p_f.append(float(mt.radians(pitch))) yaw_f.append(float(mt.radians(yaw))) sin_r_f.append(mt.sin(float(mt.radians(roll)))) sin_p_f.append(mt.sin(float(mt.radians(pitch)))) sin_y_f.append(mt.sin(float(mt.radians(yaw)))) cos_r_f.append(mt.cos(float(mt.radians(roll)))) cos_p_f.append(mt.cos(float(mt.radians(pitch)))) cos_y_f.append(mt.cos(float(mt.radians(yaw)))) rs_f.append(float(imu_data.angular_velocity.x)) ps_f.append(float(imu_data.angular_velocity.y)) ys_f.append(float(imu_data.angular_velocity.z)) ix.append(float(ixx)) iy.append(float(iyy)) iz.append(float(izz)) m.append(float(mass)) u0.append(act_controls.controls[0]) u1.append(act_controls.controls[1]) u2.append(act_controls.controls[2]) u3.append(act_controls.controls[3]) pose.pose.position.x = y_new[row] pose.pose.position.y = x_trgt[row] pose.pose.position.z = 3 row = row + 1 x_des.append(0) y_des.append(0) z_des.append(3) sin_y_des.append(yaw) cos_y_des.append(yaw) w_mag.append(0) w_x.append(0) w_y.append(0) w_z.append(0) n_t = curr_time - prev_time #delta_t = float(n_t.nsecs) * 1e-9 a_x.append( (float(local_velocity.twist.linear.x) - prev_vx) / delta_t) a_y.append( (float(local_velocity.twist.linear.y) - prev_vy) / delta_t) a_z.append( (float(local_velocity.twist.linear.z) - prev_vz) / delta_t) prev_vx = float(local_velocity.twist.linear.x) prev_vy = float(local_velocity.twist.linear.y) prev_vz = float(local_velocity.twist.linear.z) aplha_x.append( (float(imu_data.angular_velocity.x) - prev_omega_x) / delta_t) aplha_y.append( (float(imu_data.angular_velocity.y) - prev_omega_y) / delta_t) aplha_z.append( (float(imu_data.angular_velocity.z) - prev_omega_z) / delta_t) ax_f.append(float(imu_data.linear_acceleration.x)) ay_f.append(float(imu_data.linear_acceleration.y)) az_f.append(float(imu_data.linear_acceleration.z)) prev_omega_x = float(imu_data.angular_velocity.x) prev_omega_y = float(imu_data.angular_velocity.y) prev_omega_z = float(imu_data.angular_velocity.z) theta += 0.5 count += 1 local_pos_pub.publish(pose) print('data_points =', data_points, " count = ", count, "row =", row) if (count >= data_points): break prev_time = curr_time rate.sleep() nn1_yaw_input_state = np.array([ vx_f, vy_f, vz_f, rs_f, ps_f, ys_f, sin_r_f, sin_p_f, sin_y_f, cos_r_f, cos_p_f, cos_y_f, u3 ], ndmin=2).transpose() nn1_yaw_grd_truth = np.array([a_x, a_y, a_z], ndmin=2).transpose() nn2_yaw_input_state = np.array([ vx_f, vy_f, vz_f, rs_f, ps_f, ys_f, sin_r_f, sin_p_f, sin_y_f, cos_r_f, cos_p_f, cos_y_f, u0, u1, u2 ], ndmin=2).transpose() nn2_yaw_grd_truth = np.array([aplha_x, aplha_y, aplha_z], ndmin=2).transpose() print('nn1_x_rand_y_input_state :', nn1_yaw_input_state.shape) print('nn1_x_rand_y_grd_truth :', nn1_yaw_grd_truth.shape) print('nn2_x_rand_y_input_state :', nn2_yaw_input_state.shape) print('nn2_x_rand_y_grd_truth :', nn2_yaw_grd_truth.shape) np.save('nn1_x_rand_y_input_state.npy', nn1_yaw_input_state) np.save('nn1_x_rand_y_grd_truth.npy', nn1_yaw_grd_truth) np.save('nn2_x_rand_y_input_state.npy', nn2_yaw_input_state) np.save('nn2_x_rand_y_grd_truth.npy', nn2_yaw_grd_truth) s_x_rand_y = np.array([ x_f, y_f, z_f, vx_f, vy_f, vz_f, sin_r_f, sin_p_f, sin_y_f, cos_r_f, cos_p_f, cos_y_f, rs_f, ps_f, ys_f, r_f, p_f, yaw_f ], ndmin=2).transpose() u_x_rand_y = np.array([u0, u1, u2, u3], ndmin=2).transpose() a_x_rand_y = np.array([ax_f, ay_f, az_f], ndmin=2).transpose() print('s_x_rand_y :', s_x_rand_y.shape) print('u_x_rand_y :', u_x_rand_y.shape) print('a_x_rand_y :', a_x_rand_y.shape) np.save('s_x_rand_y.npy', s_x_rand_y) np.save('u_x_rand_y.npy', u_x_rand_y) np.save('a_x_rand_y.npy', a_x_rand_y)
from mavros_msgs.srv import SetModeRequest from mavros_msgs.srv import SetModeResponse from mavros_msgs.srv import CommandBool from mavros_msgs.srv import CommandBoolRequest from mavros_msgs.srv import CommandBoolResponse from sensor_msgs.msg import Imu import argparse parser = argparse.ArgumentParser() parser.add_argument("data_points", help="data_points", type=int) args = parser.parse_args() data_points = args.data_points current_state = State() imu_data = Imu() act_controls = ActuatorControl() PI = 3.14 k_p_yaw = 0.05 k_d_yaw = 0 k_p_pitch = 0.18 k_d_pitch = 0.045 k_p_roll = 0.18 k_d_roll = 0.045 x_f = [] y_f = [] z_f = []
# Prepare service calls #arm_cmd = CommandBool() #arm_cmd.request.value = True #mode_cmd = SetMode() #mode_cmd # Prepare dummy pose sp #send a few setpoints before starting #pList = 10*[PoseStamped()] pose_sp = PoseStamped() pose_sp.pose.position.x = 5 pose_sp.pose.position.y = 2 pose_sp.pose.position.z = 2 actuator_sp = ActuatorControl() actuator_sp.controls[5] = 0.5 actuator_sp.group_mix = ActuatorControl.PX4_MIX_MANUAL_PASSTHROUGH i = 50 while (not rospy.is_shutdown()) and i > 0: pub_pose_sp.publish(pose_sp) i = i - 1 rate.sleep() # Loop until ROS Shutdown last_request = time.time() while not rospy.is_shutdown(): #rospy.loginfo('state: ' + cur_state.mode) if (cur_state.mode != "OFFBOARD" and (time.time() - last_request > 1)): rospy.loginfo('attempting OFFBOARD')
def control(): # sp = np.load("xy_sin.npy") state_sub = rospy.Subscriber("/mavros/state", State, state_cb, queue_size=10) rospy.wait_for_service('mavros/cmd/arming') arming_client = rospy.ServiceProxy('mavros/cmd/arming', CommandBool) rospy.wait_for_service('mavros/set_mode') set_mode_client = rospy.ServiceProxy('mavros/set_mode', SetMode) acutator_control_pub = rospy.Publisher("/mavros/actuator_control", ActuatorControl, queue_size=10) local_pos_pub = rospy.Publisher("/mavros/setpoint_position/local", PoseStamped, queue_size=10) mocap_pos_pub = rospy.Publisher("/mavros/mocap/pose", PoseStamped, queue_size=10) imu_sub = rospy.Subscriber("/mavros/imu/data", Imu, imu_cb, queue_size=1) local_pos_sub = rospy.Subscriber("/mavros/local_position/pose", PoseStamped, lp_cb, queue_size=1) local_vel_sub = rospy.Subscriber("/mavros/local_position/velocity", TwistStamped, lv_cb, queue_size=1) act_control_sub = rospy.Subscriber("/mavros/act_control/act_control_pub", ActuatorControl, act_cb, queue_size=1) rospy.init_node('control', anonymous=True) rate = rospy.Rate(50.0) # print("*"*80) while not rospy.is_shutdown() and not current_state.connected: rate.sleep() rospy.loginfo(current_state.connected) # print("#"*80) pose = PoseStamped() # mocap_pose = PoseStamped() offb_set_mode = SetModeRequest() offb_set_mode.custom_mode = "OFFBOARD" arm_cmd = CommandBoolRequest() arm_cmd.value = True last_request = rospy.Time.now() i = 0 act = ActuatorControl() flag1 = False flag2 = False prev_imu_data = Imu() prev_time = rospy.Time.now() # prev_x = 0 # prev_y = 0 # prev_z = 0 # prev_vx = 0 # prev_vy = 0 # prev_vz = 0 prev_omega_x = 0 prev_omega_y = 0 prev_omega_z = 0 prev_vx = 0 prev_vy = 0 prev_vz = 0 delta_t = 0.02 count = 0 theta = 0.0 # rospy.loginfo("Outside") while not rospy.is_shutdown(): if current_state.mode != "OFFBOARD" and ( rospy.Time.now() - last_request > rospy.Duration(5.0)): offb_set_mode_response = set_mode_client(offb_set_mode) if offb_set_mode_response.mode_sent: rospy.loginfo("Offboard enabled") flag1 = True last_request = rospy.Time.now() else: if current_state.armed == False: arm_cmd_response = arming_client(arm_cmd) if arm_cmd_response.success: rospy.loginfo("Vehicle armed") flag2 = True last_request = rospy.Time.now() # rospy.loginfo("Inside") if flag1 and flag2: x_f.append(float(local_position.pose.position.x)) y_f.append(float(local_position.pose.position.y)) z_f.append(float(local_position.pose.position.z)) vx_f.append(float(local_velocity.twist.linear.x)) vy_f.append(float(local_velocity.twist.linear.y)) vz_f.append(float(local_velocity.twist.linear.z)) # print(local_velocity.twist.linear.x) orientation = [ imu_data.orientation.w, imu_data.orientation.x, imu_data.orientation.y, imu_data.orientation.z ] (roll, pitch, yaw) = quaternion_to_euler_angle( imu_data.orientation.w, imu_data.orientation.x, imu_data.orientation.y, imu_data.orientation.z) r_f.append(float(mt.radians(roll))) p_f.append(float(mt.radians(pitch))) yaw_f.append(float(mt.radians(yaw))) sin_r_f.append(mt.sin(float(mt.radians(roll)))) sin_p_f.append(mt.sin(float(mt.radians(pitch)))) sin_y_f.append(mt.sin(float(mt.radians(yaw)))) cos_r_f.append(mt.cos(float(mt.radians(roll)))) cos_p_f.append(mt.cos(float(mt.radians(pitch)))) cos_y_f.append(mt.cos(float(mt.radians(yaw)))) rs_f.append(float(imu_data.angular_velocity.x)) ps_f.append(float(imu_data.angular_velocity.y)) ys_f.append(float(imu_data.angular_velocity.z)) ix.append(float(ixx)) iy.append(float(iyy)) iz.append(float(izz)) m.append(float(mass)) u0.append(act_controls.controls[0]) u1.append(act_controls.controls[1]) u2.append(act_controls.controls[2]) u3.append(act_controls.controls[3]) pose.pose.position.x = theta pose.pose.position.z = 6 + 2 * mt.sin(theta * PI / 6) x_des.append(theta) y_des.append(0) z_des.append(6 + 2 * mt.sin(theta * PI / 6)) sin_y_des.append(yaw_des) cos_y_des.append(yaw_des) w_mag.append(0) w_x.append(0) w_y.append(0) w_z.append(0) a_x.append( (float(local_velocity.twist.linear.x) - prev_vx) / delta_t) a_y.append( (float(local_velocity.twist.linear.y) - prev_vy) / delta_t) a_z.append( (float(local_velocity.twist.linear.z) - prev_vz) / delta_t) prev_vx = float(local_velocity.twist.linear.x) prev_vy = float(local_velocity.twist.linear.y) prev_vz = float(local_velocity.twist.linear.z) aplha_x.append( (float(imu_data.angular_velocity.x) - prev_omega_x) / delta_t) aplha_y.append( (float(imu_data.angular_velocity.y) - prev_omega_y) / delta_t) aplha_z.append( (float(imu_data.angular_velocity.z) - prev_omega_z) / delta_t) prev_omega_x = float(imu_data.angular_velocity.x) prev_omega_y = float(imu_data.angular_velocity.y) prev_omega_z = float(imu_data.angular_velocity.z) theta += 1.0 / 80 count += 1 local_pos_pub.publish(pose) if (count >= data_points): break rate.sleep() nn1_xz_input_state = np.array([ vx_f, vy_f, vz_f, rs_f, ps_f, ys_f, sin_r_f, sin_p_f, sin_y_f, cos_r_f, cos_p_f, cos_y_f, u3 ], ndmin=2).transpose() nn1_xz_grd_truth = np.array([a_x, a_y, a_z], ndmin=2).transpose() nn2_xz_input_state = np.array([ vx_f, vy_f, vz_f, rs_f, ps_f, ys_f, sin_r_f, sin_p_f, sin_y_f, cos_r_f, cos_p_f, cos_y_f, u0, u1, u2 ], ndmin=2).transpose() nn2_xz_grd_truth = np.array([aplha_x, aplha_y, aplha_z], ndmin=2).transpose() print('nn1_xz_input_state :', nn1_xz_input_state.shape) print('nn1_xz_grd_truth :', nn1_xz_grd_truth.shape) print('nn2_xz_input_state :', nn2_xz_input_state.shape) print('nn2_xz_grd_truth :', nn2_xz_grd_truth.shape) np.save('nn1_80_sin_xz_input_state.npy', nn1_xz_input_state) np.save('nn1_80_sin_xz_grd_truth.npy', nn1_xz_grd_truth) np.save('nn2_80_sin_xz_input_state.npy', nn2_xz_input_state) np.save('nn2_80_sin_xz_grd_truth.npy', nn2_xz_grd_truth) s_xz_sin = np.array([ x_f, y_f, z_f, vx_f, vy_f, vz_f, sin_r_f, sin_p_f, sin_y_f, cos_r_f, cos_p_f, cos_y_f, rs_f, ps_f, ys_f ], ndmin=2).transpose() u_xz_sin = np.array([u0, u1, u2, u3], ndmin=2).transpose() print('s_xz_sin :', s_xz_sin.shape) print('u_xz_sin :', u_xz_sin.shape) np.save('s_80_xz_sin.npy', s_xz_sin) np.save('u_80_xz_sin.npy', u_xz_sin)
mavros_msgs.srv.CommandBool) armService(True) except rospy.ServiceException, e: print "Service arm call failed: %s" % e rospy.wait_for_service('/mavros/set_mode') try: flightModeService = rospy.ServiceProxy('/mavros/set_mode', mavros_msgs.srv.SetMode) #http://wiki.ros.org/mavros/CustomModes for custom modes isModeChanged = flightModeService( custom_mode='OFFBOARD') #return true or false except rospy.ServiceException, e: print "service set_mode call failed: %s. GUIDED Mode could not be set. Check that GPS is enabled" % e act = ActuatorControl() act.header = Header() act.controls[0] = 0.0 #ros_roll act.controls[1] = 0.0 #ros_pitch act.controls[2] = 0.0 + 0.2 #ros_yaw+offset act.controls[3] = 0.0 #ros_throttle act.controls[4] = 0.0 act.controls[5] = 0.0 act.controls[6] = 0.0 act.controls[7] = 0.0 while not rospy.is_shutdown(): pub_act.publish(act) act.controls[2] = 0.0 + 0.2 #ros_yaw+offset act.controls[3] = 0.5 #ros_throttle rate.sleep()
def __init__(self): ### define gym spaces ### self.min_action = 0.0 self.max_action = 1.0 self.min_position = 0.1 self.max_position = 25 self.max_speed = 3 self.low_state = np.array([self.min_position, -self.max_speed]) self.high_state = np.array([self.max_position, self.max_speed]) # self.low_state = np.array([self.min_position]) # self.high_state = np.array([self.max_position]) self.action_space = spaces.Box(low=self.min_action, high=self.max_action, shape=(1, ), dtype=np.float32) self.observation_space = spaces.Box(low=self.low_state, high=self.high_state, dtype=np.float32) self.current_state = State() self.imu_data = Imu() self.act_controls = ActuatorControl() self.pose = PoseStamped() self.mocap_pose = PoseStamped() self.thrust_ctrl = Thrust() self.attitude_target = AttitudeTarget() self.local_velocity = TwistStamped() self.global_velocity = TwistStamped() ### define ROS messages ### self.offb_set_mode = SetModeRequest() self.offb_set_mode.custom_mode = "OFFBOARD" self.arm_cmd = CommandBoolRequest() self.arm_cmd.value = True self.disarm_cmd = CommandBoolRequest() self.disarm_cmd.value = False ## ROS Subscribers self.state_sub = rospy.Subscriber("/mavros/state", State, self.state_cb, queue_size=10) self.imu_sub = rospy.Subscriber("/mavros/imu/data", Imu, self.imu_cb, queue_size=10) self.local_pos_sub = rospy.Subscriber("/mavros/local_position/pose", PoseStamped, self.lp_cb, queue_size=10) self.local_vel_sub = rospy.Subscriber( "/mavros/local_position/velocity_local", TwistStamped, self.lv_cb, queue_size=10) self.act_control_sub = rospy.Subscriber( "/mavros/act_control/act_control_pub", ActuatorControl, self.act_cb, queue_size=10) self.global_alt_sub = rospy.Subscriber( "/mavros/global_position/rel_alt", Float64, self.ra_cb, queue_size=10) self.global_pos_sub = rospy.Subscriber( "/mavros/global_position/gp_vel", TwistStamped, self.gv_cb, queue_size=10) ## ROS Publishers self.local_pos_pub = rospy.Publisher("/mavros/setpoint_position/local", PoseStamped, queue_size=10) self.mocap_pos_pub = rospy.Publisher("/mavros/mocap/pose", PoseStamped, queue_size=10) self.acutator_control_pub = rospy.Publisher("/mavros/actuator_control", ActuatorControl, queue_size=10) self.setpoint_raw_pub = rospy.Publisher( "/mavros/setpoint_raw/attitude", AttitudeTarget, queue_size=10) self.thrust_pub = rospy.Publisher("/mavros/setpoint_attitude/thrust", Thrust, queue_size=10) ## initiate gym enviornment self.init_env() ## ROS Services rospy.wait_for_service('mavros/cmd/arming') self.arming_client = rospy.ServiceProxy('mavros/cmd/arming', CommandBool) rospy.wait_for_service('mavros/set_mode') self.set_mode_client = rospy.ServiceProxy('mavros/set_mode', SetMode) rospy.wait_for_service('mavros/set_stream_rate') set_stream_rate = rospy.ServiceProxy("mavros/set_stream_rate", StreamRate) set_stream_rate(StreamRateRequest.STREAM_POSITION, 50, 1) set_stream_rate(StreamRateRequest.STREAM_ALL, 50, 1) self.setpoint_msg = mavros.setpoint.PoseStamped( header=mavros.setpoint.Header(frame_id="att_pose", stamp=rospy.Time.now()), ) self.offb_arm()
def __init__(self): self.wins = 0 self.bounds_counter = 1 self.z_limit = 3 self.x_limit = 3 self.y_limit = 3 self.ang_vel_limit = 3 self.ang_pos_limit = 0.4 ### define gym space ### self.min_action = np.array([0, -1, -1, -1]) self.max_action = np.array([1, 1, 1, 1]) self.min_lin_pos = np.array([0.1, -20, -20]) self.max_lin_pos = np.array([40, 20, 20]) self.min_lin_vel = 10 * np.array([-1, -1, -1]) self.max_lin_vel = 10 * np.array([1, 1, 1]) self.min_lin_accl = 30 * np.array([-1, -1, -1]) self.max_lin_accl = 30 * np.array([1, 1, 1]) self.min_ang_pos = np.array([-1.57, -1.57, -1.57]) self.max_ang_pos = np.array([1.57, 1.57, 1.57]) self.min_ang_vel = 10 * np.array([-1, -1, -1]) self.max_ang_vel = 10 * np.array([1, 1, 1]) # self.low_state = np.array([ self.min_lin_pos, self.min_lin_vel, self.min_lin_accl, self.min_ang_pos, self.min_ang_vel ]).flatten() # self.high_state = np.array([self.max_lin_pos, self.max_lin_vel, self.max_lin_accl, self.max_ang_pos, self.max_ang_vel ]).flatten() self.low_state = np.array([ self.min_lin_pos, self.min_lin_vel, self.min_lin_accl, self.min_ang_pos, self.min_ang_vel ]).flatten() self.high_state = np.array([ self.max_lin_pos, self.max_lin_vel, self.max_lin_accl, self.max_ang_pos, self.max_ang_vel ]).flatten() self.action_space = spaces.Box(low=self.min_action, high=self.max_action, dtype=np.float32) self.observation_space = spaces.Box(low=self.low_state, high=self.high_state, dtype=np.float32) self.current_state = State() self.imu_data = Imu() self.act_controls = ActuatorControl() self.pose = PoseStamped() self.mocap_pose = PoseStamped() self.thrust_ctrl = Thrust() self.attitude_target = AttitudeTarget() self.local_velocity = TwistStamped() self.global_velocity = TwistStamped() self.ground_truth = ModelStates() self.local_accel = Imu() # rospy.signal_shutdown('Resrating rospy') # subprocess.Popen(args='make px4_sitl gazebo', cwd='../Firmware', shell=True, start_new_session=True) # subprocess.Popen(args='roslaunch mavros px4.launch fcu_url:="udp://:[email protected]:14557"', cwd='../Firmware', shell=True, start_new_session=True) ### define ROS messages ### self.offb_set_mode = SetModeRequest() self.offb_set_mode.custom_mode = "OFFBOARD" self.arm_cmd = CommandBoolRequest() self.arm_cmd.value = True self.disarm_cmd = CommandBoolRequest() self.disarm_cmd.value = False ## ROS Subscribers self.state_sub = rospy.Subscriber("/mavros/state", State, self.state_cb, queue_size=10) self.imu_sub = rospy.Subscriber("/mavros/imu/data", Imu, self.imu_cb, queue_size=10) self.local_pos_sub = rospy.Subscriber("/mavros/local_position/pose", PoseStamped, self.lp_cb, queue_size=10) self.local_vel_sub = rospy.Subscriber( "/mavros/local_position/velocity_body", TwistStamped, self.lv_cb, queue_size=10) self.local_acc_sub = rospy.Subscriber("/mavros/imu/data", Imu, self.lacc_cb, queue_size=10) self.act_control_sub = rospy.Subscriber( "/mavros/act_control/act_control_pub", ActuatorControl, self.act_cb, queue_size=10) self.global_alt_sub = rospy.Subscriber( "/mavros/global_position/rel_alt", Float64, self.ra_cb, queue_size=10) self.global_pos_sub = rospy.Subscriber( "/mavros/global_position/gp_vel", TwistStamped, self.gv_cb, queue_size=10) self.ground_truth_sub = rospy.Subscriber("/gazebo/model_states", ModelStates, self.gt_cb, queue_size=10) ## ROS Publishers self.local_pos_pub = rospy.Publisher("/mavros/setpoint_position/local", PoseStamped, queue_size=10) self.mocap_pos_pub = rospy.Publisher("/mavros/mocap/pose", PoseStamped, queue_size=10) self.acutator_control_pub = rospy.Publisher("/mavros/actuator_control", ActuatorControl, queue_size=10) self.setpoint_raw_pub = rospy.Publisher( "/mavros/setpoint_raw/attitude", AttitudeTarget, queue_size=10) self.thrust_pub = rospy.Publisher("/mavros/setpoint_attitude/thrust", Thrust, queue_size=10) ## initiate gym enviornment self.init_env() ## ROS Services rospy.wait_for_service('mavros/cmd/arming') self.arming_client = rospy.ServiceProxy('mavros/cmd/arming', CommandBool) rospy.wait_for_service('mavros/set_mode') self.set_mode_client = rospy.ServiceProxy('mavros/set_mode', SetMode) rospy.wait_for_service('mavros/set_stream_rate') set_stream_rate = rospy.ServiceProxy("mavros/set_stream_rate", StreamRate) set_stream_rate(StreamRateRequest.STREAM_POSITION, 100, 1) set_stream_rate(StreamRateRequest.STREAM_ALL, 100, 1) self.setpoint_msg = mavros.setpoint.PoseStamped( header=mavros.setpoint.Header(frame_id="att_pose", stamp=rospy.Time.now()), ) self.offb_arm()