def goto(self, waypoint, duration=rospy.Duration(60, 0), low_fuel_trip=False): """ Go to specific waypoint action """ init_fuel = self.fuel start = rospy.Time.now() wp = self.waypoints[waypoint - 1] original = GlobalPositionTarget() original.header.seq = 1 original.header.stamp = rospy.Time.now() original.header.frame_id = 'map' original.coordinate_frame = GlobalPositionTarget.FRAME_GLOBAL_REL_ALT original.type_mask = 0b001111111000 original.latitude = wp['lat'] original.longitude = wp['long'] original.altitude = 0.0 original.yaw = yaw_ned_to_enu(wp['yaw']) original.yaw_rate = 2.0 rospy.loginfo("%s is going to waypoint %d, lat: %.5f, long: %.5f" % (self.namespace, waypoint, wp['lat'], wp['long'])) reached_original = self.goto_coordinate(original, low_fuel_trip=low_fuel_trip, radius=self._radius, duration=duration) rospy.loginfo("%s is reaching waypoint %d, lat: %.5f, long: %.5f" % (self.namespace, waypoint, wp['lat'], wp['long'])) self.calculate_fuel_rate(init_fuel, start, self.fuel_rate_std) return reached_original
def inspect_wt(self, duration=rospy.Duration(600, 0)): """ ASV inspection for a WT that involves pointing a camera to the WT while ASV is moving back and forth """ fuel = self.fuel start = rospy.Time.now() # position where drone is originated in one of the wps wp = self.waypoints[self._current_wp - 1] original = GlobalPositionTarget() original.header.seq = 1 original.header.stamp = rospy.Time.now() original.header.frame_id = 'map' original.coordinate_frame = GlobalPositionTarget.FRAME_GLOBAL_REL_ALT original.type_mask = 0b001111111000 original.latitude = wp['lat'] original.longitude = wp['long'] original.altitude = 0.0 original.yaw = yaw_ned_to_enu(wp['yaw']) original.yaw_rate = 2.0 rospy.loginfo("%s is scanning a wind turbine..." % self.namespace) first_blade = self.blade_inspect(original, [0.0, 90.0, 0.0], duration) self.calculate_fuel_rate(fuel, start, self.fuel_rate_std) rospy.loginfo("%s has done the inspection..." % self.namespace) return first_blade
def task_allocation(): global wp_publisher, vehicle_pos, rate task_list = task_manager.get_tasks() mission_complete = False task_in_progress = False print "Node Begins" while not mission_complete: if not task_in_progress: print "Task Allocation Begin\n" task = task_manager.get_optimal_task(task_list, vehicle_pos) target = GlobalPositionTarget() target.coordinate_frame = 6 target.type_mask = 4088 target.latitude = task[0] target.longitude = task[1] target.altitude = 5.0 target.header.frame_id = '/base_link' wp_publisher.publish(target) task_in_progress = True print "Task Alloted\n" if task_in_progress and task_manager.task_achieved(task, vehicle_pos): print "Removing Task from List" task_in_progress = False task_list.remove(task) if (len(task_list) == 0): mission_complete = True rate.sleep()
def next_waypoint(vehicle_number, latitude, longitude): publisher = rospy.Publisher('/vehicle' + str(vehicle_number) + '/mavros/setpoint_raw/global', GlobalPositionTarget, queue_size=10) pub = GlobalPositionTarget() pub.coordinate_frame = 6 pub.type_mask = 4088 pub.longitude = longitude pub.latitude = latitude publisher.publish(pub)
def blade_inspect(self, original, target_position, duration=rospy.Duration(300, 0)): """ Inspecting the blade given the [original] pose to return to and end [target] position to scan """ start = rospy.Time.now() reached_original = self.ACTION_FAIL # rotate the camera for _ in range(3): self._rotate_cam.publish(Int32(1)) rospy.sleep(0.1) # position where ASV is supposed to be heading = self.heading offset_x = (target_position[0] * np.cos(heading) + target_position[1] * np.sin(heading)) offset_y = (-1 * target_position[0] * np.sin(heading) + target_position[1] * np.cos(heading)) latitude_offset, longitude_offset = xy_to_longlat( offset_x, offset_y, self.global_pose.latitude) target = GlobalPositionTarget() target.header.seq = 1 target.header.stamp = rospy.Time.now() target.header.frame_id = 'map' target.coordinate_frame = GlobalPositionTarget.FRAME_GLOBAL_REL_ALT target.type_mask = 0b001111111000 target.latitude = self.global_pose.latitude + latitude_offset target.longitude = self.global_pose.longitude + longitude_offset target.altitude = target_position[2] target.yaw = yaw_ned_to_enu(heading) target.yaw_rate = 2.0 duration = duration - (rospy.Time.now() - start) start = rospy.Time.now() reached_target = self.goto_coordinate(target, low_fuel_trip=False, duration=duration) rospy.loginfo("%s is returning to %3.5f, %3.5f position..." % (self.namespace, original.latitude, original.longitude)) if reached_target == self.ACTION_SUCCESS: # rotate the camera for _ in range(3): self._rotate_cam.publish(Int32(-1)) rospy.sleep(0.1) original.header.seq = 1 original.header.stamp = rospy.Time.now() original.header.frame_id = 'map' duration = duration - (rospy.Time.now() - start) start = rospy.Time.now() reached_original = self.goto_coordinate(original, low_fuel_trip=False, duration=duration) # rotate the camera for _ in range(3): self._rotate_cam.publish(Int32(0)) rospy.sleep(0.1) return np.min([reached_target, reached_original])
def action_cb(user_data): rospy.loginfo('Mission Action') position_topic = rospy.Subscriber('/mavros/global_position/global', NavSatFix, home_callback) Alt_topic = rospy.Subscriber('/mavros/local_position/pose', PoseStamped, pose_callback) compass_topic = rospy.Subscriber('mavros/global_position/compass_hdg', Float64, compass_callback) (locked, err_x_pix, err_y_pix, target_dx, target_dy, target_dist_normal, target_yaw, theta_err) = rec_and_show.ballonchecker() rospy.sleep(0.5) time_ini = rospy.Time.now().secs print 'Action: count' while (rospy.Time.now().secs-time_ini) < config.getint('GAPL','action_time') and mode == 'GUIDED': (locked, err_x_pix, err_y_pix, target_dx, target_dy, target_dist_normal, target_yaw, theta_err) = rec_and_show.ballonchecker() if locked == 1: vel_x = 1.1*user_data.k_action*(target_dist_normal-user_data.dist_min) vel_x = saturate(vel_x, user_data.vel_x_min, user_data.vel_x_max, user_data.v_zero) vel_y = user_data.kerr_vely*target_dx vel_y = saturate(vel_y, user_data.vel_y_min, user_data.vel_y_max, user_data.v_zero) vel_z = -user_data.k_z*(target_dy) vel_z = saturation_velZ(vel_z, alt, user_data.alt_min, user_data.alt_max, target_dy, user_data.Hmax85, user_data.Hmin115) psi_dot = (((user_data.yaw_rateMax-user_data.yaw_rateMin)/(user_data.errx_max_pix-user_data.errx_min_pix))*(abs(err_x_pix)-user_data.errx_min_pix))+user_data.yaw_rateMin vel_yaw = yaw_alignment(err_x_pix, user_data.errx_min_pix, psi_dot) print "Vx=%.2f m/sec" % (vel_x),"; Vy=%.2f m/sec" % (vel_y),"; Vz=%.2f m/sec" % (vel_z),"; yaw_rate=%.2f rad/s\r" % (vel_yaw), sys.stdout.flush() set_velocity_body(vel_x, vel_y, vel_z, vel_yaw) bearing = add_angles(compass, target_yaw) target_loc = target_location(target_dist_normal, target_dx, target_dy, bearing, lat, lon, alt) if locked == 1: balloon_position = target_loc position_memory.balloon_reached(balloon_position[0], balloon_position[1], balloon_position[2]) print 'Balloon added to the list of reached' my_latit = lat my_longit = lon my_new_altit = alt + user_data.delta_alt set_position = GlobalPositionTarget() set_position.coordinate_frame = 6 set_position.latitude = my_latit set_position.longitude = my_longit set_position.altitude = my_new_altit set_position.type_mask = 4088 setpoint_position_pub.publish(set_position) time.sleep(0.5) print 'Changing Altitude' while alt < my_new_altit*0.85 and mode == 'GUIDED': continue if mode == 'ALT_HOLD': Clear_Mission() print 'Stop VideoGet and VideoShow' rec_and_show.stop() rospy.signal_shutdown('Quit for Alt_hold') os.system('rosnode kill mavros') sis.stop() elif mode == 'GUIDED' and alt >= my_new_altit*0.85: print 'New Altitude Reached' return 'finished' else: return 'failed'
def reaching_cb(user_data): rospy.loginfo('mode GUIDED') setGuidedMode() rospy.loginfo('Balloon found') rospy.loginfo('Mission Reaching') (locked, err_x_pix, err_y_pix, err_x_m, err_y_m, dist, res_w, res_h) = balloonchecker() #position_topic = rospy.Subscriber('/mavros/global_position/global', NavSatFix, home_callback) #Alt_topic = rospy.Subscriber('/mavros/local_position/pose', PoseStamped, pose_callback) #compass_topic = rospy.Subscriber('mavros/global_position/compass_hdg', Float64, compass_callback) #rospy.sleep(0.5) if locked == 1: vel_x, vel_y, vel_z, yaw_rate = dir_pid_return() set_velocity_body(vel_x, vel_y, vel_z, yaw_rate) while locked == 1 and mode == 'GUIDED': vel_x, vel_y, vel_z, yaw_rate = dir_pid_return() set_velocity_body(vel_x, vel_y, vel_z, yaw_rate) (locked, err_x_pix, err_y_pix, err_x_m, err_y_m, dist, res_w, res_h) = balloonchecker() if locked == 1 and abs(err_x_m) <= 0.1 and abs(err_y_m) < 0.1 and abs( dist) < 1.2: break if mode == 'ALT_HOLD': Clear_Mission() rospy.signal_shutdown('Quit for Alt_hold') os.system('rosnode kill ' + mavros_name) sis.stop() elif locked == 1 and abs(err_x_m) <= 0.1 and abs(err_y_m) < 0.1 and abs( dist) < 1.2: #if i got it in my hands my_latit = lat my_longit = lon #my_new_altit = alt + user_data.delta_alt my_new_altit = alt set_position = GlobalPositionTarget() set_position.coordinate_frame = 6 set_position.latitude = my_latit set_position.longitude = my_longit set_position.altitude = my_new_altit set_position.type_mask = 4088 setpoint_position_pub.publish(set_position) time.sleep(0.5) print 'STOP' print 'Balloon reached!' return 'searching' elif locked == 0: # user_data.target_loc = target_loc # print target_loc return 'searching' else: return 'failed'
def publish_target_pose(self, pose_s): coords = self.pose2wgs84(pose_s) if not coords: rospy.logerr('Cannot compute pose in UTM frame') return lat, lon, heading = coords msg = GlobalPositionTarget() msg.coordinate_frame = GlobalPositionTarget.FRAME_GLOBAL_INT msg.type_mask = (GlobalPositionTarget.IGNORE_VX + GlobalPositionTarget.IGNORE_VY + GlobalPositionTarget.IGNORE_VZ + GlobalPositionTarget.IGNORE_AFX + GlobalPositionTarget.IGNORE_AFY + GlobalPositionTarget.IGNORE_AFZ + GlobalPositionTarget.IGNORE_YAW + GlobalPositionTarget.IGNORE_YAW_RATE) msg.latitude = lat msg.longitude = lon msg.altitude = 0.0 msg.yaw = heading self.pub.publish(msg) self.pub_pose.publish(pose_s)
def publish_target_twist(self, twist_s): utm_twist_s = twist_in_frame(self.tf_buffer, twist_s, 'utm') if not utm_twist_s: rospy.logerr('Cannot compute twist in UTM frame') return # CHANGED: Mavros setpoint_raw plugin expect a velocity in ENU frame!!! v = vector2enu(utm_twist_s.twist.linear) msg = GlobalPositionTarget() msg.coordinate_frame = GlobalPositionTarget.FRAME_GLOBAL_INT msg.type_mask = (GlobalPositionTarget.IGNORE_LATITUDE + GlobalPositionTarget.IGNORE_LONGITUDE + GlobalPositionTarget.IGNORE_ALTITUDE + GlobalPositionTarget.IGNORE_AFX + GlobalPositionTarget.IGNORE_AFY + GlobalPositionTarget.IGNORE_AFZ + GlobalPositionTarget.IGNORE_YAW + GlobalPositionTarget.IGNORE_YAW_RATE) msg.velocity.x = v[0] msg.velocity.y = v[1] msg.velocity.z = v[2] self.global_pub.publish(msg) super(MAVPathFollower, self).publish_target_twist(twist_s)
def publish_target_pose(self, pose): point = _a(pose.pose.position) # print 'point' , point yaw = np.arctan2(pose.pose.orientation.z, pose.pose.orientation.w) * 2.0 latlon, heading = self.pose2wgs84(point, yaw) lon, lat = latlon.tolist()[0] # print lon, lat msg = GlobalPositionTarget() msg.coordinate_frame = GlobalPositionTarget.FRAME_GLOBAL_INT msg.type_mask = (GlobalPositionTarget.IGNORE_VX + GlobalPositionTarget.IGNORE_VY + GlobalPositionTarget.IGNORE_VZ + GlobalPositionTarget.IGNORE_AFX + GlobalPositionTarget.IGNORE_AFY + GlobalPositionTarget.IGNORE_AFZ + GlobalPositionTarget.IGNORE_YAW + GlobalPositionTarget.IGNORE_YAW_RATE) msg.latitude = lat msg.longitude = lon msg.altitude = 0.0 msg.yaw = heading self.pub.publish(msg) self.pub_pose.publish(pose)
def publish_target_pose(self, pose_s): # rospy.loginfo('publish_target_pose %s', pose_s) utm_pose_s = pose_in_frame(self.tf_buffer, pose_s, 'utm') if not utm_pose_s: rospy.logerr('Cannot compute pose in UTM frame') return lat, lon, heading = pose2wgs84(utm_pose_s) msg = GlobalPositionTarget() msg.coordinate_frame = self.coordinate_frame() msg.type_mask = (GlobalPositionTarget.IGNORE_VX + GlobalPositionTarget.IGNORE_VY + GlobalPositionTarget.IGNORE_VZ + GlobalPositionTarget.IGNORE_AFX + GlobalPositionTarget.IGNORE_AFY + GlobalPositionTarget.IGNORE_AFZ + GlobalPositionTarget.IGNORE_YAW + GlobalPositionTarget.IGNORE_YAW_RATE) msg.latitude = lat msg.longitude = lon msg.altitude = self.altitude(utm_pose_s.pose.position.z) # msg.yaw = heading self.global_pub.publish(msg) super(MAVPathFollower, self).publish_target_pose(pose_s)
current_state = State() current_position = NavSatFix() home = HomePosition() wp0 = GlobalPositionTarget() wp1 = GlobalPositionTarget() wp2 = GlobalPositionTarget() wp3 = GlobalPositionTarget() wp4 = GlobalPositionTarget() wp = GlobalPositionTarget() wp_target = GlobalPositionTarget() wp_avoidance = GlobalPositionTarget() wp_helical = GlobalPositionTarget() wp0.coordinate_frame = GlobalPositionTarget().FRAME_GLOBAL_INT wp0.type_mask = GlobalPositionTarget().IGNORE_VX + GlobalPositionTarget( ).IGNORE_VY + GlobalPositionTarget().IGNORE_VZ + GlobalPositionTarget( ).IGNORE_AFX + GlobalPositionTarget().IGNORE_AFY + GlobalPositionTarget( ).IGNORE_AFZ + GlobalPositionTarget().FORCE + GlobalPositionTarget( ).IGNORE_YAW_RATE wp0.latitude = 37.565011 wp0.longitude = 126.628919 wp0.altitude = 5.0 wp0.yaw = np.pi / 180.0 * 117.0 wp1.coordinate_frame = wp0.coordinate_frame wp1.type_mask = wp0.type_mask wp1.latitude = 37.566025 wp1.longitude = 126.628206 wp1.altitude = 100.0
def task_allocation(): global wp_publisher1, wp_publisher2, vehicle1_pos, vehicle2_pos, rate task_list = task_manager.get_tasks() tasks_finished = False vehicle1_in_progress = False vehicle2_in_progress = False vehicle1_task_complete = False vehicle2_task_complete = False print "Node Begins" while (not vehicle1_task_complete) or (not vehicle2_task_complete) or ( not tasks_finished): if not vehicle1_in_progress and not tasks_finished: print "Task Allocation Begin\n" vehicle1_task = task_manager.get_optimal_task( task_list, vehicle1_pos) target = GlobalPositionTarget() target.coordinate_frame = 6 target.type_mask = 4088 target.latitude = vehicle1_task[0] target.longitude = vehicle1_task[1] target.altitude = 5.0 target.header.frame_id = '' wp_publisher1.publish(target) vehicle1_in_progress = True vehicle1_task_complete = False task_list.remove(vehicle1_task) if (len(task_list) == 0): tasks_finished = True print "Task Alloted to Vehicle 1\n" if not vehicle2_in_progress and not tasks_finished: print "Task Allocation Begin\n" vehicle2_task = task_manager.get_optimal_task( task_list, vehicle2_pos) target = GlobalPositionTarget() target.coordinate_frame = 6 target.type_mask = 4088 target.latitude = vehicle2_task[0] target.longitude = vehicle2_task[1] target.altitude = 5.0 target.header.frame_id = '' wp_publisher2.publish(target) vehicle2_in_progress = True vehicle2_task_complete = False task_list.remove(vehicle2_task) if (len(task_list) == 0): tasks_finished = True print "Task Alloted to Vehicle 2\n" if vehicle1_in_progress and task_manager.task_achieved( vehicle1_task, vehicle1_pos): print "Vehicle1 achieved task" vehicle1_in_progress = False vehicle1_task_complete = True if vehicle2_in_progress and task_manager.task_achieved( vehicle2_task, vehicle2_pos): print "Vehicle2 achieved task" vehicle2_in_progress = False vehicle2_task_complete = True #print "Vehicle 1 task comlpete", vehicle1_task_complete #print "Vehicle 2 task comlpete", vehicle2_task_complete #print "task comlpete", tasks_finished rate.sleep() print "All tasks finished"
def cbPubCmd(self, event): if not self.started: return if self.home_pos[2] < 1: return # Wait for home pos to be received # Check if distance to waypoint is small enough to count it as reached TODO may be wrong approximation and TODO change for RTK if self.getDistanceBetweenGPS(self.current_pos, self.waypoints[self.currentWPidx]) < 2: self.currentWPidx += 1 if self.currentWPidx == self.maxWPidx: rospy.loginfo("MISSION DONE") self.started = False return # Log info about current waypoint rospy.loginfo("Current waypoint: " + str(self.currentWPidx) + " / " + str(self.maxWPidx)) # Check if mode needs to be changed for OFFBOARD and ARM vehicle (this is a startup procedure) if str(self.current_state.mode) != "OFFBOARD" and rospy.Time.now( ) - self.last_request > rospy.Duration(5.0): resp = self.set_mode_client(0, "OFFBOARD") if resp.mode_sent: rospy.loginfo("Offboard enabled") self.last_request = rospy.Time.now() else: if not self.current_state.armed and rospy.Time.now( ) - self.last_request > rospy.Duration(5.0): resp = self.arming_client(True) if resp.success: rospy.loginfo("Vehicle armed") self.last_request = rospy.Time.now() # Publish information - where should the drone fly next? pose = PoseStamped() latWP = self.waypoints[self.currentWPidx][0] lonWP = self.waypoints[self.currentWPidx][1] altWP = self.waypoints[self.currentWPidx][2] velWP = self.waypoints[self.currentWPidx][3] # latHome = self.home_pos[0] # lonHome = self.home_pos[1] # altHome = self.home_pos[2] # pose.pose.position.x = 6371000 * radians(lonWP - lonHome) * cos(radians(latHome)) # pose.pose.position.y = 6371000 * radians(latWP - latHome) # pose.pose.position.z = altWP - altHome #self.local_pos_pub.publish(pose) if self.ready: msg = GlobalPositionTarget() msg.latitude = latWP msg.longitude = lonWP msg.altitude = altWP msg.header.stamp = rospy.Time.now() msg.coordinate_frame = 5 msg.type_mask = 0b101111111000 msg.yaw = self.getNextYaw() self.global_pos_pub.publish(msg) par = ParamValue() par.integer = 0 par.real = velWP try: self.set_vel("MPC_XY_VEL_MAX", par) except Exception: print("e") else: pose = PoseStamped() pose.pose.position.x = 0 pose.pose.position.y = 0 pose.pose.position.z = 2.0 self.local_pos_pub.publish(pose) try: par = ParamValue() par.integer = 0 par.real = velWP resp = self.set_vel("MPC_XY_VEL_MAX", par) if resp.success: self.ready = True except Exception as e: print(e)
print("Publisher and Subscriber Created") arming_client = rospy.ServiceProxy('mavros/cmd/arming', CommandBool) set_mode_client = rospy.ServiceProxy('mavros/set_mode', SetMode) landing_client = rospy.ServiceProxy('mavros/cmd/land', CommandTOL) print("Clients Created") rate = rospy.Rate(20) while (not current_state.connected): print(current_state.connected) rate.sleep() print("Creating global position") point = GlobalPositionTarget() point.header.stamp = rospy.Time.now() point.coordinate_frame = GlobalPositionTarget().FRAME_GLOBAL_TERRAIN_ALT point.type_mask = GlobalPositionTarget().IGNORE_VX + GlobalPositionTarget( ).IGNORE_VY + GlobalPositionTarget().IGNORE_VZ + GlobalPositionTarget( ).IGNORE_AFX + GlobalPositionTarget().IGNORE_AFY + GlobalPositionTarget( ).IGNORE_AFZ + GlobalPositionTarget().FORCE + GlobalPositionTarget( ).IGNORE_YAW_RATE point.latitude = 37.5647106 point.longitude = 126.6276294 point.altitude = 25.0657 point.yaw = 0.0 for i in range(100): #local_pos_pub.publish(pose) point.header.stamp = rospy.Time.now() global_pos_pub.publish(point) rate.sleep()