C++ uavcan::ServiceCallResult类代码示例

void UavcanNode::cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result)
{
	uavcan::protocol::RestartNode::Response resp;
	_callback_success = result.isSuccessful();
	resp = result.getResponse();
	_callback_success &= resp.ok;
}

void UavcanNode::cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result)
{
	uavcan::protocol::param::ExecuteOpcode::Response resp;
	_callback_success = result.isSuccessful();
	resp = result.getResponse();
	_callback_success &= resp.ok;
}

 void handleResponse(const uavcan::ServiceCallResult<DataType>& result)
 {
     std::cout << result << std::endl;
     last_status = result.getStatus();
     last_response = result.getResponse();
     last_server_node_id = result.getCallID().server_node_id;
     responses.push(result.getResponse());
 }

    void handleReadResponse(const uavcan::ServiceCallResult<uavcan::protocol::file::Read>& result)
    {
        if (result.isSuccessful() &&
            result.getResponse().error.value == 0)
        {
            auto& data = result.getResponse().data;

            image_.insert(image_.end(), data.begin(), data.end());

            if (data.size() < data.capacity())  // Termination condition
            {
                status_ = Status::Success;
                uavcan::TimerBase::stop();
            }
        }
        else
        {
            status_ = Status::Failure;
            uavcan::TimerBase::stop();
        }
    }

void UavcanServers::cb_enumeration_begin(const uavcan::ServiceCallResult<uavcan::protocol::enumeration::Begin> &result)
{
	uint8_t next_id = get_next_active_node_id(result.getCallID().server_node_id.get());

	if (!result.isSuccessful()) {
		warnx("UAVCAN ESC enumeration: begin request for node %hhu timed out.", result.getCallID().server_node_id.get());
	} else if (result.getResponse().error) {
		warnx("UAVCAN ESC enumeration: begin request for node %hhu rejected: %hhu", result.getCallID().server_node_id.get(), result.getResponse().error);
	} else {
		_esc_count++;
		warnx("UAVCAN ESC enumeration: begin request for node %hhu completed OK.", result.getCallID().server_node_id.get());
	}

	if (next_id < 128) {
		// Still other active nodes to send the request to
		uavcan::protocol::enumeration::Begin::Request req;
		// TODO: Incorrect implementation; the parameter name field should be left empty.
		//       Leaving it as-is to avoid breaking compatibility with non-compliant nodes.
		req.parameter_name = "esc_index";
		req.timeout_sec = _esc_enumeration_active ? 65535 : 0;

		int call_res = _enumeration_client.call(next_id, req);
		if (call_res < 0) {
			warnx("UAVCAN ESC enumeration: couldn't send Begin request: %d", call_res);
		} else {
			warnx("UAVCAN ESC enumeration: sent Begin request");
		}
	} else {
		warnx("UAVCAN ESC enumeration: begun enumeration on all nodes.");
	}
}

void UavcanServers::cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result)
{
	bool success = result.isSuccessful();
	uint8_t node_id = result.getCallID().server_node_id.get();
	uavcan::protocol::RestartNode::Response resp = result.getResponse();
	success &= resp.ok;
	_cmd_in_progress = false;

	if (success) {
		warnx("UAVCAN command bridge: restart request for node %hhu completed OK.", node_id);
		// Clear the dirty flag
		clear_node_params_dirty(node_id);
	} else {
		warnx("UAVCAN command bridge: restart request for node %hhu failed.", node_id);
	}

	// Get the next dirty node ID and send the same command to it
	node_id = get_next_dirty_node_id(node_id);
	if (node_id < 128) {
		param_opcode(node_id);
	}
}

void UavcanServers::cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result)
{
	bool success = result.isSuccessful();
	uint8_t node_id = result.getCallID().server_node_id.get();
	uavcan::protocol::param::ExecuteOpcode::Response resp = result.getResponse();
	success &= resp.ok;
	_cmd_in_progress = false;

	if (!result.isSuccessful()) {
		warnx("UAVCAN command bridge: save request for node %hhu timed out.", node_id);
	} else if (!result.getResponse().ok) {
		warnx("UAVCAN command bridge: save request for node %hhu rejected.", node_id);
	} else {
		warnx("UAVCAN command bridge: save request for node %hhu completed OK, restarting.", node_id);

		uavcan::protocol::RestartNode::Request restart_req;
		restart_req.magic_number = restart_req.MAGIC_NUMBER;
		int call_res = _param_restartnode_client.call(node_id, restart_req);
		if (call_res < 0) {
			warnx("UAVCAN command bridge: couldn't send RestartNode: %d", call_res);
		} else {
			warnx("UAVCAN command bridge: sent RestartNode");
			_cmd_in_progress = true;
		}
	}

	if (!_cmd_in_progress) {
		/*
		 * Something went wrong, so cb_restart is never going to be called as a result of this request.
		 * To ensure we try to execute the opcode on all nodes that permit it, get the next dirty node
		 * ID and keep processing here. The dirty bit on the current node is still set, so the
		 * save/erase attempt will occur when the next save/erase command is received over MAVLink.
		 */
		node_id = get_next_dirty_node_id(node_id);
		if (node_id < 128) {
			param_opcode(node_id);
		}
	}
}

void UavcanServers::cb_enumeration_save(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result)
{
	uavcan::equipment::indication::BeepCommand beep;

	if (!result.isSuccessful()) {
		warnx("UAVCAN ESC enumeration: save request for node %hhu timed out.", result.getCallID().server_node_id.get());
		beep.frequency = 880.0f;
		beep.duration = 1.0f;
	} else if (!result.getResponse().ok) {
		warnx("UAVCAN ESC enumeration: save request for node %hhu rejected", result.getCallID().server_node_id.get());
		beep.frequency = 880.0f;
		beep.duration = 1.0f;
	} else {
		warnx("UAVCAN ESC enumeration: save request for node %hhu completed OK.", result.getCallID().server_node_id.get());
		beep.frequency = 440.0f;
		beep.duration = 0.25f;
	}

	(void)_beep_pub.broadcast(beep);

	warnx("UAVCAN ESC enumeration: completed %hhu of %hhu", _esc_enumeration_index, _esc_count);

	if (_esc_enumeration_index == uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize - 1 ||
			_esc_enumeration_index == _esc_count) {
		_esc_enumeration_active = false;

		// Tell all ESCs to stop enumerating
		uavcan::protocol::enumeration::Begin::Request req;
		req.parameter_name = "esc_index";
		req.timeout_sec = 0;
		int call_res = _enumeration_client.call(get_next_active_node_id(1), req);
		if (call_res < 0) {
			warnx("UAVCAN ESC enumeration: couldn't send Begin request to stop enumeration: %d", call_res);
		} else {
			warnx("UAVCAN ESC enumeration: sent Begin request to stop enumeration");
		}
	}
}

void UavcanServers::cb_enumeration_save(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result)
{
	const bool this_is_the_last_one =
		_esc_enumeration_index >= uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize - 1 ||
		_esc_enumeration_index >= _esc_count;

	if (!result.isSuccessful()) {
		warnx("UAVCAN ESC enumeration: save request for node %hhu timed out.", result.getCallID().server_node_id.get());
		beep(BeepFrequencyError);
	} else if (!result.getResponse().ok) {
		warnx("UAVCAN ESC enumeration: save request for node %hhu rejected", result.getCallID().server_node_id.get());
		beep(BeepFrequencyError);
	} else {
		warnx("UAVCAN ESC enumeration: save request for node %hhu completed OK.", result.getCallID().server_node_id.get());
		beep(this_is_the_last_one ? BeepFrequencySuccess : BeepFrequencyGenericIndication);
	}

	warnx("UAVCAN ESC enumeration: completed %hhu of %hhu", _esc_enumeration_index, _esc_count);

	if (this_is_the_last_one) {
		_esc_enumeration_active = false;

		// Tell all ESCs to stop enumerating
		uavcan::protocol::enumeration::Begin::Request req;
		// TODO: Incorrect implementation; the parameter name field should be left empty.
		//       Leaving it as-is to avoid breaking compatibility with non-compliant nodes.
		req.parameter_name = "esc_index";
		req.timeout_sec = 0;
		int call_res = _enumeration_client.call(get_next_active_node_id(1), req);
		if (call_res < 0) {
			warnx("UAVCAN ESC enumeration: couldn't send Begin request to stop enumeration: %d", call_res);
		} else {
			warnx("UAVCAN ESC enumeration: sent Begin request to stop enumeration");
		}
	}
}

void UavcanServers::cb_enumeration_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result)
{
	if (!result.isSuccessful()) {
		warnx("UAVCAN ESC enumeration: save request for node %hhu timed out.", result.getCallID().server_node_id.get());
	} else {
		warnx("UAVCAN ESC enumeration: save request for node %hhu completed OK.", result.getCallID().server_node_id.get());

		uavcan::protocol::param::GetSet::Response resp = result.getResponse();
		uint8_t esc_index = (uint8_t)resp.value.to<uavcan::protocol::param::Value::Tag::integer_value>();
		esc_index = std::min((uint8_t)(uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize - 1), esc_index);
		_esc_enumeration_index = std::max(_esc_enumeration_index, (uint8_t)(esc_index + 1));

		_esc_enumeration_ids[esc_index] = result.getCallID().server_node_id.get();

		uavcan::protocol::param::ExecuteOpcode::Request opcode_req;
		opcode_req.opcode = opcode_req.OPCODE_SAVE;
		int call_res = _enumeration_save_client.call(result.getCallID().server_node_id, opcode_req);
		if (call_res < 0) {
			warnx("UAVCAN ESC enumeration: couldn't send ExecuteOpcode: %d", call_res);
		} else {
			warnx("UAVCAN ESC enumeration: sent ExecuteOpcode to node %hhu (index %hhu)", _esc_enumeration_ids[esc_index], esc_index);
		}
	}
}

 void handler(const uavcan::ServiceCallResult<DataType>& tmp_result)
 {
     std::cout << tmp_result << std::endl;
     result.reset(new Result(tmp_result.getStatus(), tmp_result.getCallID(), tmp_result.getResponse()));
 }

void config_client_cb(const uavcan::ServiceCallResult<cvra::motor::config::LoadConfiguration>& call_result)
{
    if (!call_result.isSuccessful()) {
        chSysHalt("uavcan service call timeout");
    }
}

void UavcanServers::cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result)
{
	if (_count_in_progress) {
		/*
		 * Currently in parameter count mode:
		 * Iterate over all parameters for the node to which the request was
		 * originally sent, in order to find the maximum parameter ID. If a
		 * request fails, set the node's parameter count to zero.
		 */
		uint8_t node_id = result.getCallID().server_node_id.get();

		if (result.isSuccessful()) {
			uavcan::protocol::param::GetSet::Response resp = result.getResponse();
			if (resp.name.size()) {
				_count_index++;
				_param_counts[node_id] = _count_index;

				uavcan::protocol::param::GetSet::Request req;
				req.index = _count_index;

				int call_res = _param_getset_client.call(result.getCallID().server_node_id, req);
				if (call_res < 0) {
					_count_in_progress = false;
					_count_index = 0;
					warnx("UAVCAN command bridge: couldn't send GetSet during param count: %d", call_res);
					beep(BeepFrequencyError);
				}
			} else {
				_count_in_progress = false;
				_count_index = 0;
				warnx("UAVCAN command bridge: completed param count for node %hhu: %hhu", node_id, _param_counts[node_id]);
				beep(BeepFrequencyGenericIndication);
			}
		} else {
			_param_counts[node_id] = 0;
			_count_in_progress = false;
			_count_index = 0;
			warnx("UAVCAN command bridge: GetSet error during param count");
		}
	} else {
		/*
		 * Currently in parameter get/set mode:
		 * Publish a uORB uavcan_parameter_value message containing the current value
		 * of the parameter.
		 */
		if (result.isSuccessful()) {
			uavcan::protocol::param::GetSet::Response param = result.getResponse();

			struct uavcan_parameter_value_s response;
			response.node_id = result.getCallID().server_node_id.get();
			strncpy(response.param_id, param.name.c_str(), sizeof(response.param_id) - 1);
			response.param_id[16] = '\0';
			response.param_index = _param_index;
			response.param_count = _param_counts[response.node_id];

			if (param.value.is(uavcan::protocol::param::Value::Tag::integer_value)) {
				response.param_type = MAV_PARAM_TYPE_INT64;
				response.int_value = param.value.to<uavcan::protocol::param::Value::Tag::integer_value>();
			} else if (param.value.is(uavcan::protocol::param::Value::Tag::real_value)) {
				response.param_type = MAV_PARAM_TYPE_REAL32;
				response.real_value = param.value.to<uavcan::protocol::param::Value::Tag::real_value>();
			} else if (param.value.is(uavcan::protocol::param::Value::Tag::boolean_value)) {
				response.param_type = MAV_PARAM_TYPE_UINT8;
				response.int_value = param.value.to<uavcan::protocol::param::Value::Tag::boolean_value>();
			}

			if (_param_response_pub == nullptr) {
				_param_response_pub = orb_advertise(ORB_ID(uavcan_parameter_value), &response);
			} else {
				orb_publish(ORB_ID(uavcan_parameter_value), _param_response_pub, &response);
			}
		} else {
			warnx("UAVCAN command bridge: GetSet error");
		}

		_param_in_progress = false;
		_param_index++;
	}
}

void feedback_stream_pub_cb(const uavcan::ServiceCallResult<cvra::motor::config::FeedbackStream>& call_result)
{
    if (!call_result.isSuccessful()) {
        chSysHalt("uavcan service call timeout");
    }
}

void enable_client_cb(const uavcan::ServiceCallResult<cvra::motor::config::EnableMotor>& call_result)
{
    if (!call_result.isSuccessful()) {
        chSysHalt("uavcan service call timeout");
    }
}