def hover_rec(self, time_flying): recursions = self.calculate_recursions(time_flying) print(recursions) for i in range(recursions): print(i) print(self.down_sensor_distance) if self.beh_type == 'HOVER' and (self.hover_sensor_altitude_max >= self.down_sensor_distance >= self.hover_sensor_altitude_min): print("The drone is hovering") self.beh_type = 'HOVER' target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.orientation = self.imu.orientation target_raw_attitude.body_rate.x = 0 # ROLL_RATE target_raw_attitude.body_rate.y = 0 # PITCH_RATE target_raw_attitude.body_rate.z = 0 # YAW_RATE thrust = self.hover_thrust target_raw_attitude.thrust = thrust self.attitude_target_pub.publish(target_raw_attitude) time.sleep(self.Time_between_messages ) #was 0.005 (now 50hz ,500loops) elif self.beh_type == 'HOVER' and ( self.hover_sensor_altitude_max <= self.down_sensor_distance): # We have to go down print("Recovering hover position - going down") self.beh_type = 'HOVER' target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.orientation = self.imu.orientation target_raw_attitude.body_rate.x = 0 # ROLL_RATE target_raw_attitude.body_rate.y = 0 # PITCH_RATE target_raw_attitude.body_rate.z = 0 # YAW_RATE thrust = self.hover_thrust target_raw_attitude.thrust = thrust - self.Deaccumulating_thrust self.attitude_target_pub.publish(target_raw_attitude) time.sleep(self.Time_between_messages ) # was 0.005 (now 50hz ,500 loops ,5sec) elif self.beh_type == 'HOVER' and ( self.down_sensor_distance <= self.hover_sensor_altitude_min): # We have to go up print("Recovering hover position - going up") self.beh_type = 'HOVER' target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.orientation = self.imu.orientation target_raw_attitude.body_rate.x = 0 # ROLL_RATE target_raw_attitude.body_rate.y = 0 # PITCH_RATE target_raw_attitude.body_rate.z = 0 # YAW_RATE thrust = self.hover_thrust target_raw_attitude.thrust = thrust + self.Deaccumulating_thrust self.attitude_target_pub.publish(target_raw_attitude) time.sleep(self.Time_between_messages ) # was 0.005 (now 50hz ,500 loops ,5sec) print("time of hovering has ended") self.beh_type = "LANDING" self.landing_rec()
def set_att_thrust(self, att, att_type, thrust, freq): """ Offboard method that sends attitude and thrust references to the PX4 autopilot of the the vehicle. Makes convertions from the local NED frame adopted for the ISR Flying Arena to the local ENU frame used by ROS. Converts the thrust from newtons to a normalized value between 0 and 1 through mathematical expression of the thrust curve of the vehicle. Parameters ---------- att : np.array of floats with shape (3,1) or with shape (4,1) Desired attitude for the vehicle, expressed in euler angles or in a quaternion. att_type : str Must be equal to either 'euler' or 'quaternion'. Specifies the format of the desired attitude. thrust : float Desired thrust value in newtons. freq : float Topic publishing frequency, in Hz. """ pub = rospy.Publisher(self.info.drone_ns + '/mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=1) msg = AttitudeTarget() msg.header = Header() msg.header.stamp = rospy.Time.now() if att_type == 'euler': msg.orientation = Quaternion(*quaternion_from_euler( *ned_to_enu(att))) elif att_type == 'quaternion': msg.orientation = Quaternion(*SI_quaternion_to_ROS_quaternion(att)) msg.thrust = normalize_thrust(thrust, self.info.thrust_curve, norm(self.ekf.vel)) pub.publish(msg) rate = rospy.Rate(freq) rate.sleep()
def secure_landing_phase_rec(self): # Secure landing part - last cm self.beh_type = "LANDING" recursions = self.calculate_recursions(self.Secure_time_landing) thrust = self.hover_thrust for i in range(recursions): if thrust <= 0: break elif i < 200: print("Secure hover before landing") target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.orientation = self.imu.orientation target_raw_attitude.body_rate.x = 0 # ROLL_RATE target_raw_attitude.body_rate.y = 0 # PITCH_RATE target_raw_attitude.body_rate.z = 0 # YAW_RATE thrust = self.hover_thrust target_raw_attitude.thrust = thrust self.attitude_target_pub.publish(target_raw_attitude) time.sleep(self.Time_between_messages ) # was 0.005 (now 50hz ,500 loops ,5sec) elif i < 700: print("Smooth landing") target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.orientation = self.imu.orientation target_raw_attitude.body_rate.x = 0 # ROLL_RATE target_raw_attitude.body_rate.y = 0 # PITCH_RATE target_raw_attitude.body_rate.z = 0 # YAW_RATE thrust = self.hover_thrust - self.Deaccumulating_thrust target_raw_attitude.thrust = thrust self.attitude_target_pub.publish(target_raw_attitude) time.sleep(self.Time_between_messages ) # was 0.005 (now 50hz ,500 loops ,5sec) else: print("Landing") target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.orientation = self.imu.orientation target_raw_attitude.body_rate.x = 0 # ROLL_RATE target_raw_attitude.body_rate.y = 0 # PITCH_RATE target_raw_attitude.body_rate.z = 0 # YAW_RATE target_raw_attitude.thrust = thrust thrust = thrust - self.accumulating_thrust self.attitude_target_pub.publish(target_raw_attitude) time.sleep(self.Time_between_messages ) # was 0.005 (now 50hz ,500 loops ,5sec) self.disarm()
def main1(): global setpoint # thrust_pub = rospy.Publisher('/mavros/setpoint_attitude/thrust',Thrust,queue_size=10) # thrust = Thrust() attitude_pub = rospy.Publisher('/mavros/setpoint_raw/attitude',AttitudeTarget,queue_size=10) point_pub = rospy.Publisher('/waypoint',PoseStamped,queue_size=10) attitude = AttitudeTarget() attitude.type_mask = 3 attitude.header = msg.header # attitude.header.frame_id = 'FRAME_LOCAL_NED' # quaternion = tf.transformations.quaternion_from_euler(msg.roll,msg.pitch, 0) # attitude.orientation.x = quaternion[0] # attitude.orientation.y = quaternion[1] # attitude.orientation.z = quaternion[2] # attitude.orientation.w = quaternion[3] attitude.orientation = Quaternion(*tf_conversions.transformations.quaternion_from_euler(msg.roll, msg.pitch, 0)) attitude.body_rate.z = msg.yaw_rate t = msg.thrust.z/100 if t>1: t=1 elif t<-1: t=-1 attitude.thrust = (t+1)/2 attitude_pub.publish(attitude) point_pub.publish(setpoint)
def construct_target_attitude(self, body_x = 0, body_y = 0, body_z = 0, thrust=0): target_raw_attitude = AttitudeTarget() # We will fill the following message with our values: http://docs.ros.org/api/mavros_msgs/html/msg/PositionTarget.html target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.orientation = self.imu.orientation target_raw_attitude.body_rate.x = body_x # ROLL_RATE target_raw_attitude.body_rate.y = body_y # PITCH_RATE target_raw_attitude.body_rate.z = body_z # YAW_RATE target_raw_attitude.thrust = thrust self.attitude_target_pub.publish(target_raw_attitude)
def landing_rec(self, thrust, beh_type, time_flying): if (time_flying <= 0 or self.down_sensor_distance <= self.landing_sensor_altitude_min) and beh_type == "LANDING": print("Entering secure landing") return self.secure_landing_phase_rec(thrust, beh_type, self.Secure_time_landing) elif thrust > self.Landing_thrust: #and beh_type == "HOVER": print("Landing the drone down slowly") target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.orientation = self.imu.orientation target_raw_attitude.body_rate.x = 0 # ROLL_RATE target_raw_attitude.body_rate.y = 0 # PITCH_RATE target_raw_attitude.body_rate.z = 0 # YAW_RATE target_raw_attitude.thrust = thrust thrust = thrust - self.accumulating_thrust self.attitude_target_pub.publish(target_raw_attitude) time_flying = time_flying - self.Time_between_messages time.sleep(self.Time_between_messages ) # was 0.005 (now 50hz ,500 loops ,5sec) return self.landing_rec( thrust, beh_type, time_flying) #bublishing a constant parameter "not elif thrust <= self.Landing_thrust: #and beh_type == "HOVER": print("Landing the drone down slowly") target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.orientation = self.imu.orientation target_raw_attitude.body_rate.x = 0 # ROLL_RATE target_raw_attitude.body_rate.y = 0 # PITCH_RATE target_raw_attitude.body_rate.z = 0 # YAW_RATE target_raw_attitude.thrust = self.Landing_thrust thrust = self.Landing_thrust self.attitude_target_pub.publish(target_raw_attitude) time_flying = time_flying - self.Time_between_messages time.sleep(self.Time_between_messages ) # was 0.005 (now 50hz ,500 loops ,5sec) return self.landing_rec( thrust, beh_type, time_flying ) #bublishing a constant parameter "not updating thrust argument"
def lift_off_rec(self, thrust, beh_type, time_flying): if (time_flying <= 0 or self.down_sensor_distance <= self.hover_sensor_altitude_min) and beh_type == "TAKE OFF": print("time of lift off has ended") beh_type = "HOVER" return self.hover_rec(thrust, beh_type, self.Hover_time) elif beh_type == "TAKE OFF" and thrust >= self.Liftoff_thrust and self.down_sensor_distance <= self.hover_sensor_altitude_min: print("The drone is lifting off at constant thrust") target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.orientation = self.imu.orientation target_raw_attitude.body_rate.x = 0 # ROLL_RATE target_raw_attitude.body_rate.y = 0 # PITCH_RATE target_raw_attitude.body_rate.z = 0 # YAW_RATE thrust = self.Liftoff_thrust target_raw_attitude.thrust = thrust self.attitude_target_pub.publish(target_raw_attitude) time.sleep( self.Time_between_messages) #was 0.005 (now 50hz ,500loops) time_flying = time_flying - self.Time_between_messages return self.lift_off_rec(target_raw_attitude.thrust, beh_type, time_flying) elif beh_type == "TAKE OFF": print("Lifting the drone up slowly") target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.orientation = self.imu.orientation target_raw_attitude.body_rate.x = 0 # ROLL_RATE target_raw_attitude.body_rate.y = 0 # PITCH_RATE target_raw_attitude.body_rate.z = 0 # YAW_RATE target_raw_attitude.thrust = thrust + self.accumulating_thrust self.attitude_target_pub.publish(target_raw_attitude) time_flying = time_flying - self.Time_between_messages time.sleep(self.Time_between_messages ) # was 0.005 (now 50hz ,500 loops ,5sec) return self.lift_off_rec(target_raw_attitude.thrust, beh_type, time_flying)
def landing_rec(self): # Landing phase self.beh_type = "LANDING" recursions = self.calculate_recursions(self.Max_time_landing) print(recursions) thrust = self.hover_thrust for i in range(recursions): print self.down_sensor_distance if self.down_sensor_distance <= self.landing_sensor_altitude_min: break elif thrust > self.Landing_thrust: #and beh_type == "HOVER": print("Landing the drone down slowly") target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.orientation = self.imu.orientation target_raw_attitude.body_rate.x = 0 # ROLL_RATE target_raw_attitude.body_rate.y = 0 # PITCH_RATE target_raw_attitude.body_rate.z = 0 # YAW_RATE target_raw_attitude.thrust = thrust thrust = thrust - self.accumulating_thrust self.attitude_target_pub.publish(target_raw_attitude) time.sleep(self.Time_between_messages ) # was 0.005 (now 50hz ,500 loops ,5sec) elif thrust <= self.Landing_thrust: #and beh_type == "HOVER": print("Landing the drone down slowly") target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.orientation = self.imu.orientation target_raw_attitude.body_rate.x = 0 # ROLL_RATE target_raw_attitude.body_rate.y = 0 # PITCH_RATE target_raw_attitude.body_rate.z = 0 # YAW_RATE target_raw_attitude.thrust = self.Landing_thrust thrust = self.Landing_thrust self.attitude_target_pub.publish(target_raw_attitude) time.sleep(self.Time_between_messages ) # was 0.005 (now 50hz ,500 loops ,5sec) self.secure_landing_phase_rec2()
def lift_off_rec(self, thrust, time_flying): recursions = self.calculate_recursions(time_flying) print(recursions) self.beh_type = "TAKE OFF" for i in range(recursions): print(i) if self.beh_type == "TAKE OFF" and thrust < self.Liftoff_thrust: #and beh_type == "HOVER": print("Lifting the drone up slowly") target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.orientation = self.imu.orientation target_raw_attitude.body_rate.x = 0 # ROLL_RATE target_raw_attitude.body_rate.y = 0 # PITCH_RATE target_raw_attitude.body_rate.z = 0 # YAW_RATE target_raw_attitude.thrust = thrust thrust = thrust + self.accumulating_thrust self.attitude_target_pub.publish(target_raw_attitude) time.sleep(self.Time_between_messages ) # was 0.005 (now 50hz ,500 loops ,5sec) elif self.beh_type == "TAKE OFF" and thrust >= self.Liftoff_thrust and self.down_sensor_distance <= self.hover_sensor_altitude_min: print("The drone is lifting off at constant thrust") target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.orientation = self.imu.orientation target_raw_attitude.body_rate.x = 0 # ROLL_RATE target_raw_attitude.body_rate.y = 0 # PITCH_RATE target_raw_attitude.body_rate.z = 0 # YAW_RATE thrust = self.Liftoff_thrust target_raw_attitude.thrust = thrust self.attitude_target_pub.publish(target_raw_attitude) time.sleep(self.Time_between_messages ) #was 0.005 (now 50hz ,500loops) else: break self.beh_type = 'HOVER' print("time of lifting off has ended") self.hover_rec(self.hover_time)
def secure_landing_phase_rec(self, thrust, time_flying): if time_flying <= 0: return True else: target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.orientation = self.imu.orientation target_raw_attitude.body_rate.x = 0 # ROLL_RATE target_raw_attitude.body_rate.y = 0 # PITCH_RATE target_raw_attitude.body_rate.z = 0 # YAW_RATE target_raw_attitude.thrust = self.Landing_thrust thrust = self.Landing_thrust self.attitude_target_pub.publish(target_raw_attitude) time_flying = time_flying - self.Time_between_messages time.sleep(self.Time_between_messages ) # was 0.005 (now 50hz ,500 loops ,5sec) return self.secure_landing_phase_rec( thrust, beh_type, time_flying ) #bublishing a constant parameter "not updating thrust argument"
def main(args): global list_current_position, list_current_velocity, current_position, usingvelocitycontrol, usingpositioncontrol, xvel, yvel, state, cur_pose, list_error, target global global_obs_detected, list_velocity_angle, vController par.CurrentIteration = 1 if (len(args) > 1): uav_ID = str(args[1]) else: uav_ID = "1" idx = int(uav_ID) - 1 rospy.init_node("control_uav_" + uav_ID) print "UAV" + uav_ID vController = VelocityController() try: rospy.wait_for_service("/uav" + uav_ID + "/mavros/cmd/arming") rospy.wait_for_service("/uav" + uav_ID + "/mavros/set_mode") except rospy.ROSException: fail("failed to connect to service") state_sub = rospy.Subscriber("/uav" + uav_ID + "/mavros/state", State, queue_size=10, callback=state_cb) local_vel_pub = rospy.Publisher("/uav" + uav_ID + "/mavros/setpoint_velocity/cmd_vel", TwistStamped, queue_size=10) local_pos_pub = rospy.Publisher("/uav" + uav_ID + "/mavros/setpoint_position/local", PoseStamped, queue_size=10) local_pos_target = rospy.Publisher("/uav" + uav_ID + "/mavros/setpoint_raw/local", PositionTarget, queue_size=10) atittude_pub = rospy.Publisher("/uav" + uav_ID + "/mavros/setpoint_raw/attitude", AttitudeTarget, queue_size=10) thr_pub = rospy.Publisher("/uav" + uav_ID + "/mavros/setpoint_attitude/att_throttle", Float64, queue_size=10) Astar_grid_pub = rospy.Publisher("/uav" + uav_ID + "/Astar_grid", GridCells, queue_size=10) Astar_path_pub = rospy.Publisher("/uav" + uav_ID + "/Astar_path", GridCells, queue_size=10) arming_client = rospy.ServiceProxy("/uav" + uav_ID + "/mavros/cmd/arming", CommandBool) set_mode_client = rospy.ServiceProxy("/uav" + uav_ID + "/mavros/set_mode", SetMode) for i in range(4): #velocity_sub = rospy.Subscriber("/uav"+repr(i+1)+"/mavros/local_position/velocity",TwistStamped,queue_size = 10,callback=velocity_cb, callback_args=i) position_sub = rospy.Subscriber("/uav" + repr(i + 1) + "/mavros/local_position/pose", PoseStamped, queue_size=10, callback=position_cb, callback_args=(i, int(uav_ID) - 1)) scan_lidar_sub = rospy.Subscriber("/scan", LaserScan, queue_size=10, callback=scan_lidar_cb, callback_args=uav_ID) br = tf.TransformBroadcaster() r = rospy.Rate(10) print "TRY TO CONNECT" while ((not rospy.is_shutdown()) and (not current_state.connected)): #rospy.spinOnce() r.sleep() #print(current_state.connected.__class__) rospy.loginfo("CURRENT STATE CONNECTED") poses = PoseStamped() #poses.pose = Pose() #poses.pose.position = Point() target = Pose() if (idx == 0): target.position.x = 0 target.position.y = 0 if (idx == 1): target.position.x = 0 target.position.y = 0 if (idx == 2): target.position.x = 0 target.position.y = 0 if (idx == 3): target.position.x = 4 target.position.y = 4 target.position.z = 10 poses.pose.position.x = target.position.x poses.pose.position.y = target.position.y poses.pose.position.z = target.position.z q = quaternion_from_euler(0, 0, 45 * np.pi / 180.0) poses.pose.orientation.x = q[0] poses.pose.orientation.y = q[1] poses.pose.orientation.z = q[2] poses.pose.orientation.w = q[3] i = 100 #while((not rospy.is_shutdown()) and (i>0)): # local_pos_pub.publish(poses) # i = i-1 rviz_visualization_start = False last_request = rospy.Time.now() count = 0 if (idx == 1): target.position.x = 28 target.position.y = 28 if (idx == 2): target.position.x = 0 target.position.y = 17 #thread1 = Thread(target = key_function) #thread1.start() style.use('fivethirtyeight') thread2 = Thread(target=plot_error_live) thread2.start() thread3 = Thread(target=calculate_and_publish_wall, args=(Astar_grid_pub, Astar_path_pub, idx)) thread3.start() #file_error_pos_x = open(dir_path+'/txt/error_pos_x.txt','w') #file_error_pos_y = open(dir_path+'/txt/error_pos_y.txt','w') #file_error_pos_z = open(dir_path+'/txt/error_pos_z.txt','w') error_time = 0 print poses uav_color = [255, 0, 0, 255] topic = 'uav_marker_' + uav_ID uav_marker_publisher = rospy.Publisher(topic, Marker, queue_size=10) uav_marker = Marker() uav_marker.header.frame_id = "world" uav_marker.type = uav_marker.SPHERE uav_marker.action = uav_marker.ADD uav_marker.scale.x = par.ws_model['robot_radius'] * 3 uav_marker.scale.y = par.ws_model['robot_radius'] * 3 uav_marker.scale.z = 0.005 * par.RealScale uav_marker.color.r = float(uav_color[0]) / 255 uav_marker.color.g = float(uav_color[1]) / 255 uav_marker.color.b = float(uav_color[2]) / 255 uav_marker.color.a = float(uav_color[3]) / 255 uav_marker.pose.orientation.w = 1.0 uav_marker.pose.position.x = 0 #init_pos[0] uav_marker.pose.position.y = 0 #init_pos[1] uav_marker.pose.position.z = 0 #init_pos[2] uav_marker_publisher.publish(uav_marker) uav_color = [255, 255, 255, 255] topic = 'uav_goal_marker_' + uav_ID uav_goal_marker_publisher = rospy.Publisher(topic, Marker, queue_size=10) uav_goal_marker = Marker() uav_goal_marker.header.frame_id = "world" uav_goal_marker.type = uav_goal_marker.SPHERE uav_goal_marker.action = uav_goal_marker.ADD uav_goal_marker.scale.x = par.ws_model['robot_radius'] * 2 uav_goal_marker.scale.y = par.ws_model['robot_radius'] * 2 uav_goal_marker.scale.z = 0.005 * par.RealScale uav_goal_marker.color.r = float(uav_color[0]) / 255 uav_goal_marker.color.g = float(uav_color[1]) / 255 uav_goal_marker.color.b = float(uav_color[2]) / 255 uav_goal_marker.color.a = float(uav_color[3]) / 255 uav_goal_marker.pose.orientation.w = 1.0 uav_goal_marker.pose.position.x = 0 #init_pos[0] uav_goal_marker.pose.position.y = 0 #init_pos[1] uav_goal_marker.pose.position.z = 0 #init_pos[2] uav_goal_marker_publisher.publish(uav_goal_marker) uav_goal_marker = update_uav_marker( uav_goal_marker, (target.position.x, target.position.y, target.position.z, 1.0)) uav_goal_marker_publisher.publish(uav_goal_marker) last_request_rviz = rospy.Time.now() last_HRVO_request = rospy.Time.now() while (not rospy.is_shutdown()): if (rviz_visualization_start and (rospy.Time.now() - last_request_rviz > rospy.Duration(0.2))): publish_uav_position_rviz( br, list_current_position[idx].pose.position.x, list_current_position[idx].pose.position.y, list_current_position[idx].pose.position.z, uav_ID) uav_marker = update_uav_marker( uav_marker, (list_current_position[idx].pose.position.x, list_current_position[idx].pose.position.y, list_current_position[idx].pose.position.z, 1.0)) uav_marker_publisher.publish(uav_marker) last_request_rviz = rospy.Time.now() if ((not rviz_visualization_start) and (current_state.mode != 'OFFBOARD') and (rospy.Time.now() - last_request > rospy.Duration(1.0))): offb_set_mode = set_mode_client(0, 'OFFBOARD') if (offb_set_mode.mode_sent): rospy.loginfo("OFFBOARD ENABLED") last_request = rospy.Time.now() else: if ((not current_state.armed) and (rospy.Time.now() - last_request > rospy.Duration(1.0))): arm_cmd = arming_client(True) if (arm_cmd.success): rospy.loginfo("VEHICLE ARMED") rviz_visualization_start = True last_request = rospy.Time.now() #print distance3D(cur_pose.pose,target) if (distance3D(cur_pose.pose, target) < 0.5) and (not usingvelocitycontrol): print "sampai" usingvelocitycontrol = True usingpositioncontrol = False target = Pose() if (idx == 0): target.position.x = 28 target.position.y = 28 if (idx == 1): target.position.x = 0 target.position.y = 0 if (idx == 2): target.position.x = 21 target.position.y = 14 if (idx == 3): target.position.x = 0 target.position.y = 0 target.position.z = 10 print target psi_target = 45 * np.pi / 180.0 #np.pi/180.0 #np.arctan((self.target.position.y-self.prev_target.position.y)/(self.target.position.x-self.prev_target.position.x))#(linear.y,linear.x) diagram4 = GridWithWeights(par.NumberOfPoints, par.NumberOfPoints) diagram4.walls = [] for i in range(par.NumberOfPoints ): # berikan wall pada algoritma pathfinding A* for j in range(par.NumberOfPoints): if (par.ObstacleRegion[j, i] == 0): diagram4.walls.append((i, j)) for i in range(par.NumberOfPoints): for j in range(par.NumberOfPoints): if (par.ObstacleRegionWithClearence[j][i] == 0): diagram4.weights.update({(i, j): par.UniversalCost}) goal_pos = (target.position.x / par.RealScale, target.position.y / par.RealScale) start_pos = (cur_pose.pose.position.x / par.RealScale, cur_pose.pose.position.y / par.RealScale) pathx, pathy = Astar_version1(par, start_pos, goal_pos, diagram4) print pathx print pathy vController.setHeadingTarget(deg2rad(90.0)) #45.0)) target.position.x = pathx[0] * par.RealScale target.position.y = pathy[0] * par.RealScale uav_goal_marker = update_uav_marker( uav_goal_marker, (target.position.x, target.position.y, target.position.z, 1.0)) uav_goal_marker_publisher.publish(uav_goal_marker) vController.setTarget(target) vController.setPIDGainX(1, 0.0, 0.0) vController.setPIDGainY(1, 0.0, 0.0) vController.setPIDGainZ(1, 0, 0) vController.setPIDGainPHI(1, 0.0, 0.0) vController.setPIDGainTHETA(1, 0.0, 0.0) vController.setPIDGainPSI(1, 0.0, 0.0) if (usingpositioncontrol): print "kontrol posisi" poses.pose.position.x = 29 poses.pose.position.y = 29 poses.pose.position.z = 10 local_pos_pub.publish(poses) elif (usingvelocitycontrol): if (distance2D(cur_pose.pose, target) < 1.5): if (len(pathx) > 1): del pathx[0] del pathy[0] target.position.x = pathx[0] * par.RealScale target.position.y = pathy[0] * par.RealScale uav_goal_marker = update_uav_marker( uav_goal_marker, (target.position.x, target.position.y, target.position.z, 1.0)) uav_goal_marker_publisher.publish(uav_goal_marker) print target vController.setTarget(target) if (rospy.Time.now() - last_HRVO_request > rospy.Duration(0.05)): last_HRVO_request = rospy.Time.now() header.stamp = rospy.Time.now() des_vel = vController.update(cur_pose) current_agent_pose = ( list_current_position[idx].pose.position.x, list_current_position[idx].pose.position.y, list_current_position[idx].pose.position.z) current_agent_vel = (list_current_velocity[idx].twist.linear.x, list_current_velocity[idx].twist.linear.y) current_neighbor_pose = [] for i in range(par.NumberOfAgents): if (i != idx): current_neighbor_pose.append( (list_current_position[i].pose.position.x, list_current_position[i].pose.position.y)) current_neighbor_vel = [] for i in range(par.NumberOfAgents): if (i != idx): current_neighbor_vel.append( (list_current_velocity[i].twist.linear.x, list_current_velocity[i].twist.linear.y)) V_des = (des_vel.twist.linear.x, des_vel.twist.linear.y) quat_arr = np.array([ list_current_position[idx].pose.orientation.x, list_current_position[idx].pose.orientation.y, list_current_position[idx].pose.orientation.z, list_current_position[idx].pose.orientation.w ]) att = euler_from_quaternion(quat_arr, 'sxyz') #V = locplan.RVO_update_single(current_agent_pose, current_neighbor_pose, current_agent_vel, current_neighbor_vel, V_des, par,list_velocity_angle,list_distance_obs,att[2]) V, RVO_BA_all = locplan.RVO_update_single_static( current_agent_pose, current_neighbor_pose, current_agent_vel, current_neighbor_vel, V_des, par) V_msg = des_vel V_msg.twist.linear.x = V[0] V_msg.twist.linear.y = V[1] local_vel_pub.publish(V_msg) goal = (target.position.x, target.position.y, target.position.z) x = current_position list_error[0].append(error_time) list_error[1].append(goal[0] - x[0]) list_error[2].append(goal[1] - x[1]) list_error[3].append(goal[2] - x[2]) error_time = error_time + 1 k = 0.1 vx = k * (goal[0] - x[0]) vy = k * (goal[1] - x[1]) vz = k * (goal[2] - x[2]) postar = PositionTarget() postar.header = header postar.coordinate_frame = PositionTarget().FRAME_LOCAL_NED p = PositionTarget().IGNORE_PX | PositionTarget( ).IGNORE_PY | PositionTarget().IGNORE_PZ a = PositionTarget().IGNORE_AFX | PositionTarget( ).IGNORE_AFY | PositionTarget().IGNORE_AFZ v = PositionTarget().IGNORE_VX | PositionTarget( ).IGNORE_VY | PositionTarget().IGNORE_VZ y = PositionTarget().IGNORE_YAW | PositionTarget( ).IGNORE_YAW_RATE f = PositionTarget().FORCE postar.type_mask = a | y | p | f #| PositionTarget().IGNORE_YAW | PositionTarget().IGNORE_YAW_RATE | PositionTarget().FORCE postar.velocity.x = vx postar.velocity.y = vy postar.velocity.z = vz postar.position.x = goal[0] postar.position.y = goal[1] postar.position.z = goal[2] vel_msg = TwistStamped() vel_msg.header = header vel_msg.twist.linear.x = xvel vel_msg.twist.linear.y = yvel vel_msg.twist.linear.z = 0.0 vel_msg.twist.angular.x = 0.0 vel_msg.twist.angular.y = 0.0 vel_msg.twist.angular.z = 0.0 q = quaternion_from_euler(0, 0, 0.2) att = PoseStamped() att.pose.orientation.x = q[0] att.pose.orientation.y = q[1] att.pose.orientation.z = q[2] att.pose.orientation.w = q[3] att.pose.position.x = 0.0 att.pose.position.y = 0.0 att.pose.position.z = 2.0 cmd_thr = Float64() cmd_thr.data = 0.3 att_raw = AttitudeTarget() att_raw.header = Header() att_raw.body_rate = Vector3() att_raw.thrust = 0.7 #Float64() att_raw.header.stamp = rospy.Time.now() att_raw.header.frame_id = "fcu" att_raw.type_mask = 71 att_raw.orientation = Quaternion( *quaternion_from_euler(0, 0, 0.2)) else: local_pos_pub.publish(poses) count = count + 1 r.sleep() try: rospy.spin() except KeyboardInterrupt: print("Shutting down") cv2.destroyAllWindows()
def main(): # INITIALIZE ADAPTIVE NETWORK PARAMETERS: N_INPUT = 3 # Number of input network N_OUTPUT = 1 # Number of output network INIT_NODE = 1 # Number of node(s) for initial structure INIT_LAYER = 2 # Number of initial layer (minimum 2, for 1 hidden and 1 output) N_WINDOW = 500 # Sliding Window Size EVAL_WINDOW = 3 # Evaluation Window for layer Adaptation DELTA = 0.05 # Confidence Level for layer Adaptation (0.05 = 95%) ETA = 0.0001 # Minimum allowable value if divided by zero FORGET_FACTOR = 0.98 # Forgetting Factor of Recursive Calculation #EXP_DECAY = 1 # Learning Rate decay factor #LEARNING_RATE = 0.00005 # Network optimization learning rate LEARNING_RATE = 0.00002 / 2 # Network optimization learning rate WEIGHT_DECAY = 0.000125 / 5 # Regularization weight decay factor #WEIGHT_DECAY = 0.0 # Regularization weight decay factor # INITIALIZE SYSTEM AND SIMULATION PARAMETERS: IRIS_THRUST = 0.5629 # Nominal thrust for IRIS Quadrotor RATE = 30.0 # Sampling Frequency (Hz) # PID_GAIN_X = [0.25,0.0,0.02] # PID Gain Parameter [KP,KI,KD] axis-X # PID_GAIN_Y = [0.25,0.0,0.02] # PID Gain Parameter [KP,KI,KD] axis-Y PID_GAIN_X = [0.15, 0.0, 0.004] # PID Gain Parameter [KP,KI,KD] axis-X PID_GAIN_Y = [0.15, 0.0, 0.004] # PID Gain Parameter [KP,KI,KD] axis-Y #PID_GAIN_Z = [0.013,0.0004,0.2] # PID Gain Parameter [KP,KI,KD] axis-Z PID_GAIN_Z = [0.013, 0.0, 0.2] # PID Gain Parameter [KP,KI,KD] axis-Z SIM_TIME = 200 # Simulation time duration (s) # Initial conditions of UAV system xref = 0.0 yref = 0.0 zref = 2.0 interrx = 0.0 interry = 0.0 interrz = 0.0 errlastx = 0.0 errlasty = 0.0 errlastz = 0.0 runtime = 0.0 # Ignite the Evolving NN Controller Xcon = NetEvo(N_INPUT, N_OUTPUT, INIT_NODE) Ycon = NetEvo(N_INPUT, N_OUTPUT, INIT_NODE) Zcon = NetEvo(N_INPUT, N_OUTPUT, INIT_NODE) if INIT_LAYER > 2: pdb.set_trace() for i in range(INIT_LAYER - 2): Xcon.addhiddenlayer() Ycon.addhiddenlayer() Zcon.addhiddenlayer() dt = 1 / RATE Xcon.dt = dt Ycon.dt = dt Zcon.dt = dt Xcon.smcpar = PID_GAIN_X Ycon.smcpar = PID_GAIN_Y Zcon.smcpar = PID_GAIN_Z # PX4 API object modes = fcuModes() # Flight mode object uav = uavCommand() # UAV command object # Data Logger object logData = dataLogger() xconLog = dataLogger() yconLog = dataLogger() zconLog = dataLogger() # Initiate node and subscriber rospy.init_node('setpoint_node', anonymous=True) rate = rospy.Rate(RATE) # ROS loop rate, [Hz] # Subscribe to drone state rospy.Subscriber('mavros/state', State, uav.stateCb) # Subscribe to drone's local position #rospy.Subscriber('mavros/local_position/pose', PoseStamped, uav.posCb) rospy.Subscriber('mavros/mocap/pose', PoseStamped, uav.posCb) # Setpoint publisher att_sp_pub = rospy.Publisher('mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=10) # Initiate Attitude messages att = AttitudeTarget() att.body_rate = Vector3() att.orientation = Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0)) att.thrust = IRIS_THRUST att.type_mask = 7 # Arming the UAV --> no auto arming text = colored("Ready for Arming..Press Enter to continue...", 'green', attrs=['reverse', 'blink']) raw_input(text) while not uav.state.armed: modes.setArm() rate.sleep() text = colored('Vehicle is arming..', 'yellow') print(text) # Switch to OFFBOARD after send few setpoint messages text = colored("Vehicle Armed!! Press Enter to switch OFFBOARD...", 'green', attrs=['reverse', 'blink']) raw_input(text) while not uav.state.armed: modes.setArm() rate.sleep() text = colored('Vehicle is arming..', 'yellow') print(text) rate.sleep() k = 0 while k < 10: att_sp_pub.publish(att) rate.sleep() k = k + 1 modes.setOffboardMode() text = colored('Now in OFFBOARD Mode..', 'blue', attrs=['reverse', 'blink']) print(text) # ROS Main loop:: k = 0 while not rospy.is_shutdown(): start = time.time() rate.sleep() # Setpoint generator if runtime <= 20: xref = 0.0 yref = 0.0 zref = runtime * 1.5 / 20.0 if runtime > 20 and runtime <= 30: xref = (runtime - 20) * 1.0 / 10.0 #xref = (runtime-20)* 5.0/10.0 yref = 0.0 zref = 1.5 if runtime > 30: xref = 0.0 + 1.0 * math.cos(math.pi * (runtime - 30) / 20.0) yref = 0.0 + 1.0 * math.sin(math.pi * (runtime - 30) / 20.0) #xref = 0.0+5.0*math.cos(math.pi*(runtime-30)/60.0) #yref = 0.0+5.0*math.sin(math.pi*(runtime-30)/60.0) zref = 1.5 # Adjust the number of nodes in the latest layer (Network Width) Xcon.adjustWidth(FORGET_FACTOR, N_WINDOW) Ycon.adjustWidth(FORGET_FACTOR, N_WINDOW) #Zcon.adjustWidth(FORGET_FACTOR,N_WINDOW) # Adjust the number of layer (Network Depth) Xcon.adjustDepth(ETA, DELTA, N_WINDOW, EVAL_WINDOW) Ycon.adjustDepth(ETA, DELTA, N_WINDOW, EVAL_WINDOW) #Zcon.adjustDepth(ETA,DELTA,N_WINDOW,EVAL_WINDOW) # update current position xpos = uav.local_pos.x ypos = uav.local_pos.y zpos = uav.local_pos.z # calculate errors,delta errors, and integral errors errx, derrx, interrx = Xcon.calculateError(xref, xpos) erry, derry, interry = Ycon.calculateError(yref, ypos) errz, derrz, interrz = Zcon.calculateError(zref, zpos) # PID Controller equations #theta_ref = 0.04*errx+0.0005*interrx+0.01*derrx #phi_ref = 0.04*erry+0.0005*interry+0.01*derry # vx = PID_GAIN_X[0]*errx+PID_GAIN_X[1]*interrx+PID_GAIN_X[2]*derrx # vy = PID_GAIN_Y[0]*erry+PID_GAIN_Y[1]*interry+PID_GAIN_Y[2]*derry #thrust_ref = IRIS_THRUST+PID_GAIN_Z[0]*errz+PID_GAIN_Z[1]*interrz+PID_GAIN_Z[2]*derrz # SMC + NN controller vx = Xcon.controlUpdate(xref) # Velocity X vy = Ycon.controlUpdate(yref) # Velocity Y thz = Zcon.controlUpdate(zref) # Additional Thrust Z euler = euler_from_quaternion(uav.q) psi = euler[2] phi_ref, theta_ref = transform_control_signal(vx, vy, psi) thrust_ref = IRIS_THRUST + thz # control signal limiter #phi_ref = limiter(-1*phi_ref,0.2) phi_ref = limiter(-1 * phi_ref, 0.25) theta_ref = limiter(-1 * theta_ref, 0.25) att.thrust = limiter(thrust_ref, 1) # Phi, Theta and Psi quaternion transformation att.orientation = Quaternion( *quaternion_from_euler(phi_ref, theta_ref, 0.0)) # Publish the control signal att_sp_pub.publish(att) # NN Learning stage Xcon.optimize(LEARNING_RATE, WEIGHT_DECAY) Ycon.optimize(LEARNING_RATE, WEIGHT_DECAY) Zcon.optimize(LEARNING_RATE, WEIGHT_DECAY) # Print all states print('Xr,Yr,Zr = ', xref, yref, zref) print('X, Y, Z = ', uav.local_pos.x, uav.local_pos.y, uav.local_pos.z) print('phi, theta = ', phi_ref, theta_ref) print('att angles = ', euler) print('Thrust = ', att.thrust) print('Nodes X Y Z = ', Xcon.nNodes, Ycon.nNodes, Zcon.nNodes) print('Layer X Y Z = ', Xcon.nLayer, Ycon.nLayer, Zcon.nLayer) k += 1 runtime = k * dt # Append Data Logger logData.appendStateData(runtime, xref, yref, zref, xpos, ypos, zpos, phi_ref, theta_ref, thrust_ref) xconLog.appendControlData(runtime, Xcon.us, Xcon.un, Xcon.u, Xcon.nNodes) yconLog.appendControlData(runtime, Ycon.us, Ycon.un, Ycon.u, Ycon.nNodes) zconLog.appendControlData(runtime, Zcon.us, Zcon.un, Zcon.u, Zcon.nNodes) print('Runtime = ', runtime) print('Exec time = ', time.time() - start) print('') # Break condition if runtime > SIM_TIME: modes.setRTLMode() text = colored('Auto landing mode now..', 'blue', attrs=['reverse', 'blink']) print(text) text = colored('Now saving all logged data..', 'yellow', attrs=['reverse', 'blink']) print(text) logData.plotFigure() xconLog.plotControlData() yconLog.plotControlData() zconLog.plotControlData() text = colored('Mission Complete!!', 'green', attrs=['reverse', 'blink']) print(text) break
def hover_rec(self, thrust, beh_type, time_flying): if self.beh_type == 'HOVER' and time_flying <= 0: print("Hovering time ended") return self.landing_rec(target_raw_attitude.thrust, beh_type, self.Max_time_landing) elif beh_type == 'HOVER' and (self.hover_sensor_altitude_max >= self.down_sensor_distance >= self.hover_sensor_altitude_min): print("The drone is hovering") beh_type = 'HOVER' target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.orientation = self.imu.orientation target_raw_attitude.body_rate.x = 0 # ROLL_RATE target_raw_attitude.body_rate.y = 0 # PITCH_RATE target_raw_attitude.body_rate.z = 0 # YAW_RATE thrust = self.hover_thrust target_raw_attitude.thrust = thrust self.attitude_target_pub.publish(target_raw_attitude) time.sleep( self.Time_between_messages) #was 0.005 (now 50hz ,500loops) time_flying = time_flying - self.Time_between_messages return self.hover_rec(target_raw_attitude.thrust, beh_type, time_flying) elif beh_type == 'HOVER' and ( self.hover_sensor_altitude_max <= self.down_sensor_distance): # We have to go down print("Recovering hover position - going down") beh_type = 'HOVER' target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.orientation = self.imu.orientation target_raw_attitude.body_rate.x = 0 # ROLL_RATE target_raw_attitude.body_rate.y = 0 # PITCH_RATE target_raw_attitude.body_rate.z = 0 # YAW_RATE thrust = self.hover_thrust target_raw_attitude.thrust = thrust - self.Deaccumulating_thrust self.attitude_target_pub.publish(target_raw_attitude) time.sleep(self.Time_between_messages ) # was 0.005 (now 50hz ,500 loops ,5sec) time_flying = time_flying - self.Time_between_messages return self.hover_rec(target_raw_attitude.thrust, beh_type, time_flying) elif beh_type == 'HOVER' and ( self.down_sensor_distance <= self.hover_sensor_altitude_min): # We have to go up print("Recovering hover position - going up") beh_type = 'HOVER' target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.orientation = self.imu.orientation target_raw_attitude.body_rate.x = 0 # ROLL_RATE target_raw_attitude.body_rate.y = 0 # PITCH_RATE target_raw_attitude.body_rate.z = 0 # YAW_RATE thrust = self.hover_thrust target_raw_attitude.thrust = thrust + self.Deaccumulating_thrust self.attitude_target_pub.publish(target_raw_attitude) time.sleep(self.Time_between_messages ) # was 0.005 (now 50hz ,500 loops ,5sec) time_flying = time_flying - self.Time_between_messages return self.hover_rec(target_raw_attitude.thrust, beh_type, time_flying)
turn = turn * kb.speedBindings[key][1] print(kb.vels(speed, turn)) if (status == 14): print(kb.msg) status = (status + 1) % 15 else: #x = 0 #y = 0 #z = 0 #th = 0 if (key == '\x03'): break target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.orientation = kb.imu.orientation target_raw_attitude.body_rate.x = kb.acc_x # ROLL_RATE target_raw_attitude.body_rate.y = kb.acc_y # PITCH_RATE target_raw_attitude.body_rate.z = kb.acc_z # YAW_RATE target_raw_attitude.thrust = kb.acc_thrust attitude_target_pub.publish(target_raw_attitude) #twist = Twist() #twist.linear.x = x*speed; twist.linear.y = y*speed; twist.linear.z = z*speed #twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = th*turn #pub.publish(twist) except Exception as e: print(e) finally:
from time import sleep import rospy from geometry_msgs.msg import Quaternion from geometry_msgs.msg import Vector3 from mavros_msgs.msg import Thrust from mavros_msgs.msg import AttitudeTarget node = rospy.init_node("thrust_test") rate = rospy.Rate(20) # Hz pub = rospy.Publisher('/mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=1) print("looping!") while True: msg = AttitudeTarget() msg.orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) msg.body_rate = Vector3(x=0.0, y=0.0, z=0.0) msg.thrust = -0.001 pub.publish(msg) rate.sleep() print("Sending thrust [%f]!" % msg.thrust)