def thread_offboard(self): rospy.init_node('offboard', anonymous=True) self.uav_id = rospy.get_param("~id", "") rospy.loginfo(self.uav_id + ":start offboard control..") rate = rospy.Rate(20) rate.sleep() rospy.Subscriber(self.uav_id + "/cmd/thrust", Float32, self.thrustcallback) rospy.Subscriber(self.uav_id + "/cmd/orientation", Quaternion, self.quaternioncallback) rospy.Subscriber(self.uav_id + "/mavros/state", State, self.statecallback) rospy.Subscriber(self.uav_id + "/mavros/local_position/pose", PoseStamped, self.poscallback) rospy.Subscriber(self.uav_id + "/mavros/local_position/velocity", TwistStamped, self.velcallback) arming_client = rospy.ServiceProxy(self.uav_id + "/mavros/cmd/arming", CommandBool) pub = rospy.Publisher(self.uav_id + "/mavros/setpoint_raw/attitude", AttitudeTarget, queue_size=20) arm_cmd = CommandBoolRequest() arm_cmd.value = True set_mode_client = rospy.ServiceProxy(self.uav_id + "/mavros/set_mode", SetMode) offb_set_mode = SetModeRequest() offb_set_mode.base_mode = 0 offb_set_mode.custom_mode = "OFFBOARD" now = rospy.get_rostime() last_request = now.secs while not (rospy.is_shutdown()): if (self.targetattitude.thrust < -1): rospy.loginfo(" error 00") break pub.publish(self.targetattitude) if ((self.current_state.mode != "OFFBOARD") and (rospy.get_rostime().secs - last_request > 2)): rospy.loginfo(self.current_state.mode) if ((set_mode_client.call(offb_set_mode) == True) and (offb_set_mode.response.success == True)): rospy.loginfo(self.uav_id + " offboard enabled") last_request = rospy.get_rostime().secs else: if ((self.current_state.armed == False) and (rospy.get_rostime().secs - last_request > 2)): rospy.loginfo(self.uav_id + " arming...") if (arming_client.call(arm_cmd).success): rospy.loginfo(self.uav_id + " armed") last_request = rospy.get_rostime().secs rate.sleep()
def arm(self, do_arming): if self.current_state.armed and do_arming: pass else: # wait for service rospy.wait_for_service("mavros/cmd/arming") # service client set_arm = rospy.ServiceProxy("mavros/cmd/arming", CommandBool) # set request object req = CommandBoolRequest() req.value = do_arming # check response if do_arming: while not rospy.is_shutdown() and not self.current_state.armed: try: # request set_arm.call(req) except rospy.ServiceException, e: print "Service did not process request: %s" % str(e) else:
def arm(self, value=True): req = CommandBoolRequest() req.value = value if self.arm_client(req).success: rospy.loginfo('Arming/Disarming successful') return True else: rospy.logwarn('Arming/Disarming unsuccessful') return False
def arm(self, do_arming): if self.current_state.armed and do_arming: print("already armed") else: # wait for service rospy.wait_for_service("mavros/cmd/arming") # service client set_arm = rospy.ServiceProxy("mavros/cmd/arming", CommandBool) # set request object req = CommandBoolRequest() req.value = do_arming # zero time t0 = rospy.get_time() # check response if do_arming: while not rospy.is_shutdown() and not self.current_state.armed: if rospy.get_time() - t0 > 2.0: # check every 5 seconds try: # request set_arm.call(req) except rospy.ServiceException as e: print("Service did not process request: %s" % str(e)) t0 = rospy.get_time() print("armed: ", self.current_state.armed) else: while not rospy.is_shutdown() and self.current_state.armed: if rospy.get_time() - t0 > 0.5: # check every 5 seconds try: # request set_arm.call(req) except rospy.ServiceException as e: print("Service did not process request: %s" % str(e)) t0 = rospy.get_time()
def arm(self): """Arms the drone for takeoff. Returns: 0 (int): Arming successful. -1 (int): Arming unsuccessful. """ self.set_destination(0, 0, 0, 0) for _ in range(100): self.local_pos_pub.publish(self.waypoint_g) rospy.sleep(0.01) rospy.loginfo(CBLUE2 + "Arming Drone" + CEND) arm_request = CommandBoolRequest(True) while not rospy.is_shutdown() and not self.current_state_g.armed: rospy.sleep(0.1) response = self.arming_client(arm_request) self.local_pos_pub.publish(self.waypoint_g) else: if response.success: rospy.loginfo(CGREEN2 + "Arming successful" + CEND) return 0 else: rospy.logerr(CRED2 + "Arming failed" + CEND) return -1
def arm(self, val): if self.current_state.armed: rospy.loginfo('Already Armed!') else: rospy.wait_for_service('/mavros/cmd/arming') arm_client = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool) req = CommandBoolRequest() req.value = val t0 = rospy.get_rostime() while not rospy.is_shutdown() and (self.current_state.armed != val): if (rospy.get_rostime() - t0) > rospy.Duration(2.0): try: arm_client.call(req) except rospy.ServiceException, e: rospy.loginfo('Service did not process request: %s' % str(e)) t0 = rospy.get_rostime() rospy.loginfo('Armed: ' + str(val))
def LandDisArm(self): #Landing req = CommandBoolRequest() d_req = CommandBoolRequest() req.value = False d_req.value = True if self._current_pose.pose.position.z > 1: LandService = rospy.ServiceProxy('/mavros/cmd/land', CommandTOL) try: ret = LandService(min_pitch=0, yaw=0, latitude=self._current_gps.latitude,\ longitude=self._current_gps.longitude, altitude=self._current_pose.pose.position.z) if ret.success: rospy.loginfo("Took-off") else: rospy.loginfo("Failed taking-off") except rospy.ServiceException, e: print ("Service takeoff call failed") while not ret.success: rospy.logwarn("stuck in a loop!!!") self._rate.sleep() self._arming_client.call(d_req) ret = LandService(min_pitch=0, yaw=0, latitude=self._current_gps.latitude,\ longitude=self._current_gps.longitude, altitude=self._current_pose.pose.position.z) self._arming_client.call(req) if ret.success: rospy.loginfo("Landing initiated") else: rospy.loginfo("Failed to initiate landing")
def __init__(self): self.home_gps_pub = rospy.Publisher('/plane_home_gps', NavSatFix, queue_size=1) self.plane_local_pos_pub = rospy.Publisher( '/mavros/setpoint_position/local', PoseStamped, queue_size=1) self.start_trace_pub = rospy.Publisher('/start_trace', Bool, queue_size=1) self.change_attitude_pub = rospy.Publisher('/change_to_attitude', Bool, queue_size=1) self.plane_target_pos = PoseStamped() self.plane_curr = PoseStamped() self.car_pos = PoseStamped() self.init_hover_mode = True self.change_attitude = Bool() self.follow_mode = Bool() self.start_trace_msg = Bool() self.fail_safe_mode = False self.car_distance_x = 5 self.init_x = 0 self.init_y = 0 self.init_z = 3 self.destination_x = 0 self.destination_y = 0 self.delta_x = 0 self.delta_y = 0 self.points_num_x = 0 # need how many points to reach pre-position self.points_num_y = 0 # need how many points to reach pre-position self.point_x = 0 self.point_y = 0 self.step_x = 0 self.step_y = 0 self.traj_points_count = 0 # increase count in while loop to pub trajectory points self.waypoints_num = 0 self.waypoints_list_x = list() self.waypoints_list_y = list() self.plan_enable = True self.plan_num = 0 """>>>>>>test in gazebo """ self.uav_state = State() self.arm_cmd = CommandBoolRequest() rospy.Subscriber('/mavros/state', State, self.state_cb) self.arming_client = rospy.ServiceProxy("/mavros/cmd/arming", CommandBool) self.set_mode_client = rospy.ServiceProxy("/mavros/set_mode", SetMode) self.offb_set_mode = SetModeRequest() """<<<<<<test in gazebo """
def __init__(self, num): if num < 0 or num > 80: print "Error: initializing ", num, " drone. Bad number" self.shutdown_hook() #topics self.state_sub = rospy.Subscriber('/uav' + str(num) + '/mavros/state', State, self.state_cb) self.position_sub = rospy.Subscriber( '/uav' + str(num) + '/mavros/local_position/pose', PoseStamped, self.pose_cb) self.velocity_pub = rospy.Publisher( '/uav' + str(num) + '/mavros/setpoint_velocity/cmd_vel_unstamped', Twist, queue_size=1) self.position_pub = rospy.Publisher('uav' + str(num) + '/mavros/setpoint_position/local', PoseStamped, queue_size=1) self.arming_client = rospy.ServiceProxy( '/uav' + str(num) + '/mavros/cmd/arming', CommandBool) self.mode_client = rospy.ServiceProxy( '/uav' + str(num) + '/mavros/set_mode', SetMode) self.command_sub = rospy.Subscriber( '/uav' + str(num) + '/mavros/command', Int32MultiArray, self.cmd_cb) self.sensor_sub = rospy.Subscriber( '/uav' + str(num) + '/mavros/sensor', Float32MultiArray, self.sensor_cb) #variables self.velocity_obj = Twist() self.position_obj = PoseStamped() self.position_send_obj = PoseStamped() self.offb_srv_msg = SetModeRequest() self.arming_srv_msg = CommandBoolRequest() self.state = State() self.state.connected = False self.last_time = rospy.Time.now() self.rate = rospy.Rate(20) self.helmet_flag = False self.object_flag = False self.ctrl_c = False self.obstacle = False self.height = 2.5 #shutdown rospy.on_shutdown(self.shutdownhook)
def ArmTakeOff(self, arm, alt = 5): req = CommandBoolRequest() d_req = CommandBoolRequest() if self._current_state.armed and arm: rospy.loginfo("already armed") else: # wait for service rospy.wait_for_service("mavros/cmd/arming") # set request object req.value = arm d_req.value = not arm # zero time t0 = rospy.get_time() # check response while not rospy.is_shutdown() and not self._current_state.armed: if rospy.get_time() - t0 > 2.0: # check every 5 seconds try: # request self._arming_client.call(req) except rospy.ServiceException, e: print ("Service did not process request") t0 = rospy.get_time() rospy.logwarn("ARMED!!!")
def listener(): global current_state # 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) rospy.loginfo("start listenning") rate = rospy.Rate(100) rate.sleep() #rospy.Subscriber("/mavros/imu/data", Imu, callback) #rospy.Subscriber("/uav2/mavros/local_position/pose", PoseStamped, callback) rospy.Subscriber("/uav2/mavros/state", State, callback) arming_client = rospy.ServiceProxy("/uav2/mavros/cmd/arming", CommandBool) pub = rospy.Publisher("/uav2/mavros/setpoint_attitude/att_throttle", Float64, queue_size=10) pub1 = rospy.Publisher("/uav2/mavros/setpoint_attitude/attitude", PoseStamped, queue_size=10) #pub=rospy.Publisher("/uav2/mavros/setpoint_position/local",PoseStamped,queue_size=10) arm_cmd = CommandBoolRequest() arm_cmd.value = True pose = Float64() pose.data = 0.6 att = PoseStamped() att.pose.orientation.x = 0.5 att.pose.orientation.y = 0.0 att.pose.orientation.z = 0.0 att.pose.orientation.w = 0.866 ''' pose=PoseStamped() pose.pose.position.x=0 pose.pose.position.y=0 pose.pose.position.z=1 ''' rospy.loginfo("running") set_mode_client = rospy.ServiceProxy("/uav2/mavros/set_mode", SetMode) offb_set_mode = SetModeRequest() offb_set_mode.base_mode = 0 offb_set_mode.custom_mode = "OFFBOARD" now = rospy.get_rostime() last_request = now.secs while not (rospy.is_shutdown()): pub.publish(pose) pub1.publish(att) #rospy.loginfo(current_state.mode) if ((current_state.mode != "OFFBOARD") and (rospy.get_rostime().secs - last_request > 2)): rospy.loginfo(current_state.mode) rospy.loginfo("1") if (set_mode_client.call(offb_set_mode).success): #if(set_mode_client.call(offb_set_mode)==True and offb_set_mode.response.success==True): rospy.loginfo("offbrd enabled") last_request = rospy.get_rostime().secs else: if ((current_state.armed == False) and (rospy.get_rostime().secs - last_request > 2)): rospy.loginfo("2") if (arming_client.call(arm_cmd).success): rospy.loginfo("armed") last_request = rospy.get_rostime().secs rate.sleep()
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)
def uav_takeoff(self): rospy.init_node('takeoff', anonymous=True) rate = rospy.Rate(20.0) rate.sleep() rospy.Subscriber('mavros/state', State, self.state_cb) local_pose_pub = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=150) rospy.Subscriber('mavros/odometry/in',Odometry,self.odometry_cb) arming_client = rospy.ServiceProxy('mavros/cmd/arming', CommandBool) setmode_client = rospy.ServiceProxy('mavros/set_mode', SetMode) # rospy.spin() # print 1 # wait for FCU connected while (not rospy.is_shutdown()) and (not self.current_state.connected): rate.sleep() # print 2 # for i in range(100): # if not rospy.is_shutdown(): # local_pose_pub.publish(self.pose) # rate.sleep() # print i local_pose_pub.publish(self.pose) takeoff_set_mode = SetModeRequest() takeoff_set_mode.base_mode = 0 takeoff_set_mode.custom_mode = "OFFBOARD" arm_cmd = CommandBoolRequest() arm_cmd.value = True last_request = rospy.get_rostime() rospy.wait_for_service('mavros/set_mode') # print 3 # print self.current_state.connected # print self.current_state.mode while not rospy.is_shutdown(): if self.current_state.mode != "OFFBOARD" and (rospy.get_rostime() - last_request) > rospy.Duration(3): if setmode_client.call(takeoff_set_mode).mode_sent: rospy.loginfo("offboard enabled") last_request = rospy.get_rostime() print self.current_state.mode else: if (not self.current_state.armed) and (rospy.get_rostime()-last_request) > rospy.Duration(3): if arming_client.call(arm_cmd).success: rospy.loginfo("Vehicle armd") last_request = rospy.get_rostime() # tf axis has problem ,odo_x for setpoint_y and odo_y for setpoint_x error_x = abs(self.current_odometry.pose.pose.position.y - self.pose.pose.position.x) error_y = abs(self.current_odometry.pose.pose.position.x - self.pose.pose.position.y) error_z = abs(-self.current_odometry.pose.pose.position.z - self.pose.pose.position.z) error_sum = abs(error_x) + abs(error_y) + abs(error_z) if error_sum < 0.5: print "get point"+" "+str([self.pose.pose.position.x, self.pose.pose.position.y, self.pose.pose.position.z]) self.sequence += 1 self.get_waypoint(self.sequence, 5) self.set_newpoint(self.waypoint_x, self.waypoint_y, self.waypoint_z) for n in range(60): # delay 3.0s local_pose_pub.publish(self.pose) rate.sleep() elif error_sum >= 0.5: local_pose_pub.publish(self.pose) rate.sleep()
def disarm(self): arm_state = CommandBoolRequest() arm_state.value = False self.ws.call_service("mavros/cmd/arming", arm_state)
def px4_teleop_key(): rospy.init_node("px4_teleop_key_pub", anonymous=True) rospy.loginfo("Node Initialized") state_sub = rospy.Subscriber("mavros/state", State, state_cb, queue_size=10) pos_teleop_pub = rospy.Publisher("mavros/setpoint_position/local", PoseStamped, queue_size=10) arming_client = rospy.ServiceProxy("mavros/cmd/arming", CommandBool) set_mode_client = rospy.ServiceProxy("mavros/set_mode", SetMode) rospy.loginfo("Subscriber, Publisher and Service Clients are initilized") rate = rospy.Rate(20.0) while not rospy.is_shutdown() and current_state.connected: rate.sleep() pos_teleop_msg = PoseStamped() pos_teleop_msg.header.stamp = rospy.Time.now() pos_teleop_msg.pose.position.x = 0. pos_teleop_msg.pose.position.y = 0. pos_teleop_msg.pose.position.z = 2.0 for i in range(100): pos_teleop_pub.publish(pos_teleop_msg) rate.sleep() set_mode_req = SetModeRequest() set_mode_req.custom_mode = "OFFBOARD" arm_cmd_req = CommandBoolRequest() arm_cmd_req.value = True while not set_mode_client(set_mode_req).success: pass rospy.loginfo("Offboard enabled") while not arming_client(arm_cmd_req).success: pass rospy.loginfo("Vehicle armed") pos_teleop_pub.publish(pos_teleop_msg) try: stdscr = curses.initscr() curses.noecho() curses.cbreak() stdscr.keypad(1) show_key_config(stdscr) while not rospy.is_shutdown(): if not current_state.mode=="OFFBOARD": set_mode_client(set_mode_req) op = stdscr.getch() publish_pos(pos_teleop_pub, pos_teleop_msg, op) rate.sleep() finally: curses.nocbreak() stdscr.keypad(0) curses.echo() curses.endwin()
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 run(self): self.load_points() rospy.loginfo('Points are loaded...') rospy.Subscriber(self.prefix + '/mavros/state', State, self.state_cb) rospy.loginfo("Drone" + str(self.drone) + ": Subscribed to the state topic") rospy.Subscriber(self.prefix + '/mavros/local_position/pose', PoseStamped, self.check_local_position) local_pos_pub = rospy.Publisher(self.prefix + '/mavros/setpoint_position/local', PoseStamped, queue_size=10) rospy.wait_for_service(self.prefix + '/mavros/cmd/arming') arming = rospy.ServiceProxy(self.prefix + '/mavros/cmd/arming', CommandBool) rospy.wait_for_service(self.prefix + '/mavros/set_mode') set_mode = rospy.ServiceProxy(self.prefix + '/mavros/set_mode', SetMode) rate = rospy.Rate(20.0) while (not rospy.is_shutdown()) and (not self.mavros_state.connected): rospy.loginfo("Drone" + str(self.drone) + ": Waiting for a connection") rospy.sleep(1) for i in range(10): local_pos_pub.publish(self.desired_point) # rospy.loginfo(i) rate.sleep() offb_set_mode = SetModeRequest() offb_set_mode.custom_mode = "OFFBOARD" arm_cmd = CommandBoolRequest() arm_cmd.value = True last_request = rospy.Time().now() rospy.loginfo(last_request) while (not rospy.is_shutdown()) and (not self.finish_mission): if self.mavros_state.mode != 'OFFBOARD' and \ (rospy.Time().now() - last_request > rospy.Duration(5.0)): response = set_mode(offb_set_mode) if response.mode_sent: rospy.loginfo("Drone" + str(self.drone) + ": Offboard enabled") last_request = rospy.Time().now() else: if not self.mavros_state.armed and \ (rospy.Time().now() - last_request > rospy.Duration(5.0)): response = arming(arm_cmd) if response.success: rospy.loginfo("Drone" + str(self.drone) + ": Vehicle armed") last_request = rospy.Time().now() local_pos_pub.publish(self.desired_point) rate.sleep() rospy.loginfo("Drone" + str(self.drone) + ": mission finished") rospy.loginfo("Landing point is {}".format(self.land_point)) rospy.wait_for_service(self.prefix + "/mavros/cmd/land") land = rospy.ServiceProxy(self.prefix + "/mavros/cmd/land", CommandTOL) land(self.land_point) self.result_queue.put((self.drone, self.liability))
def offboard_node(): rospy.init_node("offb_node") r = rospy.Rate(20) rospy.Subscriber("mavros/state", State, state_cb) local_pos_pub = rospy.Publisher("mavros/setpoint_position/local", PoseStamped, queue_size=1000) arming_client = rospy.ServiceProxy("mavros/cmd/arming", CommandBool) set_mode_client = rospy.ServiceProxy("mavros/set_mode", SetMode) while not rospy.is_shutdown() and not current_state.connected: r.sleep() pose = PoseStamped() pose.pose.position.x = 0 pose.pose.position.y = 0 pose.pose.position.z = 2 for i in range(100): local_pos_pub.publish(pose) r.sleep() if rospy.is_shutdown(): break offb_set_mode = SetModeRequest() offb_set_mode.custom_mode = "OFFBOARD" arm_cmd = CommandBoolRequest() arm_cmd.value = True last_request = rospy.Time.now() while not rospy.is_shutdown(): if current_state.mode != "OFFBOARD" \ and (rospy.Time.now() - last_request > rospy.Duration(5)): try: offb_set_mode_resp = set_mode_client(offb_set_mode) if offb_set_mode_resp.mode_sent: rospy.loginfo("Offboard enabled") except rospy.ServiceException as e: rospy.logwarn(e) last_request = rospy.Time.now() else: if not current_state.armed \ and (rospy.Time.now() - last_request > rospy.Duration(5)): try: arm_cmd_resp = arming_client(arm_cmd) if arm_cmd_resp.success: rospy.loginfo("Vehicle armed") except rospy.ServiceException as e: rospy.logwarn(e) last_request = rospy.Time.now() local_pos_pub.publish(pose) r.sleep()
def run(self): parser = argparse.ArgumentParser( description='Swarm control', formatter_class=argparse.ArgumentDefaultsHelpFormatter) commands = ['arm', 'disarm', 'land', 'offboard', 'takeoff'] parser.add_argument('command', choices=commands, help='Command') parser.add_argument('agent', type=int, nargs='?', default=None, help='Agent ID') args = parser.parse_args() master = masterapi.Master('/') system_state = master.getSystemState() self.services = [service[0] for service in system_state[2]] self.topics = [t for t, _ in rospy.get_published_topics()] if args.command == 'arm': self.call_service('mavros/cmd/arming', service_class=CommandBool, request=CommandBoolRequest(True), agent=args.agent, function=self._are_agents_on_ground) elif args.command == 'disarm': self.call_service('mavros/cmd/arming', service_class=CommandBool, request=CommandBoolRequest(False), agent=args.agent, function=self._are_agents_on_ground) elif args.command == 'land': self.call_service('mavros/cmd/land', service_class=CommandTOL, request=CommandTOLRequest(), agent=args.agent) elif args.command == 'offboard': self.call_service('mavros/set_mode', service_class=SetMode, request=SetModeRequest(custom_mode='OFFBOARD'), agent=args.agent) elif args.command == 'takeoff': # Takeoff is a combination of takeoff and arming! n = rospy.get_param('/gcs/n') altitude = rospy.get_param('/gcs/origin_altitude') agents = [args.agent] if args.agent is not None else list( range(1, n + 1)) for agent in agents: template = '/drone_{}/mavros/home_position/home' home_position = rospy.wait_for_message(template.format(agent), HomePosition) request = CommandTOLRequest() request.latitude = home_position.geo.latitude request.longitude = home_position.geo.longitude request.altitude = altitude request.yaw = 0.5 * np.pi self.call_service('mavros/cmd/takeoff', service_class=CommandTOL, request=request, agent=agent) self.call_service('mavros/cmd/arming', service_class=CommandBool, request=CommandBoolRequest(True), agent=args.agent, function=self._are_agents_on_ground)
def uav_takeoff(self): rospy.init_node('takeoff', anonymous=True) rate = rospy.Rate(20.0) rate.sleep() rospy.Subscriber('mavros/state', State, self.state_cb) local_pose_pub = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=150) velocity_pub = rospy.Publisher( 'mavros/setpoint_velocity/cmd_vel_unstamped', Twist, queue_size=10) rospy.Subscriber('mavros/odometry/in', Odometry, self.odometry_cb) home_position_pub = rospy.Publisher('mavros/home_position/home', HomePosition, queue_size=10) arming_client = rospy.ServiceProxy('mavros/cmd/arming', CommandBool) setmode_client = rospy.ServiceProxy('mavros/set_mode', SetMode) # rospy.spin() # print 1 # wait for FCU connected while (not rospy.is_shutdown()) and (not self.current_state.connected): rate.sleep() print("state connected") # for i in range(100): # if not rospy.is_shutdown(): # local_pose_pub.publish(self.pose) # rate.sleep() # print i local_pose_pub.publish(self.pose) takeoff_set_mode = SetModeRequest() takeoff_set_mode.base_mode = 0 takeoff_set_mode.custom_mode = "OFFBOARD" arm_cmd = CommandBoolRequest() arm_cmd.value = True last_request = rospy.get_rostime() rospy.wait_for_service('mavros/set_mode') print("initialized") # print self.current_state.connected # print self.current_state.mode vel_mode = 0 self.vel = self.vel_change(vel_mode) flag = 0 while not rospy.is_shutdown(): print("in loop") if self.current_state.mode != "OFFBOARD" and ( rospy.get_rostime() - last_request) > rospy.Duration(3): if setmode_client.call(takeoff_set_mode).mode_sent: rospy.loginfo("offboard enabled") last_request = rospy.get_rostime() print self.current_state.mode else: if ( not self.current_state.armed ) and (rospy.get_rostime() - last_request) > rospy.Duration(3): if arming_client.call(arm_cmd).success: rospy.loginfo("Vehicle armd") last_request = rospy.get_rostime() if flag == 0: if abs(self.current_odometry.pose.pose.position.z + 1) > 0.1: local_pose_pub.publish(self.pose) rate.sleep() print(self.current_odometry.pose.pose.position.z) if abs(self.current_odometry.pose.pose.position.z + 1) < 0.2: flag = 1 if flag == 1: for n in range(60): # delay 3.0s velocity_pub.publish(self.vel) print(self.vel.linear.x) rate.sleep() vel_mode = vel_mode + 1 if vel_mode == 5: vel_mode = 1 self.vel = self.vel_change(vel_mode) # tf axis has problem ,odo_x for setpoint_y and odo_y for setpoint_x # error_x = abs(self.current_odometry.pose.pose.position.y - self.pose.pose.position.x) # error_y = abs(self.current_odometry.pose.pose.position.x - self.pose.pose.position.y) # error_z = abs(-self.current_odometry.pose.pose.position.z - self.pose.pose.position.z) # error_sum = abs(error_x) + abs(error_y) + abs(error_z) # # home_position_pub.publish(self.home_position) # # if error_sum < 0.5: # print "get point"+" "+str([self.pose.pose.position.x, self.pose.pose.position.y, self.pose.pose.position.z]) # # self.sequence += 1 # self.get_waypoint(self.sequence, 5) # self.set_newpoint(self.waypoint_x, self.waypoint_y, self.waypoint_z) # for n in range(60): # delay 3.0s # local_pose_pub.publish(self.pose) # rate.sleep() # elif error_sum >= 0.5: # local_pose_pub.publish(self.pose) rate.sleep()
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()
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)
"""pub local position for 100 counts""" local_pub_pos = PoseStamped() local_pub_pos.pose.position.x = 0 local_pub_pos.pose.position.y = 0 local_pub_pos.pose.position.z = 2 count = 0 while not (rospy.is_shutdown() or count > 50): pub_local_position.publish(local_pub_pos) print("pub local position 100 ") rate.sleep() count += 1 """change Mode""" arm_cmd = CommandBoolRequest() arm_cmd.value = True set_mode_client = rospy.ServiceProxy("/mavros/set_mode", SetMode) offb_set_mode = SetModeRequest() offb_set_mode.base_mode = 0 offb_set_mode.custom_mode = "OFFBOARD" now = rospy.get_rostime() last_request = now.secs while not (rospy.is_shutdown()): # pub_gps_geo.publish(gps_pub_geo_msg) # pub_gps_global.publish(gps_pub_msg) pub_local_position.publish(local_pub_pos) if((uav_state.mode != "OFFBOARD") and (rospy.get_rostime().secs-last_request > 2)): rospy.loginfo(uav_state.mode)
# Wait for FCU to connect to MAVROS while not rospy.is_shutdown() and not current_state.connected: rate.sleep() rospy.loginfo('FCU not connected') rospy.loginfo('Publishing initial waypoints') # Setpoints need to be streaming before commanding a switch to OFFBOARD Mode for i in range(50): if not rospy.is_shutdown(): pose_publisher.publish(PoseStamped()) rate.sleep() offb_mode_req = SetModeRequest() offb_mode_req.custom_mode = 'OFFBOARD' arm_req = CommandBoolRequest() arm_req.value = True arm_client(arm_req) last_req = rospy.Time.now() while not rospy.is_shutdown(): while not current_state.armed and current_state.mode != 'OFFBOARD': timer_cb(None) rate.sleep() rospy.Timer(rospy.Duration(5), timer_cb) rospy.loginfo('Taking off') takeoff(takeoff_client, 3) time_start = rospy.Time.now() while not rospy.is_shutdown() and rospy.Time.now() < time_start + rospy.Duration(3):
pose_sub = rospy.Subscriber('/uav0/mavros/local_position/pose', PoseStamped, pose_cb) pose_msg = PoseStamped() # velocity publisher velocity_pub = rospy.Publisher( '/uav0/mavros/setpoint_velocity/cmd_vel_unstamped', Twist, queue_size=1) velocity_msg = Twist() # set_mode service set_mode_srv = rospy.ServiceProxy('/uav0/mavros/set_mode', SetMode) set_mode_msg = SetModeRequest() # arm service arming_srv = rospy.ServiceProxy('/uav0/mavros/cmd/arming', CommandBool) arming_msg = CommandBoolRequest() # rest of initialization ctrl_c = False rospy.on_shutdown(shutdownhook) rate = rospy.Rate(20) ######## set up flight ######## # wait for connection to drone while not ctrl_c and not state_msg.connected: rate.sleep() # publish velocity thread velocity_thread = threading.Thread(target=velocity_publisher) velocity_thread.daemon = True