C++ utils::enum_value方法代码示例

	/**
	 * 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(bool broadcast,
			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)
	{
		using mavlink::common::MAV_RESULT;

		unique_lock lock(mutex);

		L_CommandTransaction::iterator ack_it;

		/* check transactions */
		for (const auto &tr : ack_waiting_list) {
			if (tr.expected_command == command) {
				ROS_WARN_THROTTLE_NAMED(10, "cmd", "CMD: Command %u already in progress", command);
				return false;
			}
		}

		/**
		 * @note APM & PX4 master always send COMMAND_ACK. Old PX4 never.
		 * Don't expect any ACK in broadcast mode.
		 */
		bool is_ack_required = (confirmation != 0 || m_uas->is_ardupilotmega() || m_uas->is_px4()) && !broadcast;
		if (is_ack_required)
			ack_it = ack_waiting_list.emplace(ack_waiting_list.end(), command);

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

		if (is_ack_required) {
			lock.unlock();
			bool is_not_timeout = wait_ack_for(*ack_it);
			lock.lock();

			success = is_not_timeout && ack_it->result == enum_value(MAV_RESULT::ACCEPTED);
			result = ack_it->result;

			ack_waiting_list.erase(ack_it);
		}
		else {
			success = true;
			result = enum_value(MAV_RESULT::ACCEPTED);
		}

		return true;
	}

	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)
	{
		using mavlink::common::MAV_COMPONENT;

		const uint8_t tgt_sys_id = (broadcast) ? 0 : m_uas->get_tgt_system();
		const uint8_t tgt_comp_id = (broadcast) ? 0 :
			(use_comp_id_system_control) ?
				enum_value(MAV_COMPONENT::COMP_ID_SYSTEM_CONTROL) : m_uas->get_tgt_component();

		mavlink::common::msg::COMMAND_INT cmd;
		cmd.target_system = tgt_sys_id;
		cmd.target_component = tgt_comp_id;
		cmd.frame = frame;
		cmd.command = command;
		cmd.current = current;
		cmd.autocontinue = autocontinue;
		cmd.param1 = param1;
		cmd.param2 = param2;
		cmd.param3 = param3;
		cmd.param4 = param4;
		cmd.x = x;
		cmd.y = y;
		cmd.z = z;

		UAS_FCU(m_uas)->send_message_ignore_drop(cmd);
	}

	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)
	{
		using mavlink::common::MAV_COMPONENT;

		const uint8_t tgt_sys_id = (broadcast) ? 0 : m_uas->get_tgt_system();
		const uint8_t tgt_comp_id = (broadcast) ? 0 :
			(use_comp_id_system_control) ?
				enum_value(MAV_COMPONENT::COMP_ID_SYSTEM_CONTROL) : m_uas->get_tgt_component();
		const uint8_t confirmation_fixed = (broadcast) ? 0 : confirmation;

		mavlink::common::msg::COMMAND_LONG cmd;
		cmd.target_system = tgt_sys_id;
		cmd.target_component = tgt_comp_id;
		cmd.command = command;
		cmd.confirmation = confirmation_fixed;
		cmd.param1 = param1;
		cmd.param2 = param2;
		cmd.param3 = param3;
		cmd.param4 = param4;
		cmd.param5 = param5;
		cmd.param6 = param6;
		cmd.param7 = param7;

		UAS_FCU(m_uas)->send_message_ignore_drop(cmd);
	}

	bool set_home_cb(mavros_msgs::CommandHome::Request &req,
			mavros_msgs::CommandHome::Response &res)
	{
		using mavlink::common::MAV_CMD;
		return send_command_long_and_wait(false,
				enum_value(MAV_CMD::DO_SET_HOME), 1,
				(req.current_gps) ? 1.0 : 0.0,
				0, 0, 0, req.latitude, req.longitude, req.altitude,
				res.success, res.result);
	}

	bool arming_cb(mavros_msgs::CommandBool::Request &req,
			mavros_msgs::CommandBool::Response &res)
	{
		using mavlink::common::MAV_CMD;
		return send_command_long_and_wait(false,
				enum_value(MAV_CMD::COMPONENT_ARM_DISARM), 1,
				(req.value) ? 1.0 : 0.0,
				0, 0, 0, 0, 0, 0,
				res.success, res.result);
	}

	bool land_cb(mavros_msgs::CommandTOL::Request &req,
			mavros_msgs::CommandTOL::Response &res)
	{
		using mavlink::common::MAV_CMD;
		return send_command_long_and_wait(false,
				enum_value(MAV_CMD::NAV_LAND), 1,
				0, 0, 0,
				req.yaw,
				req.latitude, req.longitude, req.altitude,
				res.success, res.result);
	}

        bool trigger_control_cb(mavros_msgs::CommandTriggerControl::Request &req,
			mavros_msgs::CommandTriggerControl::Response &res)
	{
		using mavlink::common::MAV_CMD;
		return send_command_long_and_wait(false,
				enum_value(MAV_CMD::DO_TRIGGER_CONTROL), 1,
				(req.trigger_enable)? 1.0 : 0.0,
				req.integration_time,
				0, 0, 0, 0, 0,
				res.success, res.result);
	}

void MavRos::mavlink_pub_cb(const mavlink_message_t *mmsg, Framing framing)
{
	auto rmsg = boost::make_shared<mavros_msgs::Mavlink>();

	if  (mavlink_pub.getNumSubscribers() == 0)
		return;

	rmsg->header.stamp = ros::Time::now();
	mavros_msgs::mavlink::convert(*mmsg, *rmsg, enum_value(framing));
	mavlink_pub.publish(rmsg);
}

	inline void set_target(MsgT &cmd, bool broadcast)
	{
		using mavlink::common::MAV_COMPONENT;

		const uint8_t tgt_sys_id = (broadcast) ? 0 : m_uas->get_tgt_system();
		const uint8_t tgt_comp_id = (broadcast) ? 0 :
			(use_comp_id_system_control) ?
				enum_value(MAV_COMPONENT::COMP_ID_SYSTEM_CONTROL) : m_uas->get_tgt_component();

		cmd.target_system = tgt_sys_id;
		cmd.target_component = tgt_comp_id;
	}

	bool trigger_interval_cb(mavros_msgs::CommandTriggerInterval::Request &req,
			mavros_msgs::CommandTriggerInterval::Response &res)
	{
		using mavlink::common::MAV_CMD;

		// trigger interval can only be set when triggering is disabled
		return send_command_long_and_wait(false,
				enum_value(MAV_CMD::DO_SET_CAM_TRIGG_INTERVAL), 1,
				req.cycle_time,
				req.integration_time,
				0, 0, 0, 0, 0,
				res.success, res.result);
	}

	explicit CommandTransaction(uint16_t command) :
		ack(),
		expected_command(command),
		// Default result if wait ack timeout
		result(enum_value(mavlink::common::MAV_RESULT::FAILED))
	{ }

	/**
	 * 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(bool broadcast,
			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)
	{
		using mavlink::common::MAV_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", "CMD: Command %u alredy in progress", command);
				return false;
			}

		/**
		 * @note APM & PX4 master always send COMMAND_ACK. Old PX4 never.
		 * Don't expect any ACK in broadcast mode.
		 */
		bool is_ack_required = confirmation != 0 || m_uas->is_ardupilotmega() || m_uas->is_px4();
		if (is_ack_required && !broadcast)
			ack_waiting_list.push_back(new CommandTransaction(command));

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

		if (is_ack_required && !broadcast) {
			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", "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 == enum_value(MAV_RESULT::ACCEPTED);
			result = (*it)->result;

			delete *it;
			ack_waiting_list.erase(it);
		}
		else {
			success = true;
			result = enum_value(MAV_RESULT::ACCEPTED);
		}

		return true;
	}