C++ utils类代码示例

MAVConnTCPClient::MAVConnTCPClient(uint8_t system_id, uint8_t component_id,
		std::string server_host, unsigned short server_port) :
	MAVConnInterface(system_id, component_id),
	tx_in_progress(false),
	tx_q {},
	rx_buf {},
	io_service(),
	io_work(new io_service::work(io_service)),
	socket(io_service)
{
	if (!resolve_address_tcp(io_service, conn_id, server_host, server_port, server_ep))
		throw DeviceError("tcp: resolve", "Bind address resolve failed");

	logInform(PFXd "Server address: %s", conn_id, to_string_ss(server_ep).c_str());

	try {
		socket.open(tcp::v4());
		socket.connect(server_ep);
	}
	catch (boost::system::system_error &err) {
		throw DeviceError("tcp", err);
	}

	// give some work to io_service before start
	io_service.post(std::bind(&MAVConnTCPClient::do_recv, this));

	// run io_service for async io
	io_thread = std::thread([this] () {
				utils::set_this_thread_name("mtcp%zu", conn_id);
				io_service.run();
			});
}

        void Build() {
            std::vector<utils::slice> key_slices;

            for (size_t i = 0; i < keys_.size(); i++) {
                key_slices.push_back(slice(keys_[i]));
            }

            filter_.clear();
            policy_->create_filter(&key_slices[0], key_slices.size(), &filter_);
            keys_.clear();

            if (kVerbose >= 2) DumpFilter();
        }

void
NdbapiDriver::initProperties() {
    CrundDriver::initProperties();

    cout << "setting ndb properties ..." << flush;

    ostringstream msg;

    mgmdConnect = toString(props[L"ndb.mgmdConnect"]);
    if (mgmdConnect.empty()) {
        mgmdConnect = string("localhost");
    }

    catalog = toString(props[L"ndb.catalog"]);
    if (catalog.empty()) {
        catalog = string("crunddb");
    }

    schema = toString(props[L"ndb.schema"]);
    if (schema.empty()) {
        schema = string("def");
    }

    //if (msg.tellp() == 0) {
    if (msg.str().empty()) {
        cout << "      [ok]" << endl;
    } else {
        cout << endl << msg.str() << endl;
    }

    descr = "ndbapi(" + mgmdConnect + ")";
}

	/**
	 * 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;
	}

Conf& Conf::def_int(std::string name,
            int lower_bound,
            int upper_bound,
            int default_value) {
    assert2(lower_bound <= default_value && default_value <= upper_bound,
                MS() << "Default value for " << name << "not in range.");
    auto i = make_shared<Int>();
    i->lower_bound = lower_bound;
    i->upper_bound = upper_bound;
    i->default_value = default_value;
    i->value = default_value;

    items[name] = i;
    return *this;
}

	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);
	}

 vector<BeamTreeResult<T>> best_trees(vector<Mat<T>> input, int beam_width) const {
     auto leaves = convert_to_leaves(input);
     vector<PartialTree> candidates = { PartialTree(leaves) };
     while (candidates[0].nodes.size() > 1) {
         vector<PartialTree> new_candidates;
         for (auto& candidate: candidates) {
             for (auto& new_candidate: cangen(candidate, beam_width)) {
                 new_candidates.emplace_back(new_candidate);
             }
         }
         sort(new_candidates.begin(), new_candidates.end(),
         [this](const PartialTree& c1, const PartialTree& c2) {
             return candidate_log_probability(c1) > candidate_log_probability(c2);
         });
         candidates = vector<PartialTree>(
                          new_candidates.begin(),
                          new_candidates.begin() + min((size_t)beam_width, new_candidates.size())
                      );
         for (size_t cidx = 0; cidx + 1 < candidates.size(); ++cidx) {
             assert2(candidates[cidx].nodes.size() == candidates[cidx + 1].nodes.size(),
                     "Generated candidates of different sizes.");
         }
     }
     vector<BeamTreeResult<T>> results;
     for (auto& tree: candidates) {
         results.emplace_back(tree.nodes[0], tree.derivation);
     }
     return results;
 }

shared_ptr<visualizable::Tree> visualize_derivation(vector<uint> derivation, vector<string> words) {
    using visualizable::Tree;

    vector<shared_ptr<Tree>> result;
    std::transform(words.begin(), words.end(), std::back_inserter(result),
    [](const string& a) {
        return make_shared<Tree>(a);
    });
    for (auto merge_idx : derivation) {
        vector<shared_ptr<Tree>> new_result;
        for(size_t ridx = 0; ridx < merge_idx; ++ridx) {
            new_result.push_back(result[ridx]);
        }
        new_result.push_back(make_shared<Tree>(std::initializer_list<shared_ptr<Tree>> {
            result[merge_idx],
            result[merge_idx + 1]
        }));
        for(size_t ridx = merge_idx + 2; ridx < result.size(); ++ridx) {
            new_result.push_back(result[ridx]);
        }
        result = new_result;
    }
    assert2(result.size() == 1, "Szymon messed up.");

    return result[0];
}

void MAVConnTCPClient::client_connected(size_t server_channel)
{
	logInform(PFXd "Got client, id: %zu, address: %s",
			server_channel, conn_id, to_string_ss(server_ep).c_str());

	// start recv
	socket.get_io_service().post(std::bind(&MAVConnTCPClient::do_recv, this));
}

// Quantise a subband in in-place transform order
// This version of quantise_subbands assumes multiple quantisers per subband.
// It may be used for either quantising slices or for quantising subbands with codeblocks
const Array2D quantise_subbands(const Array2D& coefficients, const BlockVector& qIndices) {
  const Index transformHeight = coefficients.shape()[0];
  const Index transformWidth = coefficients.shape()[1];
  // TO DO: Check numberOfSubbands=3n+1 ?
  const int numberOfSubbands = qIndices.size();
  const int waveletDepth = (numberOfSubbands-1)/3;
  Index stride, offset; // stride is subsampling factor, offset is subsampling phase
  Array2D result(coefficients.ranges());

  // Create a view of the coefficients, representing the LL subband, quantise it,
  // then assign the result a view of the results array. This puts the quantised
  // LL subband into the result array in in-place transform order.
  // ArrayIndices2D objects specify the subset of array elements within a view,
  // that is they specify the subsampling factor and subsampling phase.
  stride = pow(2, waveletDepth);
  const ArrayIndices2D LLindices = // LLindices specifies the samples in the LL subband
    indices[Range(0,transformHeight,stride)][Range(0,transformWidth,stride)];
  result[LLindices] =
    quantise_LLSubband(coefficients[LLindices], qIndices[0]);

  // Next quantise the other subbands
  // Note: Level numbers go from zero for the lowest ("DC") frequencies to depth for
  // the high frequencies. This corresponds to the convention in the VC-2 specification.
  // Subands go from zero ("DC") to numberOfSubbands-1 for HH at the highest level
  for (char level=1, band=1; level<=waveletDepth; ++level) {
    stride = pow(2, waveletDepth+1-level);
    offset = stride/2;
    // Create a view of coefficients corresponding to a subband, then quantise it
    //Quantise HL subband
    const ArrayIndices2D HLindices = // HLindices specifies the samples in the HL subband
      indices[Range(0,transformHeight,stride)][Range(offset,transformWidth,stride)];
    result[HLindices] = quantise_block(coefficients[HLindices], qIndices[band++]);
    //Quantise LH subband
    const ArrayIndices2D LHindices = // LHindices specifies the samples in the LH subband
      indices[Range(offset,transformHeight,stride)][Range(0,transformWidth,stride)];
    result[LHindices] = quantise_block(coefficients[LHindices], qIndices[band++]);
    //Quantise HH subband
    const ArrayIndices2D HHindices = // HHindices specifies the samples in the HH subband
      indices[Range(offset,transformHeight,stride)][Range(offset,transformWidth,stride)];
    result[HHindices] = quantise_block(coefficients[HHindices], qIndices[band++]);
  }

  return 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 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);
	}

    virtual void run(unsigned int generations,
                     unsigned int logFrequency = 100) {
        // Generation: create the new members
        for (auto i = 0U; i < PopSize; ++i) {
            population.push_back(generator());
        }

        for (auto generation = 0U; generation < generations; ++generation) {
            // Crossover: Add missing members
            auto popSizePostSelection = population.size();
            while (population.size() < PopSize) {
                population.push_back(
                    crossover(population[random_uint(popSizePostSelection)],
                              population[random_uint(popSizePostSelection)]));
            }

            // Mutation: Mutate at least rate*popsize members
            for (size_t i = 0; i < PopSize * mutationRate; ++i) {
                auto index = random_uint(PopSize);
                mutator(population[index]);
            }

            // Selection: Destroy the least fit members
            selector(population, evaluator);

            if (evaluator(population[0]) > bestScore) {
                bestMember = population[0];
                bestScore = evaluator(population[0]);
            }

            if (generation % logFrequency == 0) {
                std::cout << "Generation(" << generation
                          << ") - Fitness:" << bestScore << std::endl;
            }
        }
        std::cout << "Best: ";
        for (const auto& allele : bestMember) {
            std::cout << allele << " ";
        }
        std::cout << std::endl << "Fitness: " << evaluator(bestMember)
                  << std::endl;
    }

        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);
	}