def mavros_arm_offboard_cb(self, timer): offb_set_mode = SetMode() offb_set_mode.custom_mode = "OFFBOARD" arm_cmd = CommandBool() arm_cmd.value = True if(self.mavros_state.mode != "OFFBOARD" and (rospy.Time.now() - self.last_mavros_request > rospy.Duration(5.0))): resp1 = self.set_mode_client(0,offb_set_mode.custom_mode) if resp1.mode_sent: #rospy.loginfo("Requested OFFBOARD") pass self.last_mavros_request = rospy.Time.now() elif (not self.mavros_state.armed and (rospy.Time.now() - self.last_mavros_request > rospy.Duration(5.0))): arm_client_1 = self.arming_client(arm_cmd.value) if arm_client_1.success: #rospy.loginfo("Requested Vehicle ARM") pass self.last_mavros_request = rospy.Time.now() armed = self.mavros_state.armed mode = self.mavros_state.mode rospy.loginfo("Vehicle armed: {}".format(armed)) rospy.loginfo("Vehicle mode: {}".format(mode)) if( (not armed ) or (mode != 'OFFBOARD')): pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.pose.position.x = 0 pose.pose.position.y = 0 pose.pose.position.z = 0 self.local_pos_pub.publish(pose) else: self.status_timer.shutdown()
def disarm(self): cmd = CommandBool() cmd.value = False self.arming_service_client = rospy.ServiceProxy( "uav" + str(self.id) + "/mavros/cmd/arming", CommandBool) while not self.arming_service_client(cmd.value).success is True: rospy.loginfo("UAV " + str(self.id) + " waiting to disarm") self.rate.sleep() rospy.loginfo("UAV " + str(self.id) + " DISARMED")
def arm(self): cmd = CommandBool() cmd.value = True self.arming_service_client = rospy.ServiceProxy( "uav" + str(self.id) + "/mavros/cmd/arming", CommandBool) while not self.arming_service_client(cmd.value).success is True: rospy.loginfo("UAV " + str(self.id) + " waiting to arm") if not self.currently_avoiding: for i in range(100): self.target_pos_pub.publish(self.desired_pos_object) self.rate.sleep() self.rate.sleep() rospy.loginfo("UAV " + str(self.id) + " ARMED")
def __init__(self, drone_id): # self.id = 0 self.id = drone_id self.id_state = 0 self.id_pub = rospy.Publisher("ID", Int32) self.arming_cmd = CommandBool() self.arming_cmd.value = True rospy.init_node("controller", anonymous=True) rospy.Subscriber("request_ID", Int32, self.get_id, queue_size=1) time.sleep(1) while self.id_state == 0: self.id_pub.publish(self.id_state) print("Waiting for ID") time.sleep(0.5) self.rate = rospy.Rate(20) print("ID received", self.id) rospy.Subscriber(str(self.id) + "/mavros/state", State, self.state_cb) rospy.Subscriber( str(self.id) + "/mavros/local_position/pose", PoseStamped, self.update_pose) self.set_mode_client = rospy.ServiceProxy( str(self.id) + "/mavros/set_mode", SetMode) self.local_pos_pub = rospy.Publisher(str(self.id) + "/mavros/setpoint_position/local", PoseStamped, queue_size=10) self.state = State() self.pose = PoseStamped() self.mode = self.state.mode self.target = PoseStamped() time.sleep(2)
def arm(self, value): self.arming_cmd = CommandBool() self.arming_cmd.value = value self.arming_client = rospy.ServiceProxy( str(self.id) + "/mavros/cmd/arming", CommandBool) while not self.arming_client(self.arming_cmd.value).success == True: self.rate.sleep() print("Arming/Disarming Completed")
class quad_obj: quad_pose = TransformStamped() quad_pose_x = TransformStamped() quad_pose_y = TransformStamped() quad_pose_z = TransformStamped() state = State() quad_goal = PoseStamped() offb_set_mode = SetMode() arm_cmd = CommandBool() takeoff_cmd = CommandTOL() landing_cmd = CommandTOL()
def return_land_disarm(self): # Return home print("Returning home.") rate = rospy.Rate(20) start_time = rospy.get_time() while ((rospy.get_time() - start_time) < 10.0): # give 20s to go back home self.command_pose = self.home_pose self.publish_command(self.command_pose) rate.sleep() # Land print("Landing.") self.landing_client(0.0, 0.0, 0.0, 0.0, 0.0) rospy.sleep(5) # disarm print("Disarm.") arm_cmd = CommandBool() arm_cmd.value = False arm_client_1 = self.arming_client(arm_cmd.value)
def start(self): rospy.init_node("offboard_node") rate = rospy.Rate(20) while (not self.mavros_state.connected): print(self.mavros_state.connected) rate.sleep() for i in range(10): self.vel_pub.publish(self.command) rate.sleep() self.offboard_state = SetMode() self.offboard_state.custom_mode = "OFFBOARD" self.arm_state = CommandBool() self.arm_state.value = True start_time = rospy.Time.now() ''' main ROS thread ''' cnt = -1 while (rospy.is_shutdown() is False): cnt += 1 self.record_stast() self.drone_state_pub.publish(self.drone_state) if self.is_offboard == False: if self.mavros_state.mode == "OFFBOARD": resp1 = self.flightModeService(0, "POSCTL") if cnt % 10 == 0: print("Enter MANUAL mode") rate.sleep() # self.start_height = self.mav_pos[2] continue else: if self.mavros_state.mode != "OFFBOARD": resp1 = self.flightModeService( 0, self.offboard_state.custom_mode) if resp1.mode_sent: print("Offboard enabled") start_time = rospy.Time.now() self.command.twist.linear.x = self.cmd_vel[0] self.command.twist.linear.y = self.cmd_vel[1] self.command.twist.linear.z = self.cmd_vel[2] self.command.twist.angular.z = self.cmd_yaw self.vel_pub.publish(self.command) rate.sleep()
class quad_obj: vehicle = Point() float_x = float() float_y = float() float_z = float() vehicle_dot = Vector3() float_xdot = float() float_ydot = float() float_zdot = float() state = State() quad_goal = PoseStamped() offb_set_mode = SetMode() arm_cmd = CommandBool() takeoff_cmd = CommandTOL() landing_cmd = CommandTOL()
def __init__(self): # Define state variable self.current_state = State() self.offb_set_mode = SetMode() self.arm_cmd = CommandBool() # Start node rospy.init_node('offb_node_python', anonymous = True) # Subscribe/Publish to relavant channels self.state_sub = rospy.Subscriber('mavros/state', State, state_cb, queue_size=10) self.local_pos_pub = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size = 10) # Connect to arming client rospy.wait_for_service('mavros/cmd/arming') try: self.arming_client = rospy.ServiceProxy('mavros/cmd/arming', CommandBool) except rospy.ServiceException, e: rospy.logerr("Service Call failed: %s", e)
print("Creating pose") pose = PoseStamped() #set position here pose.pose.position.x = 0 pose.pose.position.y = 0 pose.pose.position.z = 3 for i in range(100): local_pos_pub.publish(pose) rate.sleep() print("Creating Objects for services") offb_set_mode = SetMode() offb_set_mode.custom_mode = "OFFBOARD" arm_cmd = CommandBool() arm_cmd.value = True num = 0 land = 0 last_request = rospy.Time.now() while not rospy.is_shutdown(): #print(current_state) if land == 0: if (current_state.mode != "OFFBOARD" and (rospy.Time.now() - last_request > rospy.Duration(5.0))): resp1 = set_mode_client(0, offb_set_mode.custom_mode) if resp1.mode_sent: print("Offboard enabled") last_request = rospy.Time.now() elif (not current_state.armed
def start(self): rospy.init_node("offboard_node") rate = rospy.Rate(20) while (not self.mavros_state.connected): print(self.mavros_state.connected) rate.sleep() for i in range(10): self.vel_pub.publish(self.command) self.drone_state.data == self.task_takeoff self.drone_state_pub.publish(self.drone_state) rate.sleep() self.offboard_state = SetMode() self.offboard_state.custom_mode = "OFFBOARD" self.arm_state = CommandBool() self.arm_state.value = True start_time = rospy.Time.now() ''' main ROS thread ''' self.drone_state.data == self.task_takeoff cnt = -1 while (rospy.is_shutdown() is False): cnt += 1 # self.record_stast() # self.drone_state_pub.publish(self.drone_state) print("self.mav_yaw: {}".format(self.mav_yaw)) self.state_pub() if self.is_offboard == False: if self.mavros_state.mode == "OFFBOARD": resp1 = self.flightModeService(0, "POSCTL") if cnt % 10 == 0: print("Enter MANUAL mode") rate.sleep() # self.start_height = self.mav_pos[2] continue else: if self.mavros_state.mode != "OFFBOARD": resp1 = self.flightModeService( 0, self.offboard_state.custom_mode) if resp1.mode_sent: print("Offboard enabled") start_time = rospy.Time.now() cmd_control = self.state_control(self.drone_state.data) self.cmd_vel = np.array( [cmd_control[0], cmd_control[1], cmd_control[2]]) self.cmd_yaw = cmd_control[3] if self.is_start == True: self.cmd_vel = np.array([0, 0, 0]) self.cmd_yaw = 0 if (self.drone_state.data == self.task_takeoff or self.drone_state.data == self.task_in_search or self.drone_state.data == self.task_recognize_target or self.drone_state.data == self.task_attack or self.drone_state.data == self.task_finish or self.drone_state.data == 0): self.command.twist.linear.x = self.cmd_vel[0] self.command.twist.linear.y = self.cmd_vel[1] self.command.twist.linear.z = self.cmd_vel[2] self.command.twist.angular.z = self.cmd_yaw self.vel_pub.publish(self.command) # self.command.twist.linear.x = self.cmd_vel[0] # self.command.twist.linear.y = self.cmd_vel[1] # self.command.twist.linear.z = self.cmd_vel[2] # self.command.twist.angular.z = self.cmd_yaw # self.vel_pub.publish(self.command) rate.sleep()
def start(self): rospy.init_node("offboard_node") rate = rospy.Rate(20) obj_pub = rospy.Publisher( "ue4_ros/obj", Obj, queue_size=10) # type_pub = rospy.Publisher( # "ue4_ros/obj/vehicle_type", UInt64, queue_size=10) # id_pub = rospy.Publisher( # "ue4_ros/obj/vehicle_id", UInt64, queue_size=10) obj_msg = Obj() # type_msg = UInt64() # id_msg = UInt64() while(not self.mavros_state.connected): print(self.mavros_state.connected) rate.sleep() for i in range(10): self.vel_pub.publish(self.command) self.drone_state.data == self.task_takeoff self.drone_state_pub.publish(self.drone_state) rate.sleep() self.offboard_state = SetMode() self.offboard_state.custom_mode = "OFFBOARD" self.arm_state = CommandBool() self.arm_state.value = True start_time = rospy.Time.now() ''' main ROS thread ''' self.drone_state.data == self.task_takeoff cnt = -1 obj_msg.position.x = 0 obj_msg.position.y = 3 obj_msg.position.z = 2 obj_msg.angule.x = 0 obj_msg.angule.y = 0 obj_msg.angule.z = 0 obj_msg.size.x = 5 obj_msg.size.y = 5 obj_msg.size.z = 1 obj_msg.type = 24 obj_msg.id = 100 while (rospy.is_shutdown() is False): cnt += 1 # self.record_stast() # self.drone_state_pub.publish(self.drone_state) print("self.mav_yaw: {}".format(self.mav_yaw)) self.state_pub() if self.is_offboard == False: if self.mavros_state.mode == "OFFBOARD": resp1 = self.flightModeService(0, "POSCTL") if cnt % 10 == 0: print("Enter MANUAL mode") rate.sleep() # start_yaw = self.mav_yaw continue else: if self.mavros_state.mode != "OFFBOARD": resp1 = self.flightModeService( 0, self.offboard_state.custom_mode) if resp1.mode_sent: print("Offboard enabled") start_time = rospy.Time.now() # cmd_control = self.state_control(self.drone_state.data) # self.cmd_vel = np.array( # [cmd_control[0], cmd_control[1], cmd_control[2]]) # self.cmd_yaw = cmd_control[3] self.cmd_vel = np.array([0, 0, 0]) des_pos = np.array([10, 0, 3]) self.cmd_yaw = 0.1 if self.is_start == True: self.cmd_vel = self.pos_control( des_pos, self.mav_pos, self.P_pos, 1) # self.cmd_vel = np.array([0, 0, 0]) # self.cmd_vel[0] = 1 self.cmd_yaw = 0.1 obj_msg.position.x = obj_msg.position.x + 0.05 # obj_msg.angule.x = obj_msg.angule.x + 0.05 # if (self.drone_state.data == self.task_takeoff or self.drone_state.data == self.task_in_search or # self.drone_state.data == self.task_recognize_target or self.drone_state.data == self.task_attack or # self.drone_state.data == self.task_finish or self.drone_state.data == 0): # self.command.twist.linear.x = self.cmd_vel[0] # self.command.twist.linear.y = self.cmd_vel[1] # self.command.twist.linear.z = self.cmd_vel[2] # self.command.twist.angular.z = self.cmd_yaw # self.vel_pub.publish(self.command) # print("mav_pos: {}".format(self.mav_pos)) self.command.twist.linear.x = self.cmd_vel[0] self.command.twist.linear.y = self.cmd_vel[1] self.command.twist.linear.z = self.cmd_vel[2] self.vel_pub.publish(self.command) obj_pub.publish(obj_msg) # type_pub.publish(type_msg) # id_pub.publish(id_msg) rate.sleep()
def call_arming(self): rospy.loginfo_once('Arming...') msg = CommandBool() resp = self.service_arming.call(True) return resp.success
def __init__(self): rospy.init_node('sim_tello_interface_node', anonymous=True) #无人机状态 self.current_state_ = State() self.flight_state_ = self.FlightState.WAITING self.mavstateSub_ = rospy.Subscriber('/mavros/state', State, self.mavstateCallback) self.gazeboPoseSub_ = rospy.Subscriber( '/gazebo/model_states', ModelStates, self.gazeboPoseCallback ) #gazebo world中各种模型的状态,包括name,pose={position(x,y,z)+orition(x,y,z,w)}和twist={linear(x,y,z)+angular(x,y,z)} #无人机在世界坐标系下的位姿 self.R_wu_ = R.from_quat([0, 0, 0, 1]) self.t_wu_ = np.zeros([3], dtype='float') #无人机的初始位姿,在第一次收到相关topic后初始化,用于进行Gazebo世界坐标与MAVROS本地坐标的转换 self.t_init_ = np.zeros([3], dtype='float') self.yaw_init_ = 0.0 #角度制 self.R_init_ = R.from_quat([0, 0, 0, 1]) self.is_traj_local_init_ = False #封装实现tello控制函数 self.cmdTello_ = rospy.Subscriber('/tello/cmd_string', String, self.cmdUpdate) self.cmdBuffer = None #接收指令最小间隔/s self.cmd_interval = 0.5 self.last_cmd_ = rospy.Time.now() #无人机控制噪声 self.ctrl_noise_std = 10 self.truncation = 20 #定时发送控制命令(至少2Hz) self.pub_interval_ = rospy.Duration(0.02) self.publishloop_timer_ = rospy.Timer(self.pub_interval_, self.publishloopCallback) #设置无人机状态 self.arming_client_ = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool) self.land_client_ = rospy.ServiceProxy('/mavros/cmd/land', CommandBool) self.set_mode_client_ = rospy.ServiceProxy('/mavros/set_mode', SetMode) #用于发送起降命令和追踪飞行状态 self.last_request_ = rospy.Time.now() self.offb_set_mode_ = SetMode() self.arm_cmd_ = CommandBool() #目标位置信息 self.mutex_target_pose = Lock() self.uav_target_pose_local_ = PoseStamped() self.uav_target_pose_local_.header.seq = 1 self.uav_target_pose_local_.header.frame_id = 'map' self.uav_target_pose_local_.header.stamp = rospy.Time.now() self.uav_target_pose_local_.pose.position.x = 0 self.uav_target_pose_local_.pose.position.y = 0 self.uav_target_pose_local_.pose.position.z = 1.5 self.uav_target_pose_local_.pose.orientation.x = 0 self.uav_target_pose_local_.pose.orientation.y = 0 self.uav_target_pose_local_.pose.orientation.z = 0 self.uav_target_pose_local_.pose.orientation.w = 1 #当前姿态信息 self.current_pose_ = PoseStamped() #世界坐标 self.current_pose_.header.seq = 1 self.current_pose_.header.frame_id = 'map' self.current_pose_.pose.orientation.w = 1 #发布目标位置 self.uav_target_pose_local_pub_ = rospy.Publisher( '/mavros/setpoint_position/local', PoseStamped, queue_size=100) #在切换到offboard模式之前,必须先发送一些期望点信息到飞控中。 不然飞控会拒绝切换到offboard模式。 rate = rospy.Rate(20) for i in range(10): self.uav_target_pose_local_pub_.publish( self.uav_target_pose_local_) rate.sleep() try: rospy.spin() except KeyboardInterrupt: pass