Ejemplo n.º 1
0
  def set_offboard_mode(self):
      done_evt = threading.Event()
      def state_cb(state):
          if state.armed:
              self.state_mach = self.ARMED
              rospy.loginfo("Drone armed!")
          elif state.mode == "OFFBOARD":
              self.state_mach = self.OFFBOARD
              rospy.loginfo("Drone in OFFBOARD mode!")
              done_evt.set()
          else:
              self.state_mach = self.DISARMED
              rospy.loginfo("Drone disarmed!")

      try:
          set_mode = rospy.ServiceProxy(mavros.get_topic('set_mode'), SetMode)
          ret = set_mode(custom_mode="OFFBOARD")
      except rospy.ServiceException as ex:
          fault(ex)

      sub = rospy.Subscriber(mavros.get_topic('state'), State, state_cb)

      if not ret.mode_sent:
          fault("Request failed. Check mavros logs")

      if not done_evt.wait(5):
          fault("Timed out!")
    def test_mixed_rates(self):
        pub = rospy.Publisher("wolf_twist", Twist, queue_size=0)

        yaw_rate = rospy.wait_for_message(mavros.get_topic("rc", "override"),
                                          OverrideRCIn)
        yaw_rate = rospy.wait_for_message(mavros.get_topic("rc", "override"),
                                          OverrideRCIn)
        twist_out = Twist()

        twist_out.angular.z = 0.1
        twist_out.linear.x = 0.1
        twist_out.linear.z = 0.1
        pub.publish(twist_out)
        yaw_rate = rospy.wait_for_message(mavros.get_topic("rc", "override"),
                                          OverrideRCIn).channels[3]
        lateral_rate = rospy.wait_for_message(
            mavros.get_topic("rc", "override"), OverrideRCIn).channels[4]
        vertical_rate = rospy.wait_for_message(
            mavros.get_topic("rc", "override"), OverrideRCIn).channels[2]
        self.assertEqual(
            yaw_rate, 1550.0,
            "PIXHAWK NODE: RC rate does not match sent rate, was {} should be {}"
            .format(yaw_rate, 1550))
        self.assertEqual(
            lateral_rate, 1550.0,
            "PIXHAWK NODE: RC rate does not match sent rate, was {} should be {}"
            .format(lateral_rate, 1550))
        self.assertEqual(
            vertical_rate, 1550.0,
            "PIXHAWK NODE: RC rate does not match sent rate, was {} should be {}"
            .format(vertical_rate, 1550))
Ejemplo n.º 3
0
    def write(self, file_, parameters):
        def to_type(x):
            if isinstance(x, float):
                return 9  # REAL32
            elif isinstance(x, int):
                return 6  # INT32
            else:
                raise ValueError("unknown type: " + repr(type(x)))

        sysid = rospy.get_param(mavros.get_topic('target_system_id'), 1)
        compid = rospy.get_param(mavros.get_topic('target_component_id'), 1)

        writer = csv.writer(file_, self.CSVDialect)
        writer.writerow(("# NOTE: " + time.strftime("%d.%m.%Y %T"), ))
        writer.writerow(
            ("# Onboard parameters saved by mavparam for ({}, {})".format(
                sysid, compid), ))
        writer.writerow(
            ("# MAV ID", "COMPONENT ID", "PARAM NAME", "VALUE", "(TYPE)"))
        for p in parameters:
            writer.writerow((
                sysid,
                compid,
                p.param_id,
                p.param_value,
                to_type(p.param_value),
            ))  # XXX
Ejemplo n.º 4
0
    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()
Ejemplo n.º 5
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)
Ejemplo n.º 6
0
    def offboard(self):
        if self.armed:
            rate = rospy.Rate(20.0)  # MUST be more then 2Hz
            set_mode_client = rospy.ServiceProxy(mavros.get_topic('set_mode'), SetMode)
            local_pos_pub = rospy.Publisher(mavros.get_topic('setpoint_position', 'local'), PoseStamped, queue_size=10)

            pose = PoseStamped()
            pose.pose.position.x = 0
            pose.pose.position.y = 0
            pose.pose.position.z = self.altitude

            last_request = rospy.get_rostime()
            while not rospy.is_shutdown():
                now = rospy.get_rostime()
                if self.mode != "OFFBOARD" and (now - last_request > rospy.Duration(5)):
                    set_mode_client(base_mode=0, custom_mode="OFFBOARD")
                    rospy.loginfo("Entering OFFBOARD MODE NOW ! Use local waypoints for flying")
                    last_request = now

                pose.header.stamp = rospy.Time.now()
                pose.pose.position.x = pose.pose.position.x + 0.05
                local_pos_pub.publish(pose)
                rate.sleep()
        else:
            rospy.loginfo("Vehicle is not armed, please arm first then proceed !")
    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()
Ejemplo n.º 8
0
    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()
Ejemplo n.º 9
0
	def __init__(self):
		self.current_pose = _vector3()
		self.setpoint_pose = _vector3()
		self.mode = "None"
		self.arm = "None"

		# 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_position', 'pose'),
                                    SP.PoseStamped, self._setpoint_position_callback)

		pass
Ejemplo n.º 10
0
def param_get_all(force_pull=False):
    try:
        pull = rospy.ServiceProxy(mavros.get_topic('param', 'pull'), ParamPull)
        ret = pull(force_pull=force_pull)
    except rospy.ServiceException as ex:
        raise IOError(str(ex))

    if not ret.success:
        raise IOError("Request failed.")

    params = rospy.get_param(mavros.get_topic('param'))

    return (ret.param_received,
            sorted((Parameter(k, v) for k, v in params.iteritems()),
                   cmp=lambda x, y: cmp(x.param_id, y.param_id)))
Ejemplo n.º 11
0
def do_mode(args):
    base_mode = 0
    custom_mode = ''

    if args.custom_mode is not None:
        custom_mode = args.custom_mode.upper()
    if args.base_mode is not None:
        base_mode = args.base_mode

    done_evt = threading.Event()

    def state_cb(state):
        print_if(args.verbose, "Current mode:", state.mode)
        if state.mode == custom_mode:
            print("Mode changed.")
            done_evt.set()

    try:
        set_mode = rospy.ServiceProxy(mavros.get_topic('set_mode'), SetMode)
        ret = set_mode(base_mode=base_mode, custom_mode=custom_mode)
    except rospy.ServiceException as ex:
        fault(ex)

    if not ret.mode_sent:
        fault("Request failed. Check mavros logs")

    if not done_evt.wait(5):
        fault("Timed out!")
Ejemplo n.º 12
0
    def velocity_init(self):
        self.vx = 0.0
        self.vy = 0.0
        self.vz = 0.0
        self.yaw = 0.0
        self.velocity_publish = False
        self.use_pid = True

        self.pid_alt = pid_controller.PID()
        self.pid_alt.setPoint(1.0)
        # publisher for mavros/setpoint_position/local
        self.pub_vel = SP.get_pub_velocity_cmd_vel(queue_size=10)
        # subscriber for mavros/local_position/local
        self.sub = rospy.Subscriber(
            mavros.get_topic('local_position', 'local'), SP.PoseStamped,
            self.temp)

        try:
            thread.start_new_thread(self.navigate, ())
        except:
            fault("Error: Unable to start thread")

        # TODO(simon): Clean this up.
        self.done = False
        self.done_evt = threading.Event()
Ejemplo n.º 13
0
    def __init__(self):
        rospy.init_node('examplePilot')
        self.subState = 'GPS'
        self.GPSMode = True
        self.enable = False
        self.rate = rospy.Rate(20)

        self.exampleGPSPub = rospy.Publisher(targetWP_GPS,
                                             mavSP.PoseStamped,
                                             queue_size=5)
        self.exampleATTIPub = rospy.Publisher(targetWP_Atti,
                                              mavros_msgs.msg.AttitudeTarget,
                                              queue_size=5)
        self.pilotSubstatePub = rospy.Publisher(onB_substateSub,
                                                String,
                                                queue_size=1)

        rospy.Subscriber(onB_StateSub, String, self.onStateChange)
        rospy.Subscriber(
            mavros.get_topic('local_position',
                             'pose'), mavSP.PoseStamped, self.onPositionChange
        )  # uses mavros.get topic to find the name of the current XYZ position of the drone

        self.currUAVPos = mavSP.PoseStamped()  # used for GPS setpoints
        self.msgSPAtti = mavSP.TwistStamped()  # used for Attitude setpoints

        rospy.loginfo('examplePilot Initialised.')
Ejemplo n.º 14
0
    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)
Ejemplo n.º 15
0
def param_set_list(param_list):
    # 1. load parameters to parameter server
    for p in param_list:
        rospy.set_param(mavros.get_topic('param', p.param_id), p.param_value)

    # 2. request push all
    try:
        push = rospy.ServiceProxy(mavros.get_topic('param', 'push'), ParamPush)
        ret = push()
    except rospy.ServiceException as ex:
        raise IOError(str(ex))

    if not ret.success:
        raise IOError("Request failed.")

    return ret.param_transfered
Ejemplo n.º 16
0
 def __init__(self):
     self.hdg_subscriber = rospy.Subscriber(
         "/mavros/global_position/compass_hdg", Float64, self.compass)
     self.rel_alt = rospy.Subscriber("/mavros/global_position/rel_alt",
                                     Float64, self.altitude)
     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)
     self.state_sub = rospy.Subscriber(mavros.get_topic('state'), State,
                                       self.state_cb)
     self.img_sub = rospy.Subscriber("/camera/rgb/image_raw",
                                     Image,
                                     self.call,
                                     buff_size=2**24,
                                     queue_size=1)
     self.mono_sub = rospy.Subscriber("/camera/mono/image_raw",
                                      Image,
                                      self.mono_cb,
                                      buff_size=2**24,
                                      queue_size=1)
     self.tree = (self.achieve | (
         (self.achieve_alt | self.up) >>
         (self.yaw_correction | self.correction) >> self.forward
     )) >> (
         self.ah | (self.isdetect >> self.change_alt)
     )  #>> (self.realsense_check | self.change_alt | (self.mono_detect >> self.change_alt))
Ejemplo n.º 17
0
    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)
Ejemplo n.º 18
0
    def __init__(self):
        self.x = 0.0
        self.y = 0.0
        self.z = 0.0
        self.currentPoseX = 0
        self.currentPoseY = 0
        self.currentPoseZ = 0
        # 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.waypointsub = rospy.Subscriber("/uav_1/waypoint",
                                            SP.PoseStamped,
                                            self.updateposition,
                                            queue_size=1)

        try:
            thread.start_new_thread(self.navigate, ())
        except:
            fault("Error: Unable to start thread")

        # TODO(simon): Clean this up.
        self.done = False
        self.done_evt = threading.Event()
Ejemplo n.º 19
0
def setParam(bot, update):
    cmd = update._effective_message.text
    print cmd
    logger(bot, update)
    params = rospy.get_param(mavros.get_topic('param'))

    newval = float(cmd.split()[3])

    print "Set %s => new value %s, old value %s " % (
        cmd.split()[1], newval, params.get(cmd.split()[1]))
    if cmd.split()[1] == "" or cmd.split()[3] == "":
        update.message.reply_text("Missing data  %s => %s " %
                                  (cmd.split()[1], newval))
        return

    #pdb.set_trace()

    if newval == params.get(cmd.split()[1]):
        print "Already set: %s => %s" % (cmd.split()[1], newval)
        update.message.reply_text("Already set: %s => %s" %
                                  (cmd.split()[1], newval))
        return

    if cmd.split()[1] != "" and newval != "":
        print "Result:  %s" % mavros.param.param_set(cmd.split()[1], newval)
        update.message.reply_text(
            "Set %s => new value %s, old value %s " %
            (cmd.split()[1], newval, params.get(cmd.split()[1])))
Ejemplo n.º 20
0
def param_get_all(force_pull=False):
    try:
        pull = rospy.ServiceProxy(mavros.get_topic('param', 'pull'), ParamPull)
        ret = pull(force_pull=force_pull)
    except rospy.ServiceException as ex:
        raise IOError(str(ex))

    if not ret.success:
        raise IOError("Request failed.")

    params = rospy.get_param(mavros.get_topic('param'))

    return (ret.param_received,
            sorted((Parameter(k, v) for k, v in params.iteritems()),
                   cmp=lambda x, y: cmp(x.param_id, y.param_id))
            )
Ejemplo n.º 21
0
    def __init__(self):
        self.pos_x = 0.0
        self.pos_y = 0.0
        self.pos_z = 0.0
        self.setpoint_x = 0.0
        self.setpoint_y = 0.0
        self.setpoint_z = 0.0

        # publisher for mavros/setpoint_position/pose
        self.setpoint_position = setpoint.get_pub_position_local(queue_size=1)

        # subscriber for mavros/local_position/pose
        self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'),
                                    setpoint.PoseStamped,
                                    self._hasReached,
                                    queue_size=1)

        try:
            thread.start_new_thread(self.navigate, ())
        except:
            fault("Error: Unable to start thread")

        # TODO(simon): Clean this up.
        self.done = False
        self.done_evt = threading.Event()
Ejemplo n.º 22
0
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()
Ejemplo n.º 23
0
def get_pub_attitude_throttle(**kvargs):
    """
    Returns publisher for :setpoint_attitude: plugin, :cmd_vel: topic
    """
    return rospy.Publisher(
        mavros.get_topic('setpoint_attitude', 'att_throttle'), Float64,
        **kvargs)
    def __init__(self):
        self.handlers = []
        self.known_events = ['armed', 'disarmed']
        self.triggers = {}
        self.prev_armed = False

        self.state_sub = rospy.Subscriber(mavros.get_topic('state'), State,
                                          self.mavros_state_cb)

        try:
            params = rospy.get_param('~')
            if not isinstance(params, dict):
                raise KeyError("bad configuration")
        except KeyError as e:
            rospy.logerr('Config error: %s', e)
            return

        # load configuration
        for k, v in params.iteritems():
            # TODO: add checks for mutually exclusive options

            if 'service' in v:
                self._load_trigger(k, v)
            elif 'shell' in v:
                self._load_shell(k, v)
            else:
                rospy.logwarn("Skipping unknown section: %s", k)

        # check that events are known
        rospy.loginfo("Known events: %s", ', '.join(self.known_events))
        for h in self.handlers:
            for evt in h.events:
                if evt not in self.known_events:
                    rospy.logwarn("%s: unknown event: %s", h.name, evt)
Ejemplo n.º 25
0
def param_set_list(param_list):
    # 1. load parameters to parameter server
    for p in param_list:
        rospy.set_param(mavros.get_topic('param', p.param_id), p.param_value)

    # 2. request push all
    try:
        push = rospy.ServiceProxy(mavros.get_topic('param', 'push'), ParamPush)
        ret = push()
    except rospy.ServiceException as ex:
        raise IOError(str(ex))

    if not ret.success:
        raise IOError("Request failed.")

    return ret.param_transfered
Ejemplo n.º 26
0
def main():
    rospy.init_node('offboardX')
    rate = rospy.Rate(20)
    mavros.set_namespace('/mavros')
    state_sub = rospy.Subscriber(mavros.get_topic('state'),
                                 mavros_msgs.msg.State, state_callback)
    arming = rospy.ServiceProxy('/mavros/cmd/arming',
                                mavros_msgs.srv.CommandBool)
    #set_mode=rospy.ServiceProxy('/mavros/set_mode',SetMode)
    set_mode = rospy.ServiceProxy('/mavros/set_mode', mavros_msgs.srv.SetMode)
    #local_pub =  rospy.Publisher(mavros.get_topic('PositionTarget'),mavros_msgs.msg.PositionTarget,queue_size=10)
    local_pub = rospy.Publisher(mavros.get_topic('PositionTarget'),
                                mavros_msgs.msg.PositionTarget,
                                queue_size=10)
    pose = PositionTarget()
    pose.header = Header()
    pose.header.frame_id = "att_pose"
    pose.header.stamp = rospy.Time.now()
    pose.position.x = 0
    pose.position.y = 0
    pose.position.z = 15
    while (not state.connected):
        rate.sleep()
    for i in range(0, 50):
        local_pub.publish(pose)
    #mavros.command.arming(True)
    #set_mode(0,'OFFBOARD')
    last_request = rospy.Time.now()
    while 1:
        if (state.mode != "OFFBOARD"
                and (rospy.Time.now() - last_request > rospy.Duration(5.0))):
            print("inside22")
            if (set_mode(0, 'OFFBOARD').success):
                print("Offboard enabled")
                last_request = rospy.Time.now()
        else:
            if (not 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("entered")
        local_pub.publish(pose)
        rospy.spin()
        rate.sleep()
    return 0
Ejemplo n.º 27
0
    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
Ejemplo n.º 28
0
    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)
        }
Ejemplo n.º 29
0
    def write(self, file_, parameters):
        def to_type(x):
            if isinstance(x, float):
                return 9 # REAL32
            elif isinstance(x, int):
                return 6 # INT32
            else:
                raise ValueError("unknown type: " + repr(type(x)))

        sysid = rospy.get_param(mavros.get_topic('target_system_id'), 1)
        compid = rospy.get_param(mavros.get_topic('target_component_id'), 1)

        writer = csv.writer(file_, self.CSVDialect)
        writer.writerow(("# NOTE: " + time.strftime("%d.%m.%Y %T"), ))
        writer.writerow(("# Onboard parameters saved by mavparam for ({}, {})".format(sysid, compid), ))
        writer.writerow(("# MAV ID" , "COMPONENT ID", "PARAM NAME", "VALUE", "(TYPE)"))
        for p in parameters:
            writer.writerow((sysid, compid, p.param_id, p.param_value, to_type(p.param_value), )) # XXX
Ejemplo n.º 30
0
	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
Ejemplo n.º 31
0
    def __init__(self):
        ''' Class that acts as a server for the goto_pid service '''

        # init node
        rospy.init_node('pid_navigation_service', anonymous=True)
        mavros.set_namespace('mavros')

        # Setup Subscribers
        ## mavros state
        state_sub = rospy.Subscriber(mavros.get_topic('state'),
                                     mavros_msgs.msg.State,
                                     self._state_callback)

        # Setup publishers
        # /mavros/setpoint_velocity/cmd_vel
        self.cmd_vel_pub = rospy.Publisher("mavros/setpoint_velocity/cmd_vel",
                                           TwistStamped,
                                           queue_size=10)

        # /mavros/local_position/pose
        local_position_sub = rospy.Subscriber(
            mavros.get_topic('local_position', 'pose'), SP.PoseStamped,
            self._local_position_callback)

        # setup 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)

        # Initialize the service servers
        goto_pid_serv = rospy.Service('goto_pid_service', goto_pid,
                                      self.GotoLocPid)

        # Initialize variables
        self.local_pos = [0.0] * 4
        self.UAV_state = mavros_msgs.msg.State()

        # Setup rate
        self.rate = rospy.Rate(20)
        rospy.sleep(1)
        rospy.spin()
Ejemplo n.º 32
0
def set_mode(custom_mode='GUIDED', base_mode=0):
	print 'Setting mode to (%d, %s)' % (base_mode, custom_mode)
	try:
		set_mode = rospy.ServiceProxy(mavros.get_topic('set_mode'), SetMode)
		ret = set_mode(base_mode=base_mode, custom_mode=custom_mode)
	except rospy.ServiceException as ex:
		fault(ex)

	if not ret.success:
		fault('Request failed. Check mavros logs')
Ejemplo n.º 33
0
 def __init__(self):
     rospy.init_node('pylon_node')
     rospy.Subscriber(requestSub, Bool, self.cb_onRequest)
     rospy.Subscriber(mavros.get_topic('global_position', 'global'),
                      NavSatFix, self.cb_onPosUpdate)
     self.pylonPub = rospy.Publisher(pylonDataPub, pylonList, queue_size=1)
     self.osmAPI = "https://api.openstreetmap.org/api/0.6/map"
     self.boundary = 350  #metres -- curpos +/- (boundary / 2)
     self.dronePos = NavSatFix()
     self.pylonList = []
     rospy.spin()
Ejemplo n.º 34
0
def param_get(param_id):
    try:
        get = rospy.ServiceProxy(mavros.get_topic('param', 'get'), ParamGet)
        ret = get(param_id=param_id)
    except rospy.ServiceException as ex:
        raise IOError(str(ex))

    if not ret.success:
        raise IOError("Request failed.")

    return param_ret_value(ret)
Ejemplo n.º 35
0
def param_get(param_id):
    try:
        get = rospy.ServiceProxy(mavros.get_topic('param', 'get'), ParamGet)
        ret = get(param_id=param_id)
    except rospy.ServiceException as ex:
        raise IOError(str(ex))

    if not ret.success:
        raise IOError("Request failed.")

    return param_ret_value(ret)
Ejemplo n.º 36
0
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
Ejemplo n.º 37
0
def wait_fcu_connection(timeout=None):
    """
    Wait until establishing FCU connection
    """
    try:
        msg = rospy.wait_for_message(mavros.get_topic('state'), mavros.msg.State, timeout)
        if msg.connected:
            return True
    except rospy.ROSException as e:
        return False

    connected = threading.Event()
    def handler(msg):
        if msg.connected:
            connected.set()

    sub = rospy.Subscriber(
        mavros.get_topic('state'),
        mavros.msg.State,
        handler
    )

    return connected.wait(timeout)
	def __init__(self):
		self.x = 0.0
		self.y = 0.0
		self.z = 0.0

		self.pub = SP.get_pub_position_local(queue_size=10)
		self.sub = rospy.Subscriber( mavros.get_topic('local_position', 'pose'), SP.PoseStamped, self.reached)

		try:
			thread.start_new_thread( self.navigate, () )
		except:
			fault("Error: Unable to start the thread")

		self.done = False
		self.done_evt = threading.Event()
Ejemplo n.º 39
0
def param_set(param_id, value):
    if isinstance(value, float):
        val = ParamValue(integer=0, real=value)
    else:
        val = ParamValue(integer=value, real=0.0)

    try:
        set = rospy.ServiceProxy(mavros.get_topic('param', 'set'), ParamSet)
        ret = set(param_id=param_id, value=val)
    except rospy.ServiceException as ex:
        raise IOError(str(ex))

    if not ret.success:
        raise IOError("Request failed.")

    return param_ret_value(ret)
Ejemplo n.º 40
0
    def __init__(self, setpoint_publish):
        self.setpoint_pub = setpoint_publish
        self.current_sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'),
            SP.PoseStamped, self._local_position_callback)
        self.current = vector3()
        self.msg = mavros.setpoint.PoseStamped(
            header=mavros.setpoint.Header(
                frame_id="att_pose",
                stamp=rospy.Time.now()),
            )
        

        self.x = 0
        self.y = 0
        self.z = 0
        # default precision is 0.5
        self.precision = 0.5    
Ejemplo n.º 41
0
    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 __init__(self):
        self.x = 0.0
        self.y = 0.0
        self.z = 0.0

        # 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)

        try:
            thread.start_new_thread(self.navigate, ())
        except:
            fault("Error: Unable to start thread")

        # TODO(simon): Clean this up.
        self.done = False
        self.done_evt = threading.Event()
    def __init__(self, sonar_obs_present=[], jMAVSim=False ):
        self.x = 0.0
        self.y = 0.0
        self.z = 0.0

        self.yaw = 0.0
        self.jMAVSim = jMAVSim

        # 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)

        try:
            #thread.start_new_thread( self.navigate, () )
	    navigation_thread = threading.Thread(target=self.navigate)
            navigation_thread.start()
        except:
            print "Error: Unable to start thread"
                                    
                                    
        self.sonar_obs_present = sonar_obs_present
        self.obs_detected = False

        #current coordinates
        self.curr_x = 0.0
        self.curr_y = 0.0
        self.curr_z = 0.0

        self.x_home = 0
        self.y_home = 0
        self.z_home = 0

        
        #coordinates for original setpoint, stored to continue to desired setpoint if interrupted
        self.x_final_dest = 0.0
        self.y_final_dest = 0.0
        self.z_final_dest = 0.0
        self.yaw_final_dest = 0.0

        self.done = False
        self.done_evt = threading.Event()
    def __init__(self,actionServer):
        self.actionServer = actionServer
        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
        # 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('MappingAction', MappingAction)
        client = self.client
        #client = self.actionServer.client

        print "Waiting for mapping server"
        client.wait_for_server()
        goal = TrackingGoal()
        goal.uav_id = 3
        client.send_goal(goal)
        print "Waiting for result"
        client.wait_for_result()
        print "Result:"
        self.objects_map = client.get_result().objects_map
        print self.objects_map
        
        self.sub = rospy.Subscriber("MappingAction/feedback",ObjectsMap, self.callback)

        try:
            thread.start_new_thread(self.navigate, ())
        except:
            fault("Error: Unable to start thread")
    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 __init__(self):
        self.x = 0.0
        self.y = 0.0
        self.z = 0.0
        self.currentPoseX = 0 
        self.currentPoseY = 0
        self.currentPoseZ = 0
        # 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.waypointsub = rospy.Subscriber("/uav_1/waypoint", SP.PoseStamped, self.updateposition, queue_size=1)

        try:
            thread.start_new_thread(self.navigate, ())
        except:
            fault("Error: Unable to start thread")

        self.done = False
        self.done_evt = threading.Event()
Ejemplo n.º 47
0
    def velocity_init(self):
        self.vx = 0.0
        self.vy = 0.0
        self.vz = 0.0
        self.yaw = 0.0
        self.velocity_publish = False
        self.use_pid = True

        self.pid_alt = pid_controller.PID()
        self.pid_alt.setPoint(1.0)
        # publisher for mavros/setpoint_position/local
        self.pub_vel = SP.get_pub_velocity_cmd_vel(queue_size=10)
        # subscriber for mavros/local_position/local
        self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'local'), SP.PoseStamped, self.temp)
 
        try:
            thread.start_new_thread(self.navigate, ())
        except:
            fault("Error: Unable to start thread")
 
        # TODO(simon): Clean this up.
        self.done = False
        self.done_evt = threading.Event()
Ejemplo n.º 48
0
    def __init__(self):
        self.handlers = []
        self.known_events = ['armed', 'disarmed']
        self.triggers = {}
        self.prev_armed = False

        try:
            params = rospy.get_param('~')
            if not isinstance(params, dict):
                raise KeyError("bad configuration")
        except KeyError as e:
            rospy.logerr('Config error: %s', e)
            return

        # load configuration
        for k, v in params.iteritems():
            # TODO: add checks for mutually exclusive options

            if 'service' in v:
                self._load_trigger(k, v)
            elif 'shell' in v:
                self._load_shell(k, v)
            else:
                rospy.logwarn("Skipping unknown section: %s", k)

        # check that events are known
        rospy.loginfo("Known events: %s", ', '.join(self.known_events))
        for h in self.handlers:
            for evt in h.events:
                if evt not in self.known_events:
                    rospy.logwarn("%s: unknown event: %s", h.name, evt)

        # config loaded, we may subscribe
        self.state_sub = rospy.Subscriber(
            mavros.get_topic('state'),
            State,
            self.mavros_state_cb)
Ejemplo n.º 49
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 start_subs(self):
     # publisher for mavros/setpoint_position/local
     self.pub_vel = SP.get_pub_velocity_cmd_vel(queue_size=10)
     # subscriber for mavros/local_position/local
     self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'local'), SP.PoseStamped, self.temp)
Ejemplo n.º 51
0
def get_pub_attitude_posecov(**kvargs):
    """
    Returns publisher for :setpoint_attitude: plugin, :attituse: topic (with covariance)
    """
    return rospy.Publisher(mavros.get_topic('setpoint_attitude', 'attitude'), PoseWithCovarianceStamped, **kvargs)
Ejemplo n.º 52
0
def get_pub_position_local(**kvargs):
    """
    Returns publisher for :setpoint_position: plugin, :local: topic
    """
    return rospy.Publisher(mavros.get_topic('setpoint_position', 'local'), PoseStamped, **kvargs)
Ejemplo n.º 53
0
def get_pub_velocity_cmd_vel(**kvargs):
    """
    Returns publisher for :setpoint_velocity: plugin, :cmd_vel: topic
    """
    return rospy.Publisher(mavros.get_topic('setpoint_velocity', 'cmd_vel'), TwistStamped, **kvargs)
Ejemplo n.º 54
0
def _get_proxy(service, type):
    return rospy.ServiceProxy(mavros.get_topic('cmd', service), type)
 def halt(self, delay=0):
     self.sub_localpos = rospy.Subscriber(mavros.get_topic('local_position', 'pose'),
                                 SP.PoseStamped, self._local_position_callback)
     time.sleep(delay)
     print self.curr_x, self.curr_y, self.curr_z
     setpoint.set(self.curr_x, self.curr_y, self.curr_z, self.yaw)
Ejemplo n.º 56
0
def subscribe_waypoints(cb, **kvargs):
    return rospy.Subscriber(
        mavros.get_topic('mission', 'waypoints'), WaypointList, cb, **kvargs)
Ejemplo n.º 57
0
 def _get_proxy(name, type):
     return rospy.ServiceProxy(mavros.get_topic('mission', name), type)