def __init__(self): self.step = 0 self.lds = rospy.Subscriber("riseq/sacc/ladder_info", RiseSaccHelix, self.lds_cb) self.ref_pub_local = rospy.Publisher("riseq/sacc/setpoint_helix_local", PoseStamped, queue_size=10) self.ref_pub_global = rospy.Publisher( "riseq/sacc/setpoint_helix_global", GlobalPositionTarget, queue_size=10) self.ladder_depth = 0.0 self.ladder_height = 30 # PX4 related self.state = Odometry() self.setup_mavros() self.command_pose = PoseStamped() self.command_pose.pose.orientation.w = 1.0 self.home_pose = PoseStamped() self.home_pose_set = False self.global_home = GlobalPositionTarget() self.global_state = GlobalPositionTarget() self.global_home_pose_set = False
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 __init__(self): # Publisher to ardrone cmd_vel topic, can be run in namespace 48.213 self.global_position_target_pub = rospy.Publisher( "/mavros/setpoint_raw/global", GlobalPositionTarget, queue_size=1) # Initialize joy and cmd_vel variables self.joy_data = Joy() self.command_velocity = Twist() # Publishing to real cmd_vel self.override_control = 0 self.current_reference = GlobalPositionTarget() self.current_reference.coordinate_frame = GlobalPositionTarget.FRAME_GLOBAL_INT self.current_reference.type_mask = 2552 self.max_vel_x = rospy.get_param("~max_vel_x", 0.5) self.max_vel_y = rospy.get_param("~max_vel_y", 0.5) self.max_vel_z = rospy.get_param("~max_vel_z", 0.5) self.max_vel_yaw = rospy.get_param("~max_vel_yaw", 0.1) self.rate = rospy.get_param("~rate", 20) self.gps_factor = 0.00001 self.global_yaw = 0.0 self.get_current_global_reference_flag = 0 self.first_global_reference_flag = 0 # Subscriber to joystick topic rospy.Subscriber("/manual_control/joy", Joy, self.joyCallback, queue_size=1) # subscriber na trenutnu globalnu poziciju rospy.Subscriber("/mavros/global_position/global", NavSatFix, self.currentGlobalPositionCallback, queue_size=1) # subscriber to imu for yaw rospy.Subscriber("/mavros/imu/data", Imu, self.imuCallback, queue_size=1)
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)
a/d : increase/decrease longitude jingdu east/west space key, s : force stop CTRL-C to quit """ e = """ Communications Failed """ """ global variable """ gps_curr = NavSatFix() gps_pub_msg = GlobalPositionTarget() gps_pub_geo_msg = GeoPoseStamped() uav_state = State() ismove = Bool() ismove = False def getKey(): if os.name == 'nt': return msvcrt.getch() tty.setraw(sys.stdin.fileno()) rlist, _, _ = select.select([sys.stdin], [], [], 0.1) if rlist: key = sys.stdin.read(1) else: key = ''
queue_size=10) 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()
topic_global = '/mavros/global_position/global' topic_local = '/local_position' topic_origin = '/set_origin' topic_alt = '/mavros/altitude' topic_global_set = '/mavros/setpoint_position/global' topic_marker = "/goal_pose" frame_id = 'map' current_global_pose = None origin_pose = None local_pose_msg = PoseStamped() heiht = 0.0 goal_global_msg = GlobalPositionTarget() mask=int('111111111000', 2) def global_pose_clb(data): """ Robot Geodetic coordinates from GPS callback. :param data: GPS data :type data: sensor_msgs.msg.NavSatFix :type origin_pose: sensor_msgs.msg.NavSatFix :type current_global_pose: NavSatFix :type local_pose_msg: PoseStamped """ global origin_pose, current_global_pose, local_pose_msg, heiht
def do_helix_trajectory(self): init_x = self.state.pose.pose.position.x init_y = self.state.pose.pose.position.y init_z = self.state.pose.pose.position.z self.helix_controller = htc(vrate=0.05, radius=1.0, center=(1, 0, 0), init=(init_x, init_y, init_z), t_init=rospy.get_time(), w=0.5) self.yaw_controller = sc2(Kp=6., Kv=0.0) q = self.state.pose.pose.orientation q = [q.x, q.y, q.z, q.w] yaw, pitch, roll = tt.euler_from_quaternion(q, axes='rzyx') self.helix_controller.phase = yaw + np.pi # yaw angle + 180 degrees because its center perspective print("Phase: ", self.helix_controller.phase) rate = rospy.Rate(20) # do helix trajectory for 30 seconds print("Doing helix trajectory") #while( (rospy.get_time() - self.helix_controller.t_init) < 120.): while (self.state.pose.pose.position.z < (self.ladder_height + 2)): # state for x and y position xs = np.array([[self.state.pose.pose.position.x], [self.state.twist.twist.linear.x], [0.0]]) ys = np.array([[self.state.pose.pose.position.y], [self.state.twist.twist.linear.y], [0.0]]) states = [xs, ys] ladder_position = self.get_ladder_location() ladder_position = [0.0, 1.0] self.helix_controller.set_helix_center(ladder_position) ux, uy, uz, ref = self.helix_controller.compute_command( states, rospy.get_time()) q = self.compute_yaw() self.command_pose.header.stamp = rospy.Time.now() self.command_pose.pose.position.x = ux self.command_pose.pose.position.y = uy self.command_pose.pose.position.z = uz self.command_pose.pose.orientation.x = q[0] self.command_pose.pose.orientation.y = q[1] self.command_pose.pose.orientation.z = q[2] self.command_pose.pose.orientation.w = q[3] self.local_pos_pub.publish(self.command_pose) # publish reference trajectory in local frame refmsg = PoseStamped() refmsg.header.stamp = rospy.Time.now() refmsg.pose.position.x = ref[0][0][0] refmsg.pose.position.y = ref[1][0][0] refmsg.pose.position.z = uz self.ref_pub_local.publish(refmsg) # publish reference trajectory in global frame lat, lon = self.get_global_coordinates(ux, uy) global_refmsg = GlobalPositionTarget() global_refmsg.header.stamp = rospy.Time.now() global_refmsg.header.frame_id = self.global_state.header.frame_id global_refmsg.latitude = lat global_refmsg.longitude = lon global_refmsg.altitude = uz self.ref_pub_global.publish(global_refmsg) rate.sleep()
class bt_missions: rospy.init_node('mavros_takeoff_python') mavros.set_namespace() rate = rospy.Rate(20) dirct_ = Float64() home_ = HomePosition() current_state = State() current_ = GlobalPositionTarget() home_set_ = False isContinue = True dist_first = True dis_count = True #triangle target = [[24.985059, 121.572934], [24.985161, 121.572858]] forward_t = rospy.get_time() index = 0 time_index = 0 p0 = 0 p1 = 0 brng = 0 dLon = 0 y = 0 x = 0 current_yaw = 0 dist = 0 heading = 0 lat = 0 lon = 0 cnt = 0 set_vel_pub = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel', TwistStamped, queue_size=10) set_v_pub = rospy.Publisher('/mavros/setpoint_raw/local', PositionTarget, queue_size=10) local_pos_pub = rospy.Publisher(mavros.get_topic('setpoint_position', 'local'), PoseStamped, queue_size=10) def __init__(self): self.hdg_subscriber = rospy.Subscriber( "/mavros/global_position/compass_hdg", Float64, self.compass) self.home_sub = rospy.Subscriber("/mavros/home_position/home", HomePosition, self.setHomeGeoPointCB) self.gps_sub = rospy.Subscriber("/mavros/global_position/global", NavSatFix, self.current_position_cb) state_sub = rospy.Subscriber(mavros.get_topic('state'), State, self.state_cb) self.tree = self.acheive | ( (self.isGuided | (self.notGuided >> self.set_guided)) >> (self.yaw_correction | self.notGuided | self.notGuided | self.correction) >> self.forward) #(self.yaw_discorrection | self.notGuided | (self.yaw_correction >> def compass(self, dirct): bt_missions.dirct_ = dirct bt_missions.heading = bt_missions.dirct_.data def state_cb(self, state): bt_missions.current_state = State() bt_missions.current_state = state def angleFromCoordinate(self, lat0, long0, lat1, long1): bt_missions.dLon = (long1 - long0) bt_missions.y = math.sin(bt_missions.dLon) * math.cos(bt_missions.lat1) bt_missions.x = math.cos(lat0) * math.sin(lat1) - math.sin(lat0) * cos( lat1) * math.cos(bt_missions.dLon) bt_missions.brng = math.atan2(bt_missions.y, bt_missions.x) bt_missions.brng = math.degrees(bt_missions.brng) bt_missions.brng = (bt_missions.brng + 360) % 360 bt_missions.brng = 360 - bt_missions.brng bt_missions.brng = math.radians(bt_missions.brng) def setHomeGeoPointCB(self, home): bt_missions.home_ = home bt_missions.home_set_ = True rospy.loginfo( "Received Home (WGS84 datum): %lf, %lf, %lf" % (bt_missions.home_.geo.latitude, bt_missions.home_.geo.longitude, bt_missions.home_.geo.altitude)) def current_position_cb(self, current): bt_missions.current_ = current bt_missions.lat = bt_missions.current_.latitude bt_missions.lon = bt_missions.current_.longitude @condition def acheive(self): if bt_missions.index == 0 and bt_missions.dist_first == True: bt_missions.dist_first = False bt_missions.p0 = (bt_missions.home_.geo.latitude, bt_missions.home_.geo.longitude) bt_missions.p1 = (bt_missions.target[bt_missions.index][0], bt_missions.target[bt_missions.index][1]) bt_missions.dist = vincenty(bt_missions.p0, bt_missions.p1).meters print(bt_missions.dist) if bt_missions.dist >= 1.5: return False else: bt_missions.index = bt_missions.index + 1 elif bt_missions.index == 0 and bt_missions.dist_first == False: bt_missions.p0 = (bt_missions.current_.latitude, bt_missions.current_.longitude) bt_missions.p1 = (bt_missions.target[bt_missions.index][0], bt_missions.target[bt_missions.index][1]) bt_missions.dist = vincenty(bt_missions.p0, bt_missions.p1).meters print(bt_missions.dist) if bt_missions.dist >= 1.5: return False else: bt_missions.index = bt_missions.index + 1 elif bt_missions.index == 1: bt_missions.p0 = (bt_missions.current_.latitude, bt_missions.current_.longitude) bt_missions.p1 = (bt_missions.target[bt_missions.index][0], bt_missions.target[bt_missions.index][1]) bt_missions.dist = vincenty(bt_missions.p0, bt_missions.p1).meters print(bt_missions.dist) if bt_missions.dist >= 1.5: return False else: return True @condition def isGuided(self): if bt_missions.current_state == "GUIDED": return True else: return False @condition def notGuided(self): if bt_missions.current_state == "GUIDED": return False else: return True @condition def yaw_correction(self): if bt_missions.index == 0: bt_missions.dLon = (bt_missions.target[bt_missions.index][1] - bt_missions.home_.geo.longitude) bt_missions.y = math.sin(bt_missions.dLon) * math.cos( bt_missions.target[bt_missions.index][0]) bt_missions.x = math.cos(bt_missions.home_.geo.latitude) * math.sin( bt_missions.target[bt_missions.index][0]) - math.sin( bt_missions.home_.geo.latitude) * cos(bt_missions.target[ bt_missions.index][0]) * math.cos(bt_missions.dLon) bt_missions.brng = math.atan2(bt_missions.y, bt_missions.x) bt_missions.brng = math.degrees(bt_missions.brng) bt_missions.brng = (bt_missions.brng + 360) % 360 bt_missions.brng = 360 - bt_missions.brng bt_missions.brng = math.radians(bt_missions.brng) bt_missions.brng = math.degrees(bt_missions.brng) bt_missions.current_yaw = bt_missions.heading if bt_missions.current_yaw > ( 360 - bt_missions.brng) + 2 or bt_missions.current_yaw < ( 360 - bt_missions.brng) - 2: return False else: return True else: bt_missions.dis_count = True bt_missions.dLon = (bt_missions.target[bt_missions.index][1] - bt_missions.current_.longitude) bt_missions.y = math.sin(bt_missions.dLon) * math.cos( bt_missions.target[bt_missions.index][0]) bt_missions.x = math.cos(bt_missions.home_.geo.latitude) * math.sin( bt_missions.target[bt_missions.index][0]) - math.sin( bt_missions.current_.latitude) * cos(bt_missions.target[ bt_missions.index][0]) * math.cos(bt_missions.dLon) bt_missions.brng = math.atan2(bt_missions.y, bt_missions.x) bt_missions.brng = math.degrees(bt_missions.brng) bt_missions.brng = (bt_missions.brng + 360) % 360 bt_missions.brng = 360 - bt_missions.brng bt_missions.brng = math.radians(bt_missions.brng) bt_missions.brng = math.degrees(bt_missions.brng) bt_missions.current_yaw = bt_missions.heading if bt_missions.current_yaw > ( 360 - bt_missions.brng) + 2 or bt_missions.current_yaw < ( 360 - bt_missions.brng) - 2: return False else: return True @condition def yaw_discorrection(self): if bt_missions.index == 0: bt_missions.current_yaw = bt_missions.heading print("discorrection heading: %s" % bt_missions.current_yaw) print("discorrection brng: %s" % bt_missions.brng) if bt_missions.current_yaw > ( 360 - bt_missions.brng) + 2 or bt_missions.current_yaw < ( 360 - bt_missions.brng) - 2: bt_missions.current_yaw = bt_missions.heading return True else: return False else: bt_missions.current_yaw = bt_missions.heading print("discorrection heading: %s" % bt_missions.current_yaw) print("discorrection brng: %s" % bt_missions.brng) if bt_missions.current_yaw > ( 360 - bt_missions.brng) + 2 or bt_missions.current_yaw < ( 360 - bt_missions.brng) - 2: bt_missions.current_yaw = bt_missions.heading return True else: return False @action def set_guided(self): rospy.wait_for_service('/mavros/set_mode') try: change_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode) response = change_mode(custom_mode="GUIDED") except rospy.ServiceException as e: print("Set mode failed: %s" % e) @action def correction(self): if bt_missions.dis_count == True: bt_missions.current_yaw = bt_missions.heading while bt_missions.current_yaw > ( 360 - bt_missions.brng) + 2 or bt_missions.current_yaw < ( 360 - bt_missions.brng) - 2: msg = TwistStamped() if 360 - bt_missions.brng - bt_missions.current_yaw > 0 and 360 - bt_missions.brng - bt_missions.current_yaw < 180: msg.twist.angular.z = -0.1 elif 360 - bt_missions.brng - bt_missions.current_yaw > 0 and 360 - bt_missions.brng - bt_missions.current_yaw >= 180: msg.twist.angular.z = 0.1 elif 360 - bt_missions.brng - bt_missions.current_yaw < 0 and math.fabs( 360 - bt_missions.brng - bt_missions.current_yaw) >= 180: msg.twist.angular.z = -0.1 elif 360 - bt_missions.brng - bt_missions.current_yaw < 0 and math.fabs( 360 - bt_missions.brng - bt_missions.current_yaw) < 180: msg.twist.angular.z = 0.1 bt_missions.set_vel_pub.publish(msg) bt_missions.current_yaw = bt_missions.heading #print("first correction : %s" %bt_missions.current_yaw) else: #pass bt_missions.current_yaw = bt_missions.heading while bt_missions.current_yaw > ( 360 - bt_missions.brng ) + 2 or bt_missions.current_yaw < ( 360 - bt_missions.brng ) - 2: #and int(rospy.get_time() -bt_missions.forward_t) % 10 == 0: msg = TwistStamped() #print("forward correction: %s" %bt_missions.current_yaw) msg.twist.angular.z = math.radians(bt_missions.current_yaw - 360 + bt_missions.brng) bt_missions.set_vel_pub.publish(msg) bt_missions.current_yaw = bt_missions.heading @action def forward(self): print(123) forward_t = rospy.get_time() #while rospy.get_time() - forward_t <= 5: bt_missions.dis_count = False vel = PositionTarget() vel.header.stamp = rospy.Time.now() vel.coordinate_frame = PositionTarget.FRAME_BODY_NED vel.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ + PositionTarget.FORCE + PositionTarget.IGNORE_YAW + PositionTarget.IGNORE_YAW_RATE vel.velocity.x = 0 vel.velocity.y = 0.8 vel.velocity.z = 0 bt_missions.set_v_pub.publish(vel) bt_missions.rate.sleep() def run(self): while True: if bt_missions.isContinue == False: break bt_count = self.tree.blackboard(1) bt_state = bt_count.tick() print("bt state = %s\n" % bt_state) while bt_count == RUNNING: bt_state = bt_count.tick() print("state = %s\n" % bt_state) assert bt_state == SUCCESS or bt_state == FAILURE