C++ (Cpp) vector Examples

C++ (Cpp) vector - 30 examples found. These are the top rated real world C++ (Cpp) examples of std::vector extracted from open source projects. You can rate examples to help us improve the quality of examples.
Example #1
0
    void inverse_times(const std::vector<double>& v,
                       std::vector<double>& x) const
    {
        const size_t n = inv_diag.size();
        assert(v.size() == n);
        x.resize(n);

        for (size_t i=0; i<n; ++i) {
            unsigned int r = src_row[i];
            double yi = v[src_row[i]];
            for (SparseMatrix::const_row_iterator it = lu.row_begin(r);
                    it != lu.row_end(r);
                    ++it)
            {
                if (it.col() >= i)
                    continue;
                yi -= *it * x[it.col()];
            }
            x[i] = yi;
        }
        for (int i=n-1; i>=0; --i) {
            double yi = x[i];
            unsigned int r = src_row[i];
            for (SparseMatrix::const_row_iterator it = lu.row_begin(r);
                    it != lu.row_end(r);
                    ++it)
            {
                if ((int)it.col() <= i)
                    continue;
                yi -= *it * x[it.col()];
            }
            x[i] = inv_diag[i] * yi;
        }
    }
bool TebOptimalPlanner::plan(const std::vector<geometry_msgs::PoseStamped>& initial_plan, const geometry_msgs::Twist* start_vel, bool free_goal_vel)
{
    ROS_ASSERT_MSG(initialized_, "Call initialize() first.");
    if (!teb_.isInit())
    {
        // init trajectory
        teb_.initTEBtoGoal(initial_plan, cfg_->trajectory.dt_ref, true, cfg_->trajectory.min_samples);
    }
    else // warm start
    {
        PoseSE2 start_(initial_plan.front().pose.position.x, initial_plan.front().pose.position.y, tf::getYaw(initial_plan.front().pose.orientation));
        PoseSE2 goal_(initial_plan.back().pose.position.x, initial_plan.back().pose.position.y, tf::getYaw(initial_plan.back().pose.orientation));
        if (teb_.sizePoses()>0 && (goal_.position() - teb_.BackPose().position()).norm() < cfg_->trajectory.force_reinit_new_goal_dist) // actual warm start!
            teb_.updateAndPruneTEB(start_, goal_, cfg_->trajectory.min_samples); // update TEB
        else // goal too far away -> reinit
        {
            ROS_DEBUG("New goal: distance to existing goal is higher than the specified threshold. Reinitalizing trajectories.");
            teb_.clearTimedElasticBand();
            teb_.initTEBtoGoal(initial_plan, cfg_->trajectory.dt_ref, true, cfg_->trajectory.min_samples);
        }
    }
    if (start_vel)
        setVelocityStart(*start_vel);
    if (free_goal_vel)
        setVelocityGoalFree();
    else
        vel_goal_.first = true; // we just reactivate and use the previously set velocity (should be zero if nothing was modified)

    // now optimize
    return optimizeTEB(cfg_->optim.no_inner_iterations, cfg_->optim.no_outer_iterations);
}
Example #3
0
// Construct the output (winner / loser) list based on the book kept data
// Two possible scenarios:
// (1) Goal is BEST: Output will be decided + best ones from in_race (if required).
// (2) Goal is WORST: Output will be discarded + worst ones from in_race (if required).
std::vector<population::size_type> race_pop::construct_output_list(
    const std::vector<racer_type>& racers,
    const std::vector<population::size_type>& decided,
    const std::vector<population::size_type>& in_race,
    const std::vector<population::size_type>& discarded,
    const population::size_type n_final,
    const bool race_best)
{
    typedef population::size_type size_type;
    // To be sorted either in ascending (BEST) or descending (WORST)
    std::vector<std::pair<double, size_type> > argsort;
    for(std::vector<size_type>::const_iterator it = in_race.begin(); it != in_race.end(); ++it) {
        argsort.push_back(std::make_pair(racers[*it].m_mean, *it));
    }
    std::vector<size_type> output;
    if(race_best) {
        output = decided;
        std::sort(argsort.begin(), argsort.end(), std::less<std::pair<double, size_type> >());
    }
    else {
        output = discarded;
        std::sort(argsort.begin(), argsort.end(), std::greater<std::pair<double, size_type> >());
    }
    int sorted_idx = 0;
    while(output.size() < n_final) {
        output.push_back(argsort[sorted_idx++].second);
    }

    return output;
}
/***********************************************************************
 * transmit_worker function
 * A function to be used as a boost::thread_group thread for transmitting
 **********************************************************************/
void transmit_worker(
    std::vector<std::complex<float> > buff,
    wave_table_class wave_table,
    uhd::tx_streamer::sptr tx_streamer,
    uhd::tx_metadata_t metadata,
    size_t step,
    size_t index,
    int num_channels
) {
    std::vector<std::complex<float> *> buffs(num_channels, &buff.front());

    //send data until the signal handler gets called
    while(not stop_signal_called) {
        //fill the buffer with the waveform
        for (size_t n = 0; n < buff.size(); n++) {
            buff[n] = wave_table(index += step);
        }

        //send the entire contents of the buffer
        tx_streamer->send(buffs, buff.size(), metadata);

        metadata.start_of_burst = false;
        metadata.has_time_spec = false;
    }

    //send a mini EOB packet
    metadata.end_of_burst = true;
    tx_streamer->send("", 0, metadata);
}
Example #5
0
/* Must be called after LmrModel init is called */
void SpaceStation::Init()
{
    if (stationTypesInitted) return;
    stationTypesInitted = true;
    for (int is_orbital=0; is_orbital<2; is_orbital++) {
        std::vector<LmrModel*> models;
        if (is_orbital) LmrGetModelsWithTag("orbital_station", models);
        else LmrGetModelsWithTag("surface_station", models);

        for (std::vector<LmrModel*>::iterator i = models.begin();
                i != models.end(); ++i) {
            SpaceStationType t;
            t.modelName = (*i)->GetName();
            t.model = LmrLookupModelByName(t.modelName);
            t.dockMethod = SpaceStationType::DOCKMETHOD(is_orbital);
            t.numDockingPorts = (*i)->GetIntAttribute("num_docking_ports");
            t.dockOneAtATimePlease = (*i)->GetBoolAttribute("dock_one_at_a_time_please");
            t.ReadStageDurations();
            //printf("one at a time? %s\n", t.dockOneAtATimePlease ? "yes" : "no");
            //printf("%s: %d docking ports\n", t.modelName, t.numDockingPorts);
            if (is_orbital) {
                t.angVel = (*i)->GetFloatAttribute("angular_velocity");
                orbitalStationTypes.push_back(t);
            }
            else surfaceStationTypes.push_back(t);
        }
    }
    //printf(SIZET_FMT " orbital station types and " SIZET_FMT " surface station types.\n", orbitalStationTypes.size(), surfaceStationTypes.size());
}
Example #6
0
    void unpack(const char* data, size_t size) {
        const uint8_t* p = reinterpret_cast<const uint8_t*>(data);
        const uint8_t* pend = p+size;

        services.clear();

        original_network_id = get16(p);
        p += 3;

        while(pend - p > 4) {
            services.resize(services.size()+1);
            auto& s = services.back();

            s.service_id = get16(p);
            p += 3;
            size_t loop_length = get16(p) & 0x0FFF;
            p += 2;
            const uint8_t* ploop_end = p + loop_length;
            if(pend - p - 4 < loop_length)
                std::runtime_error("");

            while(p < ploop_end) {
                s.descriptors.resize(s.descriptors.size()+1);
                s.descriptors.back().unpack(&p, ploop_end);
            }
        }
    }