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 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.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 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 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()