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 _handle_control_mode(self, mode): """ Handles the "set_contol_mode" service. When called, places the FCU into manual flight mode and clears the last accel, vel, and pos setpoints. """ res = SetControlModeResponse() res.success = False if not self._ready: return res self._mode = mode.mode req = SetModeRequest() req.base_mode = 0 #req.custom_mode = "19" # MANUAL FLIGHT MODE req.custom_mode = "0" # STABILIZE FLIGHT MODE # req.custom_mode = "2" # ALT_HOLD FLIGHT MODE self._configure_mavros.wait_for_service() res.success = self._configure_mavros(req).mode_sent self._control_depth = True self._control_sway = True self._control_yaw = True self._latest_accel_sp = None self._latest_vel_sp = None self._latest_pos_sp = None self._latest_ap_sp = None self._latest_wp = None return res
def arm(self): # wait for connect while not rospy.is_shutdown() and self.current_state == None: rospy.loginfo("waiting for connection") self.rate.sleep() # must be streaming points before allowed to switch to offboard pose = PoseStamped() pose.pose.position.x = 0 pose.pose.position.y = 0 pose.pose.position.z = self.des_z for i in range(100): self.local_pose_pub.publish(pose) self.rate.sleep() # change to offboard mode and arm last_request = rospy.get_time() # enable offboard mode set_mode = rospy.ServiceProxy("%s/mavros/set_mode" % uav, SetMode) req = SetModeRequest() req.custom_mode = "OFFBOARD" while not rospy.is_shutdown() and (self.current_state.mode != req.custom_mode): self.local_pose_pub.publish(pose) if rospy.get_time() - last_request > 5.0: # check every 5 seconds try: set_mode.call(req) except rospy.ServiceException, e: print "Service did not process request: %s"%str(e) last_request = rospy.get_time() self.rate.sleep()
def set_mode(self, mode): if not self.current_state.connected: print "No FCU connection" elif self.current_state.mode == mode: pass else: # Request mode change with ros service rospy.wait_for_service("mavros/set_mode") set_mode = rospy.ServiceProxy("mavros/set_mode", SetMode) req = SetModeRequest() req.custom_mode = mode while not rospy.is_shutdown() and (self.current_state.mode != req.custom_mode): try: # request set_mode.call(req) except rospy.ServiceException, e: print "Service did not process request: %s" % str(e) else: return True
def set_mode(self, mode): if not self.current_state.connected: rospy.loginfo('Not Connected') elif self.current_state.mode != mode: rospy.wait_for_service('/mavros/set_mode') set_mode_client = rospy.ServiceProxy('/mavros/set_mode', SetMode) req = SetModeRequest() req.custom_mode = mode pub = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10) pose = PoseStamped() pose.pose.position.x = 0 pose.pose.position.y = 0 pose.pose.position.z = 0 t0 = rospy.get_rostime() + rospy.Duration(3.0) while not rospy.is_shutdown() and (self.current_state.mode != mode): if rospy.get_rostime() - t0 > rospy.Duration(2.0): try: rospy.loginfo('Sending') set_mode_client.call(req) except rospy.ServiceException, e: rospy.loginfo('Service did not process request: %s' % str(e)) t0 = rospy.get_rostime() pub.publish(pose) self.rate.sleep() rospy.loginfo(self.current_state.mode + ' Mode Esablished')
def request_mode(self, mode='OFFBOARD'): rospy.sleep(2) rospy.loginfo('Current mode: %s', self.state.mode) req = SetModeRequest() req.custom_mode = mode if self.mode_client(req).mode_sent: rospy.loginfo('Mode change request successful') return True else: rospy.logwarn('Mode change request unsuccessful') return False
def set_mode(self, new_mode): """ Функция отправки нового режима работы автопилота. @param new_mode: новый режим работы атопилота @type new_mode: String """ print("set mode") mode = SetModeRequest() mode.custom_mode = new_mode # if self.diagnostics.mode != new_mode: self.ws.call_service("mavros/set_mode", mode)
def main(): rospy.init_node('px4_offboard_python', anonymous=True) #state_sub = rospy.Subscriber("mavros/state", ) #while not rospy.is_shutdown() and not current_state.connected: # rospy.Rate(20) #rospy.loginfo('FCU connection successful') #rospy.Subscriber("chatter", String, callback) pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=10) #local_pos_pub = get_pub_position_local(queue_size=10) #arming_client = rospy.ServiceProxy("/mavros/arming", CommandBool) set_mode = rospy.ServiceProxy("/mavros/set_mode", SetMode) rate = rospy.Rate(20) msg = PoseStamped() msg.header = Header() msg.header.stamp = rospy.Time.now() msg.pose.position.x = 0 msg.pose.position.y = 0 msg.pose.position.z = 0.5 #for i in range(100): # local_pos_pub.Publish(msg) # rate.sleep() #offb_set_mode = SetMode() #offb_set_mode.request.custom_mode('OFFBOARD') #rospy.loginfo("Drone offboard!") req = SetModeRequest() req.custom_mode = 'OFFBOARD' set_mode.call(req) print 'mode: ' + State().mode mavros.set_namespace() command.arming(True) rospy.loginfo("Drone armed!") rospy.spin()
def set_mode(self, mode): if not self.current_state.connected: print "No FCU connection" elif self.current_state.mode == mode: print "Already in " + mode + " mode" else: # wait for service rospy.wait_for_service("mavros/set_mode") # service client set_mode = rospy.ServiceProxy("mavros/set_mode", SetMode) # set request object req = SetModeRequest() req.custom_mode = mode # zero time t0 = rospy.get_time() # check response while not rospy.is_shutdown() and (self.current_state.mode != req.custom_mode): if rospy.get_time() - t0 > 2.0: # check every 5 seconds try: # request set_mode.call(req) except rospy.ServiceException, e: print "Service did not process request: %s"%str(e) t0 = rospy.get_time() print "Mode: "+self.current_state.mode + " established"
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 execute(self, userdata): # This method is called periodically while the state is active. # Main purpose is to check state conditions and trigger a corresponding outcome. # If no outcome is returned, the state will stay active. if self._srv_type == SetMode: Logger.loginfo("In the if statement") Logger.loginfo("Executing The service %s and sending " % self._topic) self.service_client.call(self._topic, SetModeRequest(0, self._target)) else: Logger.loginfo("Executing The service %s and sending " % self._topic) self.service_client.call(self._topic, self._target) return 'Done' # One of the outcomes declared above.
def set_mode(self, mode): """This function changes the mode of the drone to a user specified mode. This takes the mode as a string. Ex. set_mode("GUIDED"). Args: mode (String): Can be set to modes given in https://ardupilot.org/copter/docs/flight-modes.html Returns: 0 (int): Mode Set successful. -1 (int): Mode Set unsuccessful. """ SetMode_srv = SetModeRequest(0, mode) response = self.set_mode_client(SetMode_srv) if response.mode_sent: rospy.loginfo(CGREEN2 + "SetMode Was successful" + CEND) return 0 else: rospy.logerr(CRED2 + "SetMode has failed" + CEND) return -1
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()
state_sub = rospy.Subscriber('/uav0/mavros/state', State, state_cb) state_msg = State() # local position subscriber 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()
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 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 _start_mission(self): print(self.namespace, 'waypoints') req = WaypointPushRequest() wp1 = Waypoint() wp1.frame = Waypoint.FRAME_GLOBAL wp1.command = CommandCode.NAV_TAKEOFF wp1.is_current = True wp1.autocontinue = True wp1.param1 = 0.3 # minimum / desired pitch wp1.param2 = 0.0 wp1.param3 = 0.0 wp1.param4 = 0.0 if self.color == 'blue' else -math.pi # yaw angle wp1.x_lat = self.start_position.latitude wp1.y_long = self.start_position.longitude wp1.z_alt = self.start_position.altitude + ALTITUDE_ABOVE_GROUND wp2 = copy.deepcopy(wp1) wp2.command = CommandCode.NAV_WAYPOINT wp2.is_current = False wp2.param1 = 0.0 wp2.param2 = 5.0 # acceptance radius wp2.param4 = 0.0 wp2.x_lat, wp2.y_long = cube_to_global(400.0, 250.0) wp3 = copy.deepcopy(wp2) wp3.autocontinue = False wp3.x_lat, wp3.y_long = cube_to_global(100.0, 250.0) req.waypoints = [wp1, wp2, wp3] service_name = '%s/mavros/mission/push' % self.namespace rospy.wait_for_service(service_name) try: service = rospy.ServiceProxy(service_name, WaypointPush) resp = service.call(req) except rospy.ServiceException as e: print(self.namespace, 'service call to push waypoints failed:', str(e), file=sys.stderr) return False if not resp.success: print(self.namespace, 'failed to push waypoints', file=sys.stderr) return False print(self.namespace, 'pushed waypoints') print(self.namespace, 'start mission') req = SetModeRequest() req.base_mode = 0 # use custom mode # first 0x04: mode auto # second 0x04: mission # third and fourth 0x0000: empty req.custom_mode = str(int('0x04040000', 0)) service_name = '%s/mavros/set_mode' % self.namespace rospy.wait_for_service(service_name) try: service = rospy.ServiceProxy(service_name, SetMode) resp = service.call(req) except rospy.ServiceException as e: print(self.namespace, 'service call to set mode failed:', str(e), file=sys.stderr) return False if not resp.success: print(self.namespace, 'failed to set mode', file=sys.stderr) return False print(self.namespace, 'started mission') self._set_state(STATE_MISSION) return True
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()
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) if((set_mode_client.call(offb_set_mode) == True) and (offb_set_mode.response.mode_sent == True)): rospy.loginfo(" offboard enabled") last_request = rospy.get_rostime().secs
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 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 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 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 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)
rate = rospy.Rate(20) # 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)
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)