def __init__(self, config): self.alt_topic = config["alt_topic"] self.gps_topic = config["gps_topic"] self.state_topic = config["state_topic"] self.armed = False self.connected = False self.mode = '' self.takeoff = False self.latitude = 0 self.longitude = 0 self.altitude = 0 rospy.init_node("mavros_vehicle", anonymous=True) mavros.set_namespace() if mavros.utils.wait_fcu_connection(timeout=10): print("Connected to Flight Controller !") self.connected = True CreateThread(self.heartbeat) print("Listening to Altitude on %s" % self.alt_topic) print("Listening to GPS on - %s" % self.gps_topic) print("Listening to State on - %s" % self.state_topic) param.param_set("NAV_RCL_ACT", 0) param.param_set("NAV_DLL_ACT", 0)
def __init__(self, hz=50): rospy.loginfo("ObstaclePathPlanner Started!") mavros.set_namespace() self.ar_pose_sub = rospy.Subscriber("/ar_pose_marker", AlvarMarkers, self.ar_pose_cb) self.local_pose_sub = rospy.Subscriber("/mavros/local_position/pose", PoseStamped, self.local_pose_cb) self.z_error_pub = rospy.Publisher("/error/z", Float64, queue_size=1) """ State: 0: Hovering in place 1: Line following 2: Going over obstacles 3: Going under obstacles """ self.state = 1 self.rate = rospy.Rate(hz) self.hz = hz # Async variables: self.markers = [] self.current_pose = None # Vars for states 2 & 3: self.current_obstacle = None self.current_obstacle_dist = None self.artag_seen_x = None self.send_state()
def main(): listener() rospy.init_node('FLYtoPOS') mavros.set_namespace() # initialize mavros module with default namespace rate = rospy.Rate(25) rospy.loginfo("Starting...") position_msg = PoseStamped() position_msg.header.frame_id = "home" x = 1 y = 1 z = 25 while not rospy.is_shutdown(): position_msg.header.stamp = rospy.Time.now() position_msg.pose.position.x = x position_msg.pose.position.y = y position_msg.pose.position.z = z pubPosition.publish(position_msg) rate.sleep() rospy.loginfo("Bye!")
def setpoint_demo(): rospy.init_node('setpoint_position_demo') mavros.set_namespace() # initialize mavros module with default namespace rate = rospy.Rate(10) setpoint = SetpointPosition() rospy.loginfo("Climb") setpoint.set(0.0, 0.0, 3.0, 0) setpoint.set(0.0, 0.0, 10.0, 5) rospy.loginfo("Sink") setpoint.set(0.0, 0.0, 8.0, 5) rospy.loginfo("Fly to the right") setpoint.set(10.0, 4.0, 8.0, 5) rospy.loginfo("Fly to the left") setpoint.set(0.0, 0.0, 8.0, 5) offset_x = 0.0 offset_y = 0.0 offset_z = 10.0 sides = 360 radius = 20 rospy.loginfo("Fly in a circle") setpoint.set(0.0, 0.0, 10.0, 3) # Climb to the starting height first i = 0 while not rospy.is_shutdown(): x = radius * cos(i * 2 * pi / sides) + offset_x y = radius * sin(i * 2 * pi / sides) + offset_y z = offset_z wait = False delay = 0 if (i == 0 or i == sides): # Let it reach the setpoint. wait = True delay = 5 setpoint.set(x, y, z, delay, wait) i = i + 1 rate.sleep() if (i > sides): rospy.loginfo("Fly home") setpoint.set(0.0, 0.0, 10.0, 5) break # Simulate a slow landing. setpoint.set(0.0, 0.0, 8.0, 5) setpoint.set(0.0, 0.0, 3.0, 5) setpoint.set(0.0, 0.0, 2.0, 2) setpoint.set(0.0, 0.0, 1.0, 2) setpoint.set(0.0, 0.0, 0.0, 2) setpoint.set(0.0, 0.0, -0.2, 2) rospy.loginfo("Bye!")
def start(): rospy.init_node(name()) mavros.set_namespace() global set_mode, set_param, arming, set_home, takeoff, command, land, wp_push, wp_clear, wp_set set_mode = get_proxy('/mavros/set_mode', SetMode) set_param = get_proxy('/mavros/param/set', ParamSet) arming = get_proxy('/mavros/cmd/arming', CommandBool) set_home = get_proxy('/mavros/cmd/set_home', CommandHome) command = get_proxy('/mavros/cmd/command', CommandLong) takeoff = get_proxy('/mavros/cmd/takeoff', CommandTOL) land = get_proxy('/mavros/cmd/land', CommandTOL) wp_push = get_proxy('/mavros/mission/push', WaypointPush) wp_clear = get_proxy('/mavros/mission/clear', WaypointClear) wp_set = get_proxy('/mavros/mission/set_current', WaypointSetCurrent) global gps_topic, imu_sub gps_topic = mavros.get_topic('global_position', 'global') rospy.Service('command/takeoff', TakeOff, handle_takeoff) rospy.Service('command/land', Land, handle_land) rospy.Service('command/flyto', FlyTo, handle_flyto) rospy.Service('command/fly_waypoints', FlyWaypoints, handle_flywaypoints) rospy.Subscriber("/mavros/global_position/global", NavSatFix, partial(values.latch_value, gps_topic, max_age=10)) # Wait to receive some data from mavros before intializing imu_sub = rospy.Subscriber("/mavros/imu/data", Imu, handle_startup) rospy.spin()
def main(): rospy.init_node('follow_ar') mavros.set_namespace() # initialize mavros module with default namespace rate = rospy.Rate(25) rospy.loginfo("Starting...") position_msg = PoseStamped() position_msg.header.frame_id = "copter_home" listener = tf.TransformListener() while not rospy.is_shutdown(): try: (trans,rot) = listener.lookupTransform('/copter_home', '/ar_marker_13', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue position_msg.header.stamp = rospy.Time.now() position_msg.pose.position.x = trans[0] position_msg.pose.position.y = trans[1] position_msg.pose.position.z = trans[2] + 5 pubPosition.publish(position_msg) rate.sleep() rospy.loginfo("Bye!")
def __init__(self, rate=10): """ Initializes publishers and subscribers, sets initial values for vars :param rate: the rate at which the setpoint_velocity is published """ assert rate > 2 # make sure rate is high enough, if new setpoint recieved within .5 seconds, robot will switch back to POSCTL self.rate = rospy.Rate(rate) mavros.set_namespace() self.bridge = CvBridge() self.pub_local_velocity_setpoint = rospy.Publisher( "/mavros/setpoint_velocity/cmd_vel", TwistStamped, queue_size=1) self.sub_line_param = rospy.Subscriber("/line/param", Line, self.line_param_cb) self.pub_error = rospy.Publisher("/line/error", Vector3, queue_size=1) # Variables dealing with publishing setpoint self.sub_state = rospy.Subscriber("/mavros/state", State, self.state_cb) self.current_state = None self.offboard_point_streaming = False # Setpoint field expressed as the desired velocity of the body-down frame # with respect to the world frame parameterized in the body-down frame self.velocity_setpoint = None
def __init__(self): rospy.init_node('listener_results', anonymous=True) self.rate = rospy.Rate(20.0) # MUST be more then 2Hz mavros.set_namespace('mavros') state_sub = rospy.Subscriber(mavros.get_topic('state'), mavros_msgs.msg.State, self.state_cb) # "/mavros/setpoint_velocity/cmd_vel" vel_pub_sub = rospy.Subscriber( mavros.get_topic('setpoint_velocity', 'cmd_vel'), TwistStamped, self.vel_pub_cb) vel_local_sub = rospy.Subscriber( mavros.get_topic('local_position', 'velocity_local'), TwistStamped, self.vel_local_cb) self.current_state = mavros_msgs.msg.State() self.vel_pub = [0.0] * 4 self.vel_local = [0.0] * 4 self.vel_pub_x = [] self.vel_pub_y = [] self.vel_pub_z = [] self.vel_local_x = [] self.vel_local_y = [] self.vel_local_z = [] #rospy.spin() self.show() self.plot()
def main(): rospy.init_node('FLYtoPOS') mavros.set_namespace() # initialize mavros module with default namespace rate = rospy.Rate(25) rospy.loginfo("Starting...") position_msg = PoseStamped() position_msg.header.frame_id = "copter_home" x = [0.0, 0.0, 5.0, 5.0, 0.0] y = [0.0, 5.0, 5.0, 0.0, 0.0] z = [5.0, 5.0, 5.0, 5.0, 0.0] while not rospy.is_shutdown(): global iterator position_msg.header.stamp = rospy.Time.now() position_msg.pose.position.x = x[iterator] position_msg.pose.position.y = y[iterator] position_msg.pose.position.z = z[iterator] pubPosition.publish(position_msg) rate.sleep() rospy.loginfo("Bye!")
def main(): rospy.init_node('FLYtoPOS') mavros.set_namespace() # initialize mavros module with default namespace rate = rospy.Rate(25) rospy.loginfo("Starting...") print("Starting...") position_msg = PoseStamped() position_msg.header.frame_id = "home" global target_position_x global target_position_y while not rospy.is_shutdown(): position_msg.header.stamp = rospy.Time.now() position_msg.pose.position.x = target_position_x position_msg.pose.position.y = target_position_y position_msg.pose.position.z = 8 pubPosition.publish(position_msg) print(position_msg) rate.sleep() rospy.loginfo("Bye!")
def __init__(self, filename=None): self.ser = serial.Serial(SERIAL_PORT, 9600) if filename: self.log_file = open("logs/%s_log.csv" % filename, 'w') else: self.log_file = open( "logs/%s_log.csv" % datetime.datetime.now().strftime('%m%d%H%M%S'), 'w') self.log_file.write( "timestamp,vel_x,vel_y,vel_z,acc_x,acc_y,acc_z,roll,pitch,yaw,rc0,rc1,rc2,rc3,vol,cur_raw,cur,power,act_vx,act_vy,act_vz,mode\n" ) mavros.set_namespace() rospy.init_node('aero_energy_logger') self.cur_state = State() self.velocity = TwistStamped() self.rc_in = RCIn() self.imu = Imu() self.manual_control = ManualControl() self.battery_state = BatteryState() self.pose = PoseStamped() self.cur_val_list = [0] * FIELD_NUM print "initialize subscribers..." self.init_subscribers() print "initialize services..." self.init_services() print "initialize publishers..." self.init_publishers() self.start_time = time.time()
def __init__(self): rospy.init_node('gps_goto') mavros.set_namespace() # initialize mavros module with default namespace self.x = 0.0 self.y = 0.0 self.z = 0.0 self.current_lat = 0.0 self.current_lon = 0.0 self.current_alt = 0.0 self.throttle_update = 0.0 self.attitude_publish = True self.pid_throttle = pid_controller.PID(P = 0.8, I = 0.8, D = 0.8, Integrator_max=1, Integrator_min=0) self.pid_throttle.setPoint(10) self.setHome = False self.home_lat = 0.0 self.home_lon = 0.0 self.home_alt = 0.0 self.publish = True self.velocity_init()
def __init__(self, copter_id = "1", mavros_string="/mavros/copter1"): rospy.init_node('velocity_goto_'+copter_id) mavros.set_namespace(mavros_string) # initialize mavros module with default namespace self.pid_alt = pid_controller.PID() self.mavros_string = mavros_string self.final_alt = 0.0 self.final_pos_x = 0.0 self.final_pos_y = 0.0 self.final_vel = 0.0 self.cur_rad = 0.0 self.cur_alt = 0.0 self.cur_pos_x = 0.0 self.cur_pos_y = 0.0 self.cur_vel = 0.0 self.vx = 0.0 self.vy = 0.0 self.vz = 0.0 self.alt_control = False self.override_nav = False self.reached = False self.done = False self.last_sign_dist = 0.0
def __init__(self): rospy.init_node('gps_goto') mavros.set_namespace( ) # initialize mavros module with default namespace self.x = 0.0 self.y = 0.0 self.z = 0.0 self.current_lat = 0.0 self.current_lon = 0.0 self.current_alt = 0.0 self.throttle_update = 0.0 self.attitude_publish = True self.pid_throttle = pid_controller.PID(P=0.8, I=0.8, D=0.8, Integrator_max=1, Integrator_min=0) self.pid_throttle.setPoint(10) self.setHome = False self.home_lat = 0.0 self.home_lon = 0.0 self.home_alt = 0.0 self.publish = True self.velocity_init()
def setpoint_demo(): rospy.init_node('setpoint_position_demo') mavros.set_namespace("/mavros") # initialize mavros module with default namespace rate = rospy.Rate(10) setpoint = SetpointPosition() rospy.loginfo("Climb") setpoint.set(0.0, 0.0, 3.0, 0) setpoint.set(0.0, 0.0, 10.0, 5) rospy.loginfo("Sink") setpoint.set(0.0, 0.0, 8.0, 5) rospy.loginfo("Fly to the right") setpoint.set(10.0, 4.0, 8.0, 5) rospy.loginfo("Fly to the left") setpoint.set(0.0, 0.0, 8.0, 5) offset_x = 0.0 offset_y = 0.0 offset_z = 10.0 sides = 360 radius = 20 rospy.loginfo("Fly in a circle") setpoint.set(0.0, 0.0, 10.0, 3) # Climb to the starting height first i = 0 while not rospy.is_shutdown(): x = radius * cos(i * 2 * pi / sides) + offset_x y = radius * sin(i * 2 * pi / sides) + offset_y z = offset_z wait = False delay = 0 if (i == 0 or i == sides): # Let it reach the setpoint. wait = True delay = 5 setpoint.set(x, y, z, delay, wait) i = i + 1 rate.sleep() if (i > sides): rospy.loginfo("Fly home") setpoint.set(0.0, 0.0, 10.0, 5) break # Simulate a slow landing. setpoint.set(0.0, 0.0, 8.0, 5) setpoint.set(0.0, 0.0, 3.0, 5) setpoint.set(0.0, 0.0, 2.0, 2) setpoint.set(0.0, 0.0, 1.0, 2) setpoint.set(0.0, 0.0, 0.0, 2) setpoint.set(0.0, 0.0, -0.2, 2) rospy.loginfo("Bye!")
def __init__(self): if not MAVLINK: mavros.set_namespace() self.last_rcio_power = RCIOPower() self.publisher = rospy.Publisher('/mavros/rc/override', mavros_msgs.msg.OverrideRCIn) self.in_subscriber = rospy.Subscriber('/mavros/rc/in', mavros_msgs.msg.RCIn, callback=log) self.out_subscriber = rospy.Subscriber('/mavros/rc/out', mavros_msgs.msg.RCOut, callback=log) self.arm_function = rospy.ServiceProxy( mavros.get_topic('cmd', 'arming'), mavros_msgs.srv.CommandBool).call alt_hold_msg = mavros_msgs.msg.OverrideRCIn() alt_hold_msg.channels = [ 0xffff, 0xffff, 0xffff, 0xffff, 1500, 0xffff, 0xffff, 0xffff ] self.publisher.publish(alt_hold_msg) else: self.master = mavutil.mavlink_connection("/dev/ttyACM0", baud=57600) print("waiting for heartbeat...") self.master.wait_heartbeat() print("Connected") self.master.mav.request_data_stream_send( master.target_system, self.master.target_component, mavutil.mavlink.MAV_DATA_STREAM_ALL, 4, 1)
def __init__(self): # Arm the drone mavros.set_namespace() command.arming(True) self.pub_sp = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10) rospy.wait_for_service('mavros/set_mode') change_mode = rospy.ServiceProxy('mavros/set_mode', SetMode) rospy.init_node('gotowaypoint', anonymous=True) rate = rospy.Rate(50) rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self._local_pose_cb) self.waypoint = PoseStamped() self.waypoint.header.frame_id = 'map' self.waypoint.pose.position.x = 0 self.waypoint.pose.position.y = 0 self.waypoint.pose.position.z = 5 self.marker_pub = rospy.Publisher('/waypoint_marker', Marker, queue_size=10) self.waypoint_marker = Marker() self.waypoints = np.array([[0,0,0], [0,1,1], [0,3,3]]) self.publish_waypoint() while not rospy.is_shutdown(): result_mode = change_mode(0,"OFFBOARD") self.pub_sp.publish(self.waypoint) rate.sleep()
def __init__(self, collect=False): rospy.loginfo("Starting robot...") self.controls = [0, 0, 0, 0, 0, 0, 0] self.fwd_image_sub = rospy.Subscriber("camera/image_raw", Image, self.fwd_image_cb) self.dwn_image_sub = rospy.Subscriber("webcam/image_raw", Image, self.dwn_image_cb) self.controls_sub = rospy.Subscriber("mavros/rc/out", RCOut, self.controls_cb) self.arm_srv = rospy.ServiceProxy("/mavros/cmd/arming", CommandBool) self.motor_pub = rospy.Publisher("/mavros/rc/override", OverrideRCIn, queue_size=1000) self.bridge = CvBridge() self.fwd_img_num = 0 self.dwn_img_num = 0 self.fwd_img_csv_fname = "/media/nvidia/fwd_img_timestamp.csv" self.dwn_img_csv_fname = "/media/nvidia/dwn_img_timestamp.csv" self.img_path = "/media/nvidia/images/" self.data_collection = collect if self.data_collection: f = open(self.fwd_img_csv_fname, "w+") f.write( "timestamp (seconds from epoch), forward camera image filename\n" ) f.close() f = open(self.dwn_img_csv_fname, "w+") f.write( "timestamp (seconds from epoch), downward camera image filename\n" ) f.close() mavros.set_namespace() self.armed = False rospy.wait_for_service("/mavros/cmd/arming")
def __init__(self): # Reqired call - I think if you are running more than one drone it allows you to give them different topic namespaces, but defaults to mavros mavros.set_namespace() self._curr_state = State() self._is_done = False self.flight_path = [] self.rate = rospy.Rate(20) self.nextPos = PoseStamped() self.lat = 0 self.lng = 0 # Various ros publishers and subscribers - somewhat self documenting self.gps_subscriber = rospy.Subscriber( mavros.get_topic('global_position', 'global'), NavSatFix, self._get_fix) self.state_subscriber = rospy.Subscriber("mavros/state", State, self._update_state) self._local_position_publisher = rospy.Publisher( "mavros/setpoint_position/local", PoseStamped, queue_size=10) self._arming_client = rospy.ServiceProxy("mavros/cmd/arming", CommandBool) self._set_mode_client = rospy.ServiceProxy("mavros/set_mode", SetMode) self._takeoff_client = rospy.ServiceProxy("mavros/cmd/takeoff", CommandTOL)
def setpoint_demo(): rospy.init_node('setpoint_position_demo') #mavros.set_namespace() # initialize mavros module with default namespace mavros.set_namespace('/ardrone/mavros') rate = rospy.Rate(10) setpoint = SetpointPosition() time.sleep(1) print setpoint.x, setpoint.y rospy.loginfo("Climb") setpoint.set(setpoint.currentPoseX, setpoint.currentPoseY, 5.0, 2) setpoint.set(setpoint.currentPoseX, setpoint.currentPoseY, 8.0, 5) setpoint.set(setpoint.currentPoseX, setpoint.currentPoseY, 10.0, 5) setpoint.set(setpoint.currentPoseX, setpoint.currentPoseY, 15.0, 5) setpoint.set(setpoint.currentPoseX, setpoint.currentPoseY, 18.0, 5) setpoint.set(setpoint.currentPoseX, setpoint.currentPoseY, 20.0, 5) rospy.loginfo("Sink") setpoint.set(0.0, 0.0, 20.0, 5) # Simulate a slow landing. setpoint.set(0.0, 0.0, 18.0, 5) setpoint.set(0.0, 0.0, 15.0, 5) setpoint.set(0.0, 0.0, 10.0, 2) setpoint.set(0.0, 0.0, 8.0, 5) setpoint.set(0.0, 0.0, 5.0, 5) setpoint.set(0.0, 0.0, -0.2, 5) rospy.loginfo("Bye!")
def main(): rospy.init_node('default_objrecog', anonymous=True) rate = rospy.Rate(10) mavros.set_namespace('/mavros') subscriber = rospy.Subscriber("/iris/camera/image_raw", Image, callback, queue_size=1) pub_cent = rospy.Publisher( '/centroid/location', String, queue_size=10) #/object/centroid #string and not Float pub_stat = rospy.Publisher('marker/location', Float32MultiArray, queue_size=10) #print("test") while not rospy.is_shutdown(): #rospy.loginfo(centroids_str) #pub_cent.publish(centroids_str) pub_stat.publish(centroids_float) rate.sleep() # k = cv2.waitKey(1) & 0xff # if k == 27: break return 0
def __init__(self, namespace="mavros"): self.rate = rospy.Rate(20) self.current_pose = PoseStamped() self.current_velocity = TwistStamped() self.target_pose = PoseStamped() self.UAV_state = mavros_msgs.msg.State() mavros.set_namespace(namespace) # setup subscriber self._state_sub = rospy.Subscriber(mavros.get_topic('state'), State, self._state_callback) self._local_position_sub = rospy.Subscriber( mavros.get_topic('local_position', 'pose'), PoseStamped, self._local_position_callback) self._local_velocity_sub = rospy.Subscriber( mavros.get_topic('local_position', 'velocity_body'), TwistStamped, self._local_velocity_callback) # setup publisher self._setpoint_local_pub = mavros.setpoint.get_pub_position_local( queue_size=10) # setup service self.set_arming = rospy.ServiceProxy(mavros.get_topic('cmd', 'arming'), mavros_msgs.srv.CommandBool) self.set_mode = rospy.ServiceProxy(mavros.get_topic('set_mode'), mavros_msgs.srv.SetMode)
def main(): # Initialize Node class parser = argparse.ArgumentParser( description="Teleoperation script for Copter-UAV") rospy.init_node('px4_control', anonymous=True) parser.add_argument('-n', '--mavros-ns', help="ROS node namespace", default="/mavros") global args args = parser.parse_args(rospy.myargv(argv=sys.argv)[1:]) threading.Thread(target=velocity_call).start() mavros.set_namespace(args.mavros_ns) #orig_settings = termios.tcgetattr(sys.stdin) #tty.setraw(sys.stdin) px4C = px4_planner() # spin the node try: rospy.spin() # # stop the node if Ctrl-C pressed except KeyboardInterrupt: print("Shutting down") cv2.destroyAllWindows()
def setpoint_demo(): rospy.init_node('setpoint_position_demo_1') #mavros.set_namespace() # initialize mavros module with default namespace mavros.set_namespace('/uav_1/mavros') rate = rospy.Rate(10) setpoint = SetpointPosition() time.sleep(1) rospy.loginfo("Climb") setpoint.takeoff(1.0) # setpoint.navigate() rospy.loginfo("Moving to Pose 1") #setpoint.setPose(0,0,1,5) rospy.loginfo("Moving to Pose 1") setpoint.setPose(7,-1,1,5) rospy.loginfo("Moving to Pose 1") setpoint.setPose(7.862669,-8.131087,1,5) rospy.loginfo("Moving to Pose 1") setpoint.setPose(-3.296570,-8.891940,1,5) rospy.loginfo("Moving to Pose 1") setpoint.setPose(-3.413211,-6,1,5) rospy.loginfo("Moving to Pose 1") setpoint.setPose(-4.830871,-6,1,5) rospy.loginfo("Moving to Pose 1") setpoint.setPose(-6.704798,-6,1,5) rospy.loginfo("Moving to Pose 1") setpoint.setPose(-6.692234,-8,1,5) rospy.loginfo("Moving to Pose 1") setpoint.setPose(-6.362257,7.847113,1,5) rospy.loginfo("Moving to Pose 1") setpoint.setPose(-4.927931,5.589492,1,5) rospy.loginfo("Moving to Pose 1") setpoint.setPose(-2.475843,6.678944,1,5) rospy.loginfo("Moving to Pose 1") setpoint.setPose(-3,8.609401,1,5) rospy.loginfo("Moving to Pose 1") setpoint.setPose(8.157134,8.609435,1,5) rospy.loginfo("Moving to Pose 1") setpoint.setPose(8.201663,1.988020,1,5) rospy.loginfo("Moving to Pose 1") setpoint.setPose(-3,1.942599,1,5) rospy.loginfo("Landing") setpoint.land() rospy.loginfo("Bye!")
def setpoint_demo(): rospy.init_node('setpoint_position_demo') mavros.set_namespace() rate = rospy.Rate(10) setpoint = SetPointPosition() rospy.loginfo("Climb") setpoint.set(0.0, 0.0, 0.5, 0) setpoint.set(0.0, 0.0, 2.0, 5) rospy.loginfo("Sink") setpoint.set(0.0, 0.0, 1.0, 5) rospy.loginfo("Climb") setpoint.set(0.0, 0.0, 2.0, 5) rospy.loginfo("Fly to the right") setpoint.set( 10.0, 4.0, 8.0, 5) rospy.loginfo("Fly to the left") setpoint.set( 0.0, 0.0, 8.0, 5) rospy.loginfo("Landing") setpoint.set(0.0, 0.0, 8.0, 5) setpoint.set(0.0, 0.0, 3.0, 5) setpoint.set(0.0, 0.0, 2.0, 2) setpoint.set(0.0, 0.0, 1.0, 2) setpoint.set(0.0, 0.0, 0.0, 2) setpoint.set(0.0, 0.0, -0.2, 2)
def main(): rospy.init_node('FLYtoPOS') mavros.set_namespace() # initialize mavros module with default namespace rate = rospy.Rate(25) rospy.loginfo("Starting...") print("Starting...") position_msg = PoseStamped() position_msg.header.frame_id = "home" global target_position_x global target_position_y global target_position_z global time_to_next_z_change global transformData transformData = TransformStamped() #setup object to receive transform tfBuffer = tf2_ros.Buffer() listener = tf2_ros.TransformListener(tfBuffer) #setup tf listener while not rospy.is_shutdown(): #lookup for the newest transform #transformData = tfBuffer.lookup_transform('usb_camera::camera_link', 'ar_marker_13', rospy.Time(0)) try: transformData = tfBuffer.lookup_transform('map', 'ar_marker_13', rospy.Time(0)) follow_ar() print transformData print '########' except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): pass time_now = time.time() if (time_now - time_prev) >= time_to_next_z_change: time_to_next_z_change = 1 if position_msg.pose.position.z < 5: target_position_z = target_position_z - 1 else: target_position_z = target_position_z - 0.2 if position_msg.pose.position.z < 0.1: target_position_z = 0 position_msg.header.stamp = rospy.Time.now() position_msg.pose.position.x = target_position_x position_msg.pose.position.y = target_position_y position_msg.pose.position.z = target_position_z print target_position_x print target_position_y print target_position_z pubPosition.publish(position_msg) #print(position_msg) rate.sleep() rospy.loginfo("Bye!")
def __init__(self, actionServer, goal): '''Initialize ros publisher, ros subscriber''' rospack = rospkg.RosPack() packagePath = rospack.get_path('kuri_object_tracking') with open(packagePath+'/config/colors.yaml', 'r') as stream: try: config_yaml = yaml.load(stream) self.H_red = config_yaml.get('H_red') self.S_red = config_yaml.get('S_red') self.V_red = config_yaml.get('V_red') self.H_blue = config_yaml.get('H_blue') self.S_blue = config_yaml.get('S_blue') self.V_blue = config_yaml.get('V_blue') self.H_green = config_yaml.get('H_green') self.S_green = config_yaml.get('S_green') self.V_green = config_yaml.get('V_green') except yaml.YAMLError as exc: print(exc) self.navigate_started = False self.bridge = CvBridge() self.actionServer = actionServer self.obstacles = Objects() self.image_sub = message_filters.Subscriber('/uav_'+str(goal.uav_id)+'/downward_cam/camera/image', Image) self.info_sub = message_filters.Subscriber('/uav_'+str(goal.uav_id)+'/downward_cam/camera/camera_info', CameraInfo) #self.image_sub = message_filters.Subscriber('/usb_cam/image_raw', Image) #self.info_sub = message_filters.Subscriber('/usb_cam/camera_info', CameraInfo) #self.outpub = rospy.Publisher('/uav_'+str(goal.uav_id)+'/downward_cam/camera/detection_image',Image, queue_size=100, latch=True) self.ts = message_filters.TimeSynchronizer([self.image_sub, self.info_sub], 10) self.ts.registerCallback(self.callback) mavros.set_namespace('/uav_'+str(goal.uav_id)+'/mavros') self.currentPoseX = -1 self.currentPoseY = -1 self.currentPoseZ = -1 self.pub = SP.get_pub_position_local(queue_size=10) self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'), SP.PoseStamped, self.updatePosition) self.image = None self.left_image = None self.right_image = None self.data_small = np.genfromtxt(packagePath+'/config/small.dat', np.float32, delimiter=',') self.data_big = np.genfromtxt(packagePath+'/config/big.dat', np.float32, delimiter=',') self.data_dropzone = np.genfromtxt(packagePath+'/config/dropzone.dat', np.float32, delimiter=',') self.samples = np.reshape(self.data_small, (-1, 7)) self.samples_big = np.reshape(self.data_big, (-1, 7)) self.samples_dropzone = np.reshape(self.data_dropzone, (-1, 7)) self.objects_count = 0 self.frame = 0 self.new_objects = Objects() self.new_objects_count = 0 self.navigating = False self.done = False self.edgeThresholdSize = 0.1 self.width = 1600 self.height = 1200
def joy_cb(self, data): if self.state_mach == self.DISARMED and data.buttons[20] == 1: mavros.set_namespace() self.set_offboard_mode() elif self.state_mach == self.OFFBOARD and data.buttons[3] == 1: command.arming(True) elif self.state_mach == self.ARMED and data.buttons[4] == 1: command.arming(False)
def __init__(self): super().__init__() self.mode_data = {} mavros.set_namespace("mavros") # register mode control self.mode_control = rospy.ServiceProxy(mavros.get_topic("set_mode"), SetMode) self.mode_control_type = GraphQLObjectType( "Mode", lambda: { "uuid": GraphQLField(GraphQLString, description="The uuid of the vehicle"), "modeString": GraphQLField(GraphQLString, description=""), "baseMode": GraphQLField(GraphQLInt, description=""), "customMode": GraphQLField(GraphQLInt, description=""), "updateTime": GraphQLField(GraphQLInt, description=""), "returncode": GraphQLField(GraphQLInt, description=""), }, description="Mode control", ) self.q = { "Mode": GraphQLField( self.mode_control_type, args={ "uuid": GraphQLArgument( GraphQLNonNull(GraphQLString), description="The uuid of the target vehicle", ) }, resolve=self.get_mode, ) } self.m = { "Mode": GraphQLField( self.mode_control_type, args=self.get_mutation_args(self.mode_control_type), resolve=self.update_mode, ) } self.s = { "Mode": GraphQLField(self.mode_control_type, subscribe=self.sub_mode, resolve=None) }
def init(): #rospy.init_node('mavsys', anonymous=True) #rospy.init_node('mavsafety', anonymous=True) #rospy.init_node('mavsetp', anonymous=True) #rospy.init_node('mavcmd', anonymous=True) mavros.set_namespace('/mavros') rospy.init_node('time', anonymous=True) return rospy.Publisher('/mavros/rc/override', OverrideRCIn, queue_size=10)
def setpoint_demo(): rospy.init_node('setpoint_position_demo_1') #mavros.set_namespace() # initialize mavros module with default namespace mavros.set_namespace('/uav_1/mavros') rate = rospy.Rate(10) setpoint = SetpointPosition() time.sleep(1) rospy.loginfo("Climb") setpoint.takeoff(1.0) # setpoint.navigate() rospy.loginfo("Moving to Pose 1") #setpoint.setPose(0,0,1,5) rospy.loginfo("Moving to Pose 1") setpoint.setPose(7, -1, 1, 5) rospy.loginfo("Moving to Pose 1") setpoint.setPose(7.862669, -8.131087, 1, 5) rospy.loginfo("Moving to Pose 1") setpoint.setPose(-3.296570, -8.891940, 1, 5) rospy.loginfo("Moving to Pose 1") setpoint.setPose(-3.413211, -6, 1, 5) rospy.loginfo("Moving to Pose 1") setpoint.setPose(-4.830871, -6, 1, 5) rospy.loginfo("Moving to Pose 1") setpoint.setPose(-6.704798, -6, 1, 5) rospy.loginfo("Moving to Pose 1") setpoint.setPose(-6.692234, -8, 1, 5) rospy.loginfo("Moving to Pose 1") setpoint.setPose(-6.362257, 7.847113, 1, 5) rospy.loginfo("Moving to Pose 1") setpoint.setPose(-4.927931, 5.589492, 1, 5) rospy.loginfo("Moving to Pose 1") setpoint.setPose(-2.475843, 6.678944, 1, 5) rospy.loginfo("Moving to Pose 1") setpoint.setPose(-3, 8.609401, 1, 5) rospy.loginfo("Moving to Pose 1") setpoint.setPose(8.157134, 8.609435, 1, 5) rospy.loginfo("Moving to Pose 1") setpoint.setPose(8.201663, 1.988020, 1, 5) rospy.loginfo("Moving to Pose 1") setpoint.setPose(-3, 1.942599, 1, 5) rospy.loginfo("Landing") setpoint.land() rospy.loginfo("Bye!")
def __init__(self, hz=60): rospy.loginfo("ARObstacleController Started!") mavros.set_namespace() self.feed_sub = rospy.Subscriber( "/camera/rgb/image_rect_color/compressed", CompressedImage, self.image_cb) self.state_sub = rospy.Subscriber("/mavros/state", State, self.state_cb) self.local_pose_sub = rospy.Subscriber("/mavros/local_position/pose", PoseStamped, self.local_pose_cb) self.local_pose_sp_pub = rospy.Publisher( "/mavros/setpoint_position/local", PoseStamped, queue_size=1) if _INTEGRATED: self.local_vel_sp_pub = rospy.Publisher("/ar_vel", TwistStamped, queue_size=1) else: self.local_vel_sp_pub = rospy.Publisher( "/mavros/setpoint_velocity/cmd_vel", TwistStamped, queue_size=1) self.pub_error = rospy.Publisher( "/obs_error", Twist, queue_size=1) # set data type to publish to error self.ar_vel = rospy.Publisher("/ar_vel", TwistStamped, queue_size=1) self.ar_pose_sub = rospy.Subscriber("/ar_aero_pose", AlvarMarkers, self.ar_pose_cb) self.rate = rospy.Rate(hz) self.current_state = State() self.current_pose = None self.current_vel = None ''' 0 is hovering in space, 1 is flying to obstacle 2 is ring flythru (marker id: 24) 3 is hurdle flyover (marker id: 12) 4 is gate flyunder (marker id: 9) ''' self.finite_state = 0 self.markers = [] self.vel_hist = [[], [], [], []] self.current_obstacle_seq = 0 self.current_obstacle_tag = None self.t_marker_last_seen = None self.t_obstacle_start = None self.local_vel_sp = TwistStamped() self.local_pose_sp = None self.offboard_vel_streaming = False self.tl = tf.TransformListener()
def main(): rospy.init_node('breck_field_gps_test1') mavros.set_namespace() # Create logger logger = Logger('logs/breck_field_gps_test1') # Connect to vehicle logger.log_info('Initializing vehicle...') vehicle = Vehicle(logger) # Initialize setpoints logger.log_info('Initializing setpoints...') gps_sp = GPS_Setpoint(vehicle) vel_sp = Velocity_Setpoint(vehicle) # Set to takeoff logger.log_info('Taking off!') if not vehicle.set_mode('AUTO.TAKEOFF'): logger.log_warning('Mode change rejected.') vehicle.arm() time.sleep(15) # Navigate to target detection location logger.log_info('Setting GPS location...') gps_sp.set(37.897002, -86.581023, 10) # Set back to OFFBOARD mode logger.log_info('Entering OFFBOARD mode and navigating to GPS location...') if not vehicle.set_mode('OFFBOARD'): logger.log_warning('Mode change rejected.') return # Wait until reached gps_sp.wait_until_reached() logger.log_info('Location reached! Loitering for 15 seconds...') logger.log_info('Entering LOITER mode...') if not vehicle.set_mode('AUTO.LOITER'): logger.log_warning('Mode change rejected.') return time.sleep(5) if not vehicle.set_mode('AUTO.LAND'): logger.log_warning('Mode change rejected.') return gps_sp.terminate() vel_sp.terminate() # Done logger.log_info('Done!')
def __init__(self): # Setup the nodes and the namespace # If the drone is a slave generate the master ID rospy.init_node("uav_pos_services", anonymous=True) self.rate = rospy.Rate(20) mavros.set_namespace('mavros') # Initialize the parameters self.local_pos = [0.0] * 4 self.global_pos = [0.0] * 4 self.UAV_state = mavros_msgs.msg.State() # setup subscribers # /mavros/state state_sub = rospy.Subscriber(mavros.get_topic('state'), mavros_msgs.msg.State, self._state_callback) # /mavros/local_position/pose local_position_sub = rospy.Subscriber( mavros.get_topic('local_position', 'pose'), SP.PoseStamped, self._local_position_callback) # /mavros/global_position/global global_position_sub = rospy.Subscriber('mavros/global_position/global', NavSatFix, self._global_position_callback) # setup publisher # /mavros/setpoint/position/local self.setpoint_local_pub = mavros.setpoint.get_pub_position_local( queue_size=10) # setup the services # /mavros/cmd/arming self.set_arming = rospy.ServiceProxy('mavros/cmd/arming', mavros_msgs.srv.CommandBool) # /mavros/set_mode self.set_mode = rospy.ServiceProxy('mavros/set_mode', mavros_msgs.srv.SetMode) # Setup global and local service server sg = rospy.Service("target_global_pos", target_global_pos, self.GotoGlobPos) sl = rospy.Service("target_local_pos", target_local_pos, self.GotoLocPos) self.setpoint_msg = mavros.setpoint.PoseStamped( header=mavros.setpoint.Header(frame_id="att_pose", stamp=rospy.Time.now()), ) rospy.loginfo("The target services were successfully initiated") rospy.spin()
def __init__(self): self.local_pos = [0.0] * 4 rospy.init_node("uav_node") self.rate = rospy.Rate(20) mavros.set_namespace('mavros') signal.signal(signal.SIGINT, self.signal_handler) self.UAV_state = mavros_msgs.msg.State() # setup subscribers # /mavros/state state_sub = rospy.Subscriber(mavros.get_topic('state'), mavros_msgs.msg.State, self._state_callback) # /mavros/imu/data rospy.Subscriber('/mavros/imu/data', Imu, self.IMU_callback) # # /mavros/local_position/pose local_position_sub = rospy.Subscriber( mavros.get_topic('local_position', 'pose'), SP.PoseStamped, self._local_position_callback) # /mavros/setpoint_raw/target_local setpoint_local_sub = rospy.Subscriber( mavros.get_topic('setpoint_raw', 'target_local'), mavros_msgs.msg.PositionTarget, self._setpoint_position_callback) # setup services # /mavros/cmd/arming self.set_arming = rospy.ServiceProxy('mavros/cmd/arming', mavros_msgs.srv.CommandBool) # mavros/cmd/command self.command_srv = rospy.ServiceProxy('mavros/cmd/command', mavros_msgs.srv.CommandLong) # /mavros/set_mode self.set_mode = rospy.ServiceProxy('mavros/set_mode', mavros_msgs.srv.SetMode) # goto_pos_services self.goto_loc_pos_serv = rospy.ServiceProxy("target_local_pos", target_local_pos) self.goto_pid_pos_serv = rospy.ServiceProxy("goto_pid_service", goto_pid) # aruco based services # self.goto_aruco_serv = rospy.ServiceProxy('goto_aruco', goto_aruco) # self.land_aruco_serv = rospy.ServiceProxy('land_aruco', land_aruco) # wait for FCU connection while (not self.UAV_state.connected): self.rate.sleep() rospy.loginfo("Uav was successfully connected") self.InputHandler()
def setpoint_demo(): rospy.init_node('setpoint_position_demo_1') mavros.set_namespace('/uav_1/mavros') rate = rospy.Rate(10) setpoint = SetpointPosition() time.sleep(1) rospy.loginfo("Landing Drone 1") setpoint.land() rospy.loginfo("Bye!")
def __init__(self): # Arm the drone mavros.set_namespace() command.arming(True) self.pub_sp = rospy.Publisher('/mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=10) rospy.init_node('gotowaypoint', anonymous=True) rate = rospy.Rate(60) # 10hz # Parse input #parser = argparse.ArgumentParser() #parser.add_argument("--boundary", help="filepath to the control boundary") #args = parser.parse_args(rospy.myargv(argsv)) #self.pub_sp = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10) rospy.wait_for_service('mavros/set_mode') self.change_mode = rospy.ServiceProxy('mavros/set_mode', SetMode) data = self.load_config('scripts/boundary.yaml') self.boundary = Boundary(data) self.controller = QUAD_position_controller( self.load_config('scripts/gainsStart.yaml')) # Set parameters self.kz = 0.05 self.hoverth = 0.565 # Subscribe to local position self.local_pose = PoseStamped() self.velocity = TwistStamped() rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self._send_command) rospy.Subscriber('/mavros/local_position/velocity_local', TwistStamped, self._read_velocity) # Set Waypoint self.waypoint = PoseStamped() self.waypoint.pose.position.x = -1.4 self.waypoint.pose.position.y = -5 self.waypoint.pose.position.z = 3 self.marker_pub = rospy.Publisher('/waypoint_marker', Marker, queue_size=10) self.waypoint_marker = Marker() self.publish_waypoint() # Define Controller rospy.spin()
def setpoint_demo(): rospy.init_node('setpoint_position_demo') #mavros.set_namespace() # initialize mavros module with default namespace mavros.set_namespace('/iris/mavros') rate = rospy.Rate(10) setpoint = SetpointPosition() rospy.loginfo("Initiate UAV") setpoint.set(-13.0, -8.5, 4.0, 0) rospy.loginfo("Cheers Mate!")
def main(): # print "TASK: "+str(sys.argv) # setup rospy env rospy.init_node('TCS_task', anonymous=True) rate = rospy.Rate(20) mavros.set_namespace('/mavros') # setup local pub setpoint_local_pub = mavros.setpoint.get_pub_position_local(queue_size=10) # setup setpoint_msg setpoint_msg = mavros.setpoint.PoseStamped( header=mavros.setpoint.Header( frame_id="local_pose", stamp=rospy.Time.now()), ) # setup local sub position_local_sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'), SP.PoseStamped, local_position_cb) # setup task pub task_watchdog = TCS_util.Task_watchdog('TASK_LOCAL_GOTO') # interprete the input position setpoint_arg = sys.argv[1].split(' ') setpoint_position.x=float(setpoint_arg[0]) setpoint_position.y=float(setpoint_arg[1]) setpoint_position.z=float(setpoint_arg[2]) print "X: {}, Y: {}, Z: {}".format(setpoint_position.x, setpoint_position.y, setpoint_position.z) # setup setpoint poisiton and prepare to publish the position set_target(setpoint_msg, setpoint_position.x, setpoint_position.y, setpoint_position.z) # In this while loop, do the job. while(not is_reached()): # When the UAV reached the position, # publish the task finished signal and exit setpoint_local_pub.publish(setpoint_msg) # TODO: publish the task status as conducting task_watchdog.report_running() rate.sleep() # TODO: publish the task status as FINISHING task_watchdog.report_finish() return 0
def main(): parser = argparse.ArgumentParser( description="This node launch/terminate processes on events.") parser.add_argument('-n', '--mavros-ns', help="ROS node namespace", default=mavros.DEFAULT_NAMESPACE) args = parser.parse_args(rospy.myargv(argv=sys.argv)[1:]) rospy.init_node("event_launcher") mavros.set_namespace(args.mavros_ns) rospy.loginfo("Starting event launcher...") launcher = Launcher() launcher.spin()
def setUp(self): rospy.init_node('test_node', anonymous=True) #self.helper = PX4TestHelper("mavros_offboard_posctl_test") #self.helper.setUp() mavros.set_namespace('/uav_2/mavros') rospy.Subscriber("/uav_2/mavros/local_position/pose", PoseStamped, self.position_callback) rospy.Subscriber("/uav_2/mavros/global_position/global", NavSatFix, self.global_position_callback) self.pub_spt = rospy.Publisher('/uav_2/mavros/setpoint_position/local', PoseStamped, queue_size=10) rospy.wait_for_service('/uav_2/mavros/cmd/command', 30) self._srv_cmd_long = rospy.ServiceProxy('/uav_2/mavros/cmd/command', CommandLong, persistent=True) self.rate = rospy.Rate(10) # 10hz self.has_global_pos = False self.local_position = PoseStamped() self.armed = False
def __init__(self, copter_id = "1"): self.copter_id = copter_id mavros_string = "/mavros" #rospy.init_node('velocity_goto_'+copter_id) mavros.set_namespace(mavros_string) # initialize mavros module with default namespace self.mavros_string = mavros_string self.final_alt = 0.0 self.final_pos_x = 0.0 self.final_pos_y = 0.0 self.final_vel = 0.0 self.cur_rad = 0.0 self.cur_alt = 0.0 self.cur_pos_x = 0.0 self.cur_pos_y = 0.0 self.cur_vel = 0.0 self.vx = 0.0 self.vy = 0.0 self.vz = 0.0 self.avz = 0.0 self.pose_open = [] self.alt_control = True self.override_nav = False self.reached = True self.done = False self.last_sign_dist = 0.0 # for local button handling self.click = " " self.button_sub = rospy.Subscriber("abpause_buttons", std_msgs.msg.String, self.handle_buttons) # publisher for mavros/copter*/setpoint_position/local self.pub_vel = SP.get_pub_velocity_cmd_vel(queue_size=10) # subscriber for mavros/copter*/local_position/local self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'local'), SP.PoseStamped, self.temp) self.cv_bridge = CvBridge() self.image_data = open('image_data.txt','w')
def setpoint_demo(): rospy.init_node('setpoint_position_demo_2') #mavros.set_namespace() # initialize mavros module with default namespace mavros.set_namespace('/uav_2/mavros') rate = rospy.Rate(10) setpoint = SetpointPosition() time.sleep(1) rospy.loginfo("Climb") setpoint.takeoff(13) rospy.loginfo("Moving to Pose 1") setpoint.setPose(25,-25,13,5) rospy.loginfo("Landing") setpoint.land() rospy.loginfo("Bye!")
def __init__(self): self.current_pose = _vector3() self.setpoint_pose = _vector3() self.mode = "None" self.arm = "None" self.guided = "None" self.timestamp = float(datetime.utcnow().strftime('%S.%f')) self.conn_delay = 0.0 rospy.init_node('UAV_Monitor') mavros.set_namespace("/mavros") # setup local_position sub self.local_position_sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'), SP.PoseStamped, self._local_position_callback) self.setpoint_local_sub = rospy.Subscriber(mavros.get_topic('setpoint_raw', 'target_local'), mavros_msgs.msg.PositionTarget, self._setpoint_position_callback) self.state_sub = rospy.Subscriber(mavros.get_topic('state'), mavros_msgs.msg.State, self._state_callback) pass
def main(): yaw = 0.0 thrust = 0.0 roll = 0.0 pitch = 0.0 flag = True rospy.init_node('setpoint_position_demo') mavros.set_namespace() # initialize mavros module with default namespace rate = rospy.Rate(10) setpoint = SetpointPosition() i = 0 while flag: #flag, roll, pitch, yaw, thrust = input_key(roll, pitch, yaw, thrust) print roll print "here" roll = sin(i) setpoint.set(10, pitch, yaw, thrust) i +=1 rospy.loginfo("Bye!")
def setpoint_demo(): rospy.init_node('explore_arena_integration_test') #mavros.set_namespace() # initialize mavros module with default namespace mavros.set_namespace('/uav_3/mavros') rate = rospy.Rate(10) setpoint = SetpointPosition() time.sleep(1) rospy.loginfo("Climb") setpoint.takeoff(10) rospy.loginfo("Moving to Pose 1") setpoint.setPose(0,0,15,2) rospy.loginfo("Moving to Pose 2") setpoint.setPose(10,10,15,10) rospy.loginfo("Landing") setpoint.land() rospy.loginfo("Bye!")
def __init__(self): self.done = False self.done_evt = threading.Event() self.isExploring = False self.progress = 0.0 self.x = 0.0 self.y = 0.0 self.z = 0.0 self.currentPoseX = 0 self.currentPoseY = 0 self.currentPoseZ = 0 self.navigating = False mavros.set_namespace('/uav_1/mavros') # publisher for mavros/setpoint_position/local self.pub = SP.get_pub_position_local(queue_size=10) # subscriber for mavros/local_position/local self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'), SP.PoseStamped, self.reached) self.objects_map = ObjectsMap() self.client = actionlib.SimpleActionClient('TrackingAction', TrackingAction) #client = self.client #client = self.actionServer.client print "Waiting for tracking server" self.client.wait_for_server() self.goal = TrackingGoal() self.goal.uav_id = 1 self.client.send_goal(self.goal) print "Waiting for result" self.client.wait_for_result() print "Result:" self.objects =self.client.get_result().tracked_objects.objects print self.objects try: thread.start_new_thread(self.navigate, ()) except: fault("Error: Unable to start thread")
def setpoint_demo(): rospy.init_node('setpoint_position_demo') #mavros.set_namespace() # initialize mavros module with default namespace mavros.set_namespace('/iris/mavros') rate = rospy.Rate(10) setpoint = SetpointPosition() rospy.loginfo("climb") setpoint.set(2.0, 0.0, 5.0, 180, 0) setpoint.set(2.0, 0.0, 10.0, 180, 1) setpoint.set(2.0, 0.0, 11.5, 180, 1) rospy.loginfo("move right") setpoint.set(2.0, 3.0, 13.5, 180, 2) setpoint.set(2.0, 5.0, 13.5, 180, 2) setpoint.set(4.0, 10.0, 13.5, 180, 1) setpoint.set(4.0, 25.0, 13.5, 180, 1) setpoint.set(4.0, 27.0, 13.5, 180, 1) setpoint.set(4.0, 31.0, 13.5, 180, 1) rospy.loginfo("rotate") setpoint.set(4.0, 31.0, 13.5, 90, 8) setpoint.set(6.5, 31.0, 10.5, 90, 8) #setpoint.set(9.0, 31.0, 10.5, 90, 5) setpoint.set(6.5, 31.0, 8.5, 90, 8) setpoint.set(12.0, 25.0, 8.5, 90, 5) setpoint.set(14.0, 25.0, 8.5, 90, 5) #setpoint.set(16.0, 26.0, 8.5, 90, 4) #setpoint.set(4.0, 10.0, 12.5, 90, 3) #setpoint.set(4.0, 25.0, 12.5, 90, 3) # Simulate a slow landing. #setpoint.set(0.0, 0.0, 8.0, 5) #setpoint.set(0.0, 0.0, 3.0, 5) #setpoint.set(0.0, 0.0, 2.0, 2) #setpoint.set(0.0, 0.0, 1.0, 2) #setpoint.set(0.0, 0.0, 0.0, 2) #setpoint.set(0.0, 0.0, -0.2, 2) rospy.loginfo("Bye!")
def __init__(self, copter_id = "1"): self.copter_id = copter_id mavros_string = NAMESPACE_PREFIX + copter_id + "/mavros" mavros.set_namespace(mavros_string) # initialize mavros module with default namespace self.mavros_string = mavros_string self.final_alt = 0.0 self.final_pos_x = 0.0 self.final_pos_y = 0.0 self.final_vel = 0.0 self.cur_rad = 0.0 self.cur_alt = 0.0 self.cur_pos_x = 0.0 self.cur_pos_y = 0.0 self.cur_vel = 0.0 self.vx = 0.0 self.vy = 0.0 self.vz = 0.0 self.pose_open = [] self.alt_control = True self.override_nav = False self.reached = True self.done = False self.last_sign_dist = 0.0 # for local button handling self.click = " " self.button_sub = rospy.Subscriber("abpause_buttons", std_msgs.msg.String, self.handle_buttons) # publisher for mavros/copter*/setpoint_position/local self.pub_vel = rospy.Publisher(mavros_string + '/setpoint_velocity/cmd_vel', TwistStamped, queue_size=10) # subscriber for mavros/copter*/local_position/local self.sub = rospy.Subscriber(mavros_string + 'local_position/local', SP.PoseStamped, self.temp)
def setpoint_demo(): settings = termios.tcgetattr(sys.stdin) rospy.init_node('setpoint_position_demo_1') #mavros.set_namespace() # initialize mavros module with default namespace mavros.set_namespace('/uav_1/mavros') rate = rospy.Rate(10) setpoint = SetpointPosition() # time.sleep(1) try: print 'reem' while(1): key = getKey() print 's' #print key if key == 't': setpoint.takeoff(2) if key == 'l': setpoint.land() if key == 'r': setpoint.setPose(1.0,0,0) if key =='f': setpoint.setPose(-1.0,0,0) if key == 'g': setpoint.setPose(0,1.0,0) if key =='d': setpoint.setPose(0,-1.0,0) if key =='y': setpoint.setPose(0,0,0) except: print 'r' #print e termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
def __init__(self, copter_id="1", mavros_string="/mavros/copter1"): #rospy.init_node('velocity_goto') mavros.set_namespace(mavros_string) # initialize mavros module with default namespace self.x = 0.0 self.y = 0.0 self.z = 0.0 self.current_lat = 0.0 self.current_lon = 0.0 self.current_alt = 0.0 self.throttle_update = 0.0 self.attitude_publish = True self.mavros_string = mavros_string self.copter_id = copter_id self.pid_throttle = pid_controller.PID(P = 0.8, I = 0.8, D = 0.8, Integrator_max=1, Integrator_min=0) self.pid_throttle.setPoint(10) self.setHome = False self.home_lat = 0.0 self.home_lon = 0.0 self.home_alt = 0.0 self.publish = True self.velocity_init()
def main(): rospy.init_node('default_offboard', anonymous=True) rate = rospy.Rate(10) mavros.set_namespace('/mavros') # setup subscriber # /mavros/state state_sub = rospy.Subscriber(mavros.get_topic('state'), mavros_msgs.msg.State, _state_callback) # /mavros/local_position/pose # local_position_sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'), # SP.PoseStamped, _local_position_callback) # /mavros/setpoint_raw/target_local setpoint_local_sub = rospy.Subscriber(mavros.get_topic('setpoint_raw', 'target_local'), mavros_msgs.msg.PositionTarget, _setpoint_position_callback) # setup publisher # /mavros/setpoint/position/local setpoint_local_pub = mavros.setpoint.get_pub_position_local(queue_size=10) # setup service # /mavros/cmd/arming set_arming = rospy.ServiceProxy('/mavros/cmd/arming', mavros_msgs.srv.CommandBool) # /mavros/set_mode set_mode = rospy.ServiceProxy('/mavros/set_mode', mavros_msgs.srv.SetMode) setpoint_msg = mavros.setpoint.PoseStamped( header=mavros.setpoint.Header( frame_id="att_pose", stamp=rospy.Time.now()), ) # wait for FCU connection while(not UAV_state.connected): rate.sleep() # create task instance task_goto = Task_GOTO_Local(setpoint_local_pub) task_stay = Task_Stay(setpoint_local_pub) # initialize the setpoint setpoint_msg.pose.position.x = 0 setpoint_msg.pose.position.y = 0 setpoint_msg.pose.position.z = 3 mavros.command.arming(True) # send 100 setpoints before starting for i in range(0,50): setpoint_local_pub.publish(setpoint_msg) rate.sleep() set_mode(0,'OFFBOARD') last_request = rospy.Time.now() step = 1 task_done = False while(True): # print "Entered whiled loop" if( UAV_state.mode != "OFFBOARD" and (rospy.Time.now() - last_request > rospy.Duration(5.0))): if( set_mode(0,'OFFBOARD').success): print "Offboard enabled" last_request = rospy.Time.now() else: if(not UAV_state.armed and (rospy.Time.now() - last_request > rospy.Duration(5.0))): if( mavros.command.arming(True)): print "Vehicle armed" last_request = rospy.Time.now() # print "Before Task loop" if (UAV_state.armed): # print "Now we are in step {0}".format(step) if (step == 1): task_goto.goto(5,5,8.1) task_done = task_goto.check_task() elif (step == 2): # stay at current position for 10 seconds task_stay.stay_at_time(10.0) task_done = task_stay.check_task() elif (step == 3): task_goto.goto(-10,0,5.1) task_done = task_goto.check_task() elif (step == 4): task_goto.goto(0,0,5.1) task_done = task_goto.check_task() else: while (UAV_state.mode != "AUTO.LAND"): set_mode(0,'AUTO.LAND') rate.sleep() return 0 # Exit program if (task_done): step+=1 task_stay.reset_stay() #setpoint_local_pub.publish(setpoint_msg) rate.sleep() return 0
def main(): rospy.init_node('default_offboard', anonymous=True) rate = rospy.Rate(20) mavros.set_namespace('/mavros') # setup subscriber # /mavros/state state_sub = rospy.Subscriber(mavros.get_topic('state'), mavros_msgs.msg.State, _state_callback) # /mavros/local_position/pose # local_position_sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'), # SP.PoseStamped, _local_position_callback) # /mavros/setpoint_raw/target_local setpoint_local_sub = rospy.Subscriber(mavros.get_topic('setpoint_raw', 'target_local'), mavros_msgs.msg.PositionTarget, _setpoint_position_callback) # setup publisher # /mavros/setpoint/position/local setpoint_local_pub = mavros.setpoint.get_pub_position_local(queue_size=10) # setup service # /mavros/cmd/arming set_arming = rospy.ServiceProxy('/mavros/cmd/arming', mavros_msgs.srv.CommandBool) # /mavros/set_mode set_mode = rospy.ServiceProxy('/mavros/set_mode', mavros_msgs.srv.SetMode) setpoint_msg = mavros.setpoint.PoseStamped( header=mavros.setpoint.Header( frame_id="att_pose", stamp=rospy.Time.now()), ) #read task list Task_mgr = TCS_util.Task_manager('task_list.log') # start setpoint_update instance setpoint_keeper = TCS_util.update_setpoint(rospy) # wait for FCU connection while(not UAV_state.connected): rate.sleep() # initialize the setpoint setpoint_msg.pose.position.x = 0 setpoint_msg.pose.position.y = 0 setpoint_msg.pose.position.z = 3 mavros.command.arming(True) # send 100 setpoints before starting for i in range(0,50): setpoint_local_pub.publish(setpoint_msg) rate.sleep() set_mode(0,'OFFBOARD') last_request = rospy.Time.now() # enter the main loop while(True): # print "Entered whiled loop" if( UAV_state.mode != "OFFBOARD" and (rospy.Time.now() - last_request > rospy.Duration(5.0))): if( set_mode(0,'OFFBOARD').success): print "Offboard enabled" last_request = rospy.Time.now() else: if(not UAV_state.armed and (rospy.Time.now() - last_request > rospy.Duration(5.0))): if( mavros.command.arming(True)): print "Vehicle armed" last_request = rospy.Time.now() # update setpoint to stay in offboard mode setpoint_keeper.update() if(Task_mgr.task_finished()): # If the current task has been done rospy.loginfo("Current task is finished!") if (not Task_mgr.alldone()): # If there are tasks left Task_mgr.nexttask() else: # Current task has been done and no task left rospy.loginfo("All tasks have been done!") while (UAV_state.mode != "AUTO.LAND"): set_mode(0,'AUTO.LAND') rate.sleep() return 0 rate.sleep() return 0
def setpoint_demo(): rospy.init_node('setpoint_position_demo') #mavros.set_namespace() # initialize mavros module with default namespace mavros.set_namespace('/iris/mavros') rate = rospy.Rate(10) setpoint = SetpointPosition() #read from a file theFile = open("/home/randa/workspace/catkin_ws/src/aircraft_inspection/inspection/scripts/path_check.txt", "r") rospy.loginfo("File Open") begin= rospy.get_time() # stamp should update rospy.loginfo("TAKEOFF 1") #setpoint.set(4.0, -30.0, 4.0, 3.14, 3) setpoint.set(3.0, -34.5, 5.0, 3.14, 5) rospy.loginfo("TAKEOFF 2") #setpoint.set(4.0, -30.0, 9.0, 3.14, 6) setpoint.set(3.0, -34.5, 9.0, 3.14, 6) rospy.loginfo("MOVING using data from the file") #setpoint.set(4.0, -29.0, 9.0, -2.3562, 8) data = theFile.readlines() temp_vals = [] temp_valsx = [] temp_valsy = [] temp_valsz = [] temp_vals.append(9.0) temp_valsx.append(3.0) temp_valsy.append(-34.5) temp_valsz.append(9.0) index=0 for line in data: index = index+1 poses = line.split() print float(poses[0]) print float(poses[1]) print float(poses[2]) print float(poses[3]) setpoint.set(temp_valsx[index-1], temp_valsy[index-1], temp_valsz[index-1], float(poses[3]), 5)#12 #setpoint.set(float(poses[0]), float(poses[1]), float(poses[2]), float(poses[3]), 7)#12 temp_valsx.append(float(poses[0])) temp_valsy.append(float(poses[1])) temp_valsz.append(float(poses[2])) #temp_vals.append(float(poses[2])) diffx = temp_valsx[index]-temp_valsx[index-1] diffy = temp_valsy[index]-temp_valsy[index-1] diffz = temp_valsz[index]-temp_valsz[index-1] #setpoint.set(float(poses[0]), float(poses[1]), temp_vals[index-1], float(poses[3]), 10) print diffz if diffz>0: # and temp_vals[index]>1.5: print "diff>1" startx = temp_valsx[index-1] starty = temp_valsy[index-1] startz = temp_valsz[index-1] endz = float(poses[2]) stepz = 0.125 stepx = 0.125 stepy = 0.125 if diffx<0: stepx=stepx*-1 elif diffx==0: stepx=0 if diffy<0: stepy=stepy*-1 elif diffy==0: stepy=0 while startz < endz: # and temp_vals[index]>1.5: setpoint.set(startx, starty, startz, float(poses[3]), 1.2) startz += stepz startx += stepx starty += stepy elif diffz<0: # and temp_vals[index]>1.5: print "diff<-1" startx = temp_valsx[index-1] starty = temp_valsy[index-1] startz = temp_valsz[index-1] endz = float(poses[2]) stepz = 0.125 stepx = 0.125 stepy = 0.125 if diffx<0: stepx=stepx*-1 elif diffx==0: stepx=0 if diffy<0: stepy=stepy*-1 elif diffy==0: stepy=0 while startz > endz: setpoint.set(startx, starty, startz, float(poses[3]), 1.2) startz -= stepz startx += stepx starty += stepy else: print "nothing" #if temp_vals[index]>1.5: setpoint.set(float(poses[0]), float(poses[1]), float(poses[2]), float(poses[3]), 5)#12 setpoint.set(float(poses[0]), float(poses[1]), float(poses[2]), float(poses[3]), 5)#12 ######testing### #setpoint.set(4.0, -29.0, 9.0, -2.3562, 8) #setpoint.set(4.0, -29.0, 9.0, 2.3562, 8) #rospy.loginfo("MOVING 3") #setpoint.set(6.0, -29.0, 7.0, 3.14159, 8) #setpoint.set(6.0, -29.0, 5.0, 2.3562, 8) end= rospy.get_time() # stamp should update elapsed =end - begin rospy.loginfo("elapsed time: %d s",elapsed) rospy.loginfo("DONE") theFile.close() #Simulate a slow landing. #setpoint.set(4.0, -28.0, 3.0, 180, 5) #setpoint.set(4.0, -28.0, 2.0, 180, 2) #setpoint.set(4.0, -28.0, 1.0, 180, 2) #setpoint.set(4.0, -28.0, 0.0, 180, 2) #setpoint.set(4.0, -28.0, -0.2, 180, 2) rospy.loginfo("Bye!")
def __init__(self, copter_id = "1", mavros_string="/mavros/copter1"): rospy.init_node('rc_override'+copter_id) mavros.set_namespace(mavros_string) # initialize mavros module with default namespace self.rc = OverrideRCIn() self.override_pub = rospy.Publisher(mavros_string+"/rc/override", OverrideRCIn, queue_size=10)
import sys import time import rospy import thread import threading import mavros from geometry_msgs.msg import PoseStamped, Quaternion from math import * from std_msgs.msg import Header from std_msgs.msg import String from mavros.utils import * from mavros import setpoint as SP from tf.transformations import quaternion_from_euler mavros.set_namespace() class Setpoint: def __init__(self, pub, rospy): self.pub = pub self.rospy = rospy self.x = 0.0 self.y = 0.0 self.z = 0.0 self.curr_x = 0.0 self.curr_y = 0.0 self.curr_z = 0.0 self.yaw = 0.0
#from matplotlib import pyplot as plt from geometry_msgs.msg import PoseStamped, Quaternion from math import * from std_msgs.msg import Header from std_msgs.msg import String from mavros.utils import * from mavros import setpoint as SP from tf.transformations import quaternion_from_euler from threading import Thread, Event #from Stereo_Vision import Stereo_Vision from MAVROS_navigation import Setpoint #from Usonic import usonic mavros.set_namespace() #initialize sonar sensor network and vision system #stereo = Stereo_Vision(cam_L_src=0,cam_R_src=1) #usonic = usonic(stuff) #initailize ROS publisher for setpoints at 10Hz rospy.init_node('setpoint_position_demo') mavros.set_namespace() # initialize mavros module with default namespace rate = rospy.Rate(10) setpoint = Setpoint(sonar_readings=[500,500,500,500,500,500], jMAVSim = True) raw_input("Press any key to begin") setpoint.altitude_change(515)
def setpoint_demo(): rospy.init_node('setpoint_position_demo') #mavros.set_namespace() # initialize mavros module with default namespace mavros.set_namespace('/iris/mavros') rate = rospy.Rate(10) setpoint = SetpointPosition() #read from a file theFile = open("/home/randa/workspace/catkin_ws/src/aircraft_inspection/inspection/scripts/path_check.txt", "r") rospy.loginfo("File Open") begin= rospy.get_time() # stamp should update rospy.loginfo("TAKEOFF 1") setpoint.set(4.5, 24.0, 5.0, 3.14, 5) rospy.loginfo("TAKEOFF 2") setpoint.set(4.5, 24.0, 7.0, 3.14, 6) rospy.loginfo("TAKEOFF 4") setpoint.set(4.5, 24.0, 7.0, 2.3562, 6) rospy.loginfo("MOVING using data from the file") #setpoint.set(4.0, -29.0, 9.0, -2.3562, 8) data = theFile.readlines() temp_vals = [] temp_vals.append(7.0) index=0 for line in data: index = index+1 poses = line.split() print float(poses[0]) print float(poses[1]) print float(poses[2]) print float(poses[3]) temp_vals.append(float(poses[2])) diff = temp_vals[index]-temp_vals[index-1] setpoint.set(float(poses[0]), float(poses[1]), temp_vals[index-1], float(poses[3]), 10) print diff if diff>1.5: # and temp_vals[index]>1.5: print "diff>1" start = temp_vals[index-1] end = float(poses[2]) step = 1.5 while start < end: # and temp_vals[index]>1.5: start += step setpoint.set(float(poses[0]), float(poses[1]), start, float(poses[3]), 4) elif diff<-1.5: # and temp_vals[index]>1.5: print "diff<-1" start = temp_vals[index-1] end = float(poses[2]) step = 1.5 while start > end: start -= step setpoint.set(float(poses[0]), float(poses[1]), start, float(poses[3]), 4) else: print "nothing" #if temp_vals[index]>1.5: setpoint.set(float(poses[0]), float(poses[1]), float(poses[2]), float(poses[3]), 4)#12 ######testing### #setpoint.set(4.0, -29.0, 9.0, -2.3562, 8) #setpoint.set(4.0, -29.0, 9.0, 2.3562, 8) #rospy.loginfo("MOVING 3") #setpoint.set(6.0, -29.0, 7.0, 3.14159, 8) #setpoint.set(6.0, -29.0, 5.0, 2.3562, 8) end= rospy.get_time() # stamp should update elapsed =end - begin rospy.loginfo("elapsed time: %d s",elapsed) rospy.loginfo("DONE") theFile.close() #Simulate a slow landing. #setpoint.set(4.0, -28.0, 3.0, 180, 5) #setpoint.set(4.0, -28.0, 2.0, 180, 2) #setpoint.set(4.0, -28.0, 1.0, 180, 2) #setpoint.set(4.0, -28.0, 0.0, 180, 2) #setpoint.set(4.0, -28.0, -0.2, 180, 2) rospy.loginfo("Bye!")