C++ UAS类代码示例

	void handle_local_position_ned(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
		mavlink_local_position_ned_t pos_ned;
		mavlink_msg_local_position_ned_decode(msg, &pos_ned);

		auto position = UAS::transform_frame_ned_enu(Eigen::Vector3d(pos_ned.x, pos_ned.y, pos_ned.z));
		auto orientation = uas->get_attitude_orientation();

		auto pose = boost::make_shared<geometry_msgs::PoseStamped>();

		pose->header = uas->synchronized_header(frame_id, pos_ned.time_boot_ms);

		tf::pointEigenToMsg(position, pose->pose.position);

		// XXX FIX ME #319
		tf::quaternionTFToMsg(orientation, pose->pose.orientation);

		local_position.publish(pose);

		if (tf_send) {
			geometry_msgs::TransformStamped transform;

			transform.header.stamp = pose->header.stamp;
			transform.header.frame_id = tf_frame_id;
			transform.child_frame_id = tf_child_frame_id;

			transform.transform.rotation = pose->pose.orientation;
			tf::vectorEigenToMsg(position, transform.transform.translation);

			tf2_broadcaster.sendTransform(transform);
		}
	}

	void command_int(bool broadcast,
			uint8_t frame, uint16_t command,
			uint8_t current, uint8_t autocontinue,
			float param1, float param2,
			float param3, float param4,
			int32_t x, int32_t y,
			float z)
	{
		mavlink_message_t msg;
		const uint8_t tgt_sys_id = (broadcast) ? 0 : uas->get_tgt_system();
		const uint8_t tgt_comp_id = (broadcast) ? 0 :
			(use_comp_id_system_control) ?
				MAV_COMP_ID_SYSTEM_CONTROL : uas->get_tgt_component();

		mavlink_msg_command_int_pack_chan(UAS_PACK_CHAN(uas), &msg,
				tgt_sys_id,
				tgt_comp_id,
				frame,
				command,
				current,
				autocontinue,
				param1, param2,
				param3, param4,
				x, y, z);
		UAS_FCU(uas)->send_message(&msg);
	}

	void command_long(bool broadcast,
			uint16_t command, uint8_t confirmation,
			float param1, float param2,
			float param3, float param4,
			float param5, float param6,
			float param7)
	{
		mavlink_message_t msg;
		const uint8_t tgt_sys_id = (broadcast) ? 0 : uas->get_tgt_system();
		const uint8_t tgt_comp_id = (broadcast) ? 0 :
			(use_comp_id_system_control) ?
				MAV_COMP_ID_SYSTEM_CONTROL : uas->get_tgt_component();
		const uint8_t confirmation_fixed = (broadcast) ? 0 : confirmation;

		mavlink_msg_command_long_pack_chan(UAS_PACK_CHAN(uas), &msg,
				tgt_sys_id,
				tgt_comp_id,
				command,
				confirmation_fixed,
				param1, param2,
				param3, param4,
				param5, param6,
				param7);
		UAS_FCU(uas)->send_message(&msg);
	}

    void connection_cb(bool connected) {
        // if connection changes, start delayed version request
        version_retries = RETRIES_COUNT;
        if (connected)
            autopilot_version_timer.start();
        else
            autopilot_version_timer.stop();

        // add/remove APM diag tasks
        if (connected && disable_diag && uas->is_ardupilotmega()) {
#ifdef MAVLINK_MSG_ID_MEMINFO
            UAS_DIAG(uas).add(mem_diag);
#endif
#ifdef MAVLINK_MSG_ID_HWSTATUS
            UAS_DIAG(uas).add(hwst_diag);
#endif
#if !defined(MAVLINK_MSG_ID_MEMINFO) || !defined(MAVLINK_MSG_ID_HWSTATUS)
            ROS_INFO_NAMED("sys", "SYS: APM detected, but mavros uses different dialect. "
                           "Extra diagnostic disabled.");
#else
            ROS_DEBUG_NAMED("sys", "SYS: APM extra diagnostics enabled.");
#endif
        }
        else {
            UAS_DIAG(uas).removeByName(mem_diag.getName());
            UAS_DIAG(uas).removeByName(hwst_diag.getName());
            ROS_DEBUG_NAMED("sys", "SYS: APM extra diagnostics disabled.");
        }
    }

	/*
	 * Arming/disarming the UAV
	 */
	void arming(const mms::Arm::ConstPtr msg){
		mavlink_message_t msg_mav;
		if (msg->arm_disarm){
			enum MAV_CMD command = MAV_CMD_COMPONENT_ARM_DISARM;
			float param1 = 1;      //1-->arm 0-->disarm
			float param2 = 0;      //not used
			float param3 = 0;      //not used
			float param4 = 0;      //not used
			float param5 = 0;      //not used
			float param6 = 0;      //not used
			float param7 = 0;      //not used
			uint8_t confirmation = 1;
			mavlink_msg_command_long_pack_chan(UAS_PACK_CHAN(uas), &msg_mav,
								uas->get_tgt_system(),
								uas->get_tgt_component(),
								command,
								confirmation,
								param1, param2,
								param3, param4,
								param5, param6,
								param7);
			UAS_FCU(uas)->send_message(&msg_mav);
			ROS_INFO("Arming UAV");
		} else {
			enum MAV_CMD command = MAV_CMD_COMPONENT_ARM_DISARM;
			float param1 = 0;      //1-->arm 0-->disarm
			float param2 = 0;      //not used
			float param3 = 0;      //not used
			float param4 = 0;      //not used
			float param5 = 0;      //not used
			float param6 = 0;      //not used
			float param7 = 0;      //not used
			uint8_t confirmation = 1;
			mavlink_msg_command_long_pack_chan(UAS_PACK_CHAN(uas), &msg_mav,
								uas->get_tgt_system(),
								uas->get_tgt_component(),
								command,
								confirmation,
								param1, param2,
								param3, param4,
								param5, param6,
								param7);
			UAS_FCU(uas)->send_message(&msg_mav);                        //TODO decide if send or not disarm by software
			ROS_INFO("Disarming UAV");
		}
	}

    bool set_mode_cb(mavros::SetMode::Request &req,
                     mavros::SetMode::Response &res) {
        mavlink_message_t msg;
        uint8_t base_mode = req.base_mode;
        uint32_t custom_mode = 0;

        if (req.custom_mode != "") {
            if (!uas->cmode_from_str(req.custom_mode, custom_mode)) {
                res.success = false;
                return true;
            }

            base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
        }

        mavlink_msg_set_mode_pack_chan(UAS_PACK_CHAN(uas), &msg,
                                       uas->get_tgt_system(),
                                       base_mode,
                                       custom_mode);
        UAS_FCU(uas)->send_message(&msg);
        res.success = true;
        return true;
    }

	void handle_timesync(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
		mavlink_timesync_t tsync;
		mavlink_msg_timesync_decode(msg, &tsync);

		uint64_t now_ns = ros::Time::now().toNSec();

		if (tsync.tc1 == 0) {
			send_timesync_msg(now_ns, tsync.ts1);
			return;
		}
		else if (tsync.tc1 > 0) {
			int64_t offset_ns = (tsync.ts1 + now_ns - tsync.tc1 * 2) / 2;
			int64_t dt = time_offset_ns - offset_ns;

			if (std::abs(dt) > 10000000) {		// 10 millisecond skew
				time_offset_ns = offset_ns;	// hard-set it.
				uas->set_time_offset(time_offset_ns);

				dt_diag.clear();
				dt_diag.set_timestamp(tsync.tc1);

				ROS_WARN_THROTTLE_NAMED(10, "time", "TM: Clock skew detected (%0.9f s). "
						"Hard syncing clocks.", dt / 1e9);
			}
			else {
				average_offset(offset_ns);
				dt_diag.tick(dt, tsync.tc1, time_offset_ns);

				uas->set_time_offset(time_offset_ns);
			}
		}
	}

    void handle_heartbeat(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
        if (!uas->is_my_target(sysid)) {
            ROS_DEBUG_NAMED("sys", "HEARTBEAT from [%d, %d] dropped.", sysid, compid);
            return;
        }

        mavlink_heartbeat_t hb;
        mavlink_msg_heartbeat_decode(msg, &hb);

        // update context && setup connection timeout
        uas->update_heartbeat(hb.type, hb.autopilot);
        uas->update_connection_status(true);
        timeout_timer.stop();
        timeout_timer.start();

        // build state message after updating uas
        auto state_msg = boost::make_shared<mavros::State>();
        state_msg->header.stamp = ros::Time::now();
        state_msg->armed = hb.base_mode & MAV_MODE_FLAG_SAFETY_ARMED;
        state_msg->guided = hb.base_mode & MAV_MODE_FLAG_GUIDED_ENABLED;
        state_msg->mode = uas->str_mode_v10(hb.base_mode, hb.custom_mode);

        state_pub.publish(state_msg);
        hb_diag.tick(hb.type, hb.autopilot, state_msg->mode, hb.system_status);
    }

	void handle_gps_raw_int(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
		mavlink_gps_raw_int_t raw_gps;
		mavlink_msg_gps_raw_int_decode(msg, &raw_gps);

		auto fix = boost::make_shared<sensor_msgs::NavSatFix>();

		fix->header = uas->synchronized_header(frame_id, raw_gps.time_usec);

		fix->status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;
		if (raw_gps.fix_type > 2)
			fix->status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
		else {
			ROS_WARN_THROTTLE_NAMED(30, "global_position", "GP: No GPS fix");
			fix->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;
		}

		fill_lla(raw_gps, fix);

		float eph = (raw_gps.eph != UINT16_MAX) ? raw_gps.eph / 1E2F : NAN;
		float epv = (raw_gps.epv != UINT16_MAX) ? raw_gps.epv / 1E2F : NAN;

		if (!isnan(eph)) {
			const double hdop = eph;

			// From nmea_navsat_driver
			fix->position_covariance[0 + 0] = \
				fix->position_covariance[3 + 1] = std::pow(hdop, 2);
			fix->position_covariance[6 + 2] = std::pow(2 * hdop, 2);
			fix->position_covariance_type =
					sensor_msgs::NavSatFix::COVARIANCE_TYPE_APPROXIMATED;
		}
		else {
			fill_unknown_cov(fix);
		}

		// store & publish
		uas->update_gps_fix_epts(fix, eph, epv, raw_gps.fix_type, raw_gps.satellites_visible);
		raw_fix_pub.publish(fix);

		if (raw_gps.vel != UINT16_MAX &&
				raw_gps.cog != UINT16_MAX) {
			double speed = raw_gps.vel / 1E2;				// m/s
			double course = angles::from_degrees(raw_gps.cog / 1E2);	// rad

			auto vel = boost::make_shared<geometry_msgs::TwistStamped>();

			vel->header.frame_id = frame_id;
			vel->header.stamp = fix->header.stamp;

			// From nmea_navsat_driver
			vel->twist.linear.x = speed * std::sin(course);
			vel->twist.linear.y = speed * std::cos(course);

			raw_vel_pub.publish(vel);
		}
	}

	/**
	 * Common function for command service callbacks.
	 *
	 * NOTE: success is bool in messages, but has unsigned char type in C++
	 */
	bool send_command_long_and_wait(uint16_t command, uint8_t confirmation,
			float param1, float param2,
			float param3, float param4,
			float param5, float param6,
			float param7,
			unsigned char &success, uint8_t &result) {
		unique_lock lock(mutex);

		/* check transactions */
		for (auto it = ack_waiting_list.cbegin();
				it != ack_waiting_list.cend(); it++)
			if ((*it)->expected_command == command) {
				ROS_WARN_THROTTLE_NAMED(10, "cmd", "Command %u alredy in progress", command);
				return false;
			}

		//! @note APM always send COMMAND_ACK, while PX4 never.
		bool is_ack_required = (confirmation != 0 || uas->is_ardupilotmega()) && !uas->is_px4();
		if (is_ack_required)
			ack_waiting_list.push_back(new CommandTransaction(command));

		command_long(command, confirmation,
				param1, param2,
				param3, param4,
				param5, param6,
				param7);

		if (is_ack_required) {
			auto it = ack_waiting_list.begin();
			for (; it != ack_waiting_list.end(); it++)
				if ((*it)->expected_command == command)
					break;

			if (it == ack_waiting_list.end()) {
				ROS_ERROR_NAMED("cmd", "CommandTransaction not found for %u", command);
				return false;
			}

			lock.unlock();
			bool is_not_timeout = wait_ack_for(*it);
			lock.lock();

			success = is_not_timeout && (*it)->result == MAV_RESULT_ACCEPTED;
			result = (*it)->result;

			delete *it;
			ack_waiting_list.erase(it);
		}
		else {
			success = true;
			result = MAV_RESULT_ACCEPTED;
		}

		return true;
	}

	void uas_store_attitude(tf::Quaternion &orientation,
			geometry_msgs::Vector3 &gyro_vec,
			geometry_msgs::Vector3 &acc_vec)
	{
		tf::Vector3 angular_velocity;
		tf::Vector3 linear_acceleration;
		tf::vector3MsgToTF(gyro_vec, angular_velocity);
		tf::vector3MsgToTF(acc_vec, linear_acceleration);

		uas->set_attitude_orientation(orientation);
		uas->set_attitude_angular_velocity(angular_velocity);
		uas->set_attitude_linear_acceleration(linear_acceleration);
	}

	/* -*- diagnostics -*- */
	void gps_diag_run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
		int fix_type, satellites_visible;
		float eph, epv;

		uas->get_gps_epts(eph, epv, fix_type, satellites_visible);

		if (satellites_visible <= 0)
			stat.summary(2, "No satellites");
		else if (fix_type < 2)
			stat.summary(1, "No fix");
		else if (fix_type == 2)
			stat.summary(0, "2D fix");
		else if (fix_type >= 3)
			stat.summary(0, "3D fix");

		stat.addf("Satellites visible", "%zd", satellites_visible);
		stat.addf("Fix type", "%d", fix_type);

		if (!isnan(eph))
			stat.addf("EPH (m)", "%.2f", eph);
		else
			stat.add("EPH (m)", "Unknown");

		if (!isnan(epv))
			stat.addf("EPV (m)", "%.2f", epv);
		else
			stat.add("EPV (m)", "Unknown");
	}

	/**
	 * Send transform to FCU position controller
	 *
	 * Note: send only XYZ, Yaw
	 */
	void send_setpoint_transform(const tf::Transform &transform, const ros::Time &stamp) {
		// ENU frame
		tf::Vector3 origin = transform.getOrigin();
		tf::Quaternion q = transform.getRotation();

		/* Documentation start from bit 1 instead 0,
		 * Ignore velocity and accel vectors, yaw rate
		 */
		uint16_t ignore_all_except_xyz_y = (1 << 11) | (7 << 6) | (7 << 3);

		if (uas->is_px4()) {
			/**
			 * Current PX4 has bug: it cuts throttle if there no velocity sp
			 * Issue #273.
			 *
			 * @todo Revesit this quirk later. Should be fixed in firmware.
			 */
			ignore_all_except_xyz_y = (1 << 11) | (7 << 6);
		}

		// ENU->NED. Issue #49.
		set_position_target_local_ned(stamp.toNSec() / 1000000,
				MAV_FRAME_LOCAL_NED,
				ignore_all_except_xyz_y,
				origin.y(), origin.x(), -origin.z(),
				0.0, 0.0, 0.0,
				0.0, 0.0, 0.0,
				tf::getYaw(q), 0.0);
	}

    void autopilot_version_cb(const ros::TimerEvent &event) {
        bool ret = false;

        try {
            auto client = nh.serviceClient<mavros::CommandLong>("cmd/command");

            mavros::CommandLong cmd{};
            cmd.request.command = MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES;
            cmd.request.confirmation = false;
            cmd.request.param1 = 1.0;

            ROS_DEBUG_NAMED("sys", "VER: Sending request.");
            ret = client.call(cmd);
        }
        catch (ros::InvalidNameException &ex) {
            ROS_ERROR_NAMED("sys", "VER: %s", ex.what());
        }

        ROS_ERROR_COND_NAMED(!ret, "sys", "VER: command plugin service call failed!");

        if (version_retries > 0) {
            version_retries--;
            ROS_WARN_COND_NAMED(version_retries != RETRIES_COUNT - 1, "sys",
                                "VER: request timeout, retries left %d", version_retries);
        }
        else {
            uas->update_capabilities(false);
            autopilot_version_timer.stop();
            ROS_WARN_NAMED("sys", "VER: your FCU don't support AUTOPILOT_VERSION, "
                           "switched to default capabilities");
        }
    }

    void handle_statustext(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
        mavlink_statustext_t textm;
        mavlink_msg_statustext_decode(msg, &textm);

        std::string text(textm.text,
                         strnlen(textm.text, sizeof(textm.text)));

        if (uas->is_ardupilotmega())
            process_statustext_apm_quirk(textm.severity, text);
        else
            process_statustext_normal(textm.severity, text);
    }