reference operator[](size_type index)
{
if(index < pos_vec.size()) {
return pos_vec[index];
} else {
pos_vec.resize(index + 1, 0);
return pos_vec[index];
}
};
static inline void parse(const bbb::json &json, std::deque<type, alloc> &vec) {
if(!json.is_array()) {
detail::print_parse_error("bbb::json_utils::parse deque", skip_json_isnt_array);
return;
}
vec.resize(json.size());
for(std::size_t i = 0; i < json.size(); ++i) {
parse(json[i], vec[i]);
}
}
inline void unpack( Stream& s, std::deque<T>& value ) {
unsigned_int size; unpack( s, size );
FC_ASSERT( size.value*sizeof(T) < MAX_ARRAY_ALLOC_SIZE );
value.resize(size.value);
auto itr = value.begin();
auto end = value.end();
while( itr != end ) {
fc::raw::unpack( s, *itr );
++itr;
}
}
/**
* Push an item into the queue.
*
* @param value The item to push into the queue.
* @param num Number to describe ordering for the items.
* It must increase monotonically.
*/
void push(T value, size_type num) {
std::lock_guard<std::mutex> lock(m_mutex);
num -= m_offset;
if (m_queue.size() <= num + 1) {
m_queue.resize(num + 2);
}
m_queue[num] = std::move(value);
m_data_available.notify_one();
}
//! Loading for std::deque
template <class Archive, class T, class A> inline
void load( Archive & ar, std::deque<T, A> & deque )
{
size_t size;
ar( make_size_tag( size ) );
deque.resize( size );
for( auto & i : deque )
ar( i );
}
//! Loading for std::deque
template <class Archive, class T, class A> inline
void CEREAL_LOAD_FUNCTION_NAME( Archive & ar, std::deque<T, A> & deque )
{
size_type size;
ar( make_size_tag( size ) );
deque.resize( static_cast<size_t>( size ) );
for( auto & i : deque )
ar( i );
}
void addpollfd(int fd, short events, FDTYPE type, std::string name) {
if(fdinfos.size() <= (size_t) fd) fdinfos.resize(fd + 1);
if(fdinfos[fd].type != FDTYPE::NONE) {
fprintf(stderr, "Attempt to add duplicate fd to poll array detected, ignoring: fd: %d\n", fd);
return;
}
fdinfos[fd].type = type;
fdinfos[fd].pollfd_offset = pollfds.size();
fdinfos[fd].name = std::move(name);
pollfds.push_back({ fd, events, 0 });
}
void RRTstar_planning(){
ROS_INFO("RRT star planning");
ob::StateSpacePtr space(new ob::RealVectorStateSpace(2));
space->as<ob::RealVectorStateSpace>()->setBounds(minX(local_map->info), maxX(local_map->info));
space->setLongestValidSegmentFraction(0.01/(maxX(local_map->info)-minX(local_map->info)));
ob::SpaceInformationPtr si(new ob::SpaceInformation(space));
si->setStateValidityChecker(ob::StateValidityCheckerPtr(new ValidityChecker(si)));
si->setup();
ob::ScopedState<> start(space);
start->as<ob::RealVectorStateSpace::StateType>()->values[0] = startState[0];
start->as<ob::RealVectorStateSpace::StateType>()->values[1] = startState[1];
ob::ScopedState<> goal(space);
goal->as<ob::RealVectorStateSpace::StateType>()->values[0] = goalState[0];
goal->as<ob::RealVectorStateSpace::StateType>()->values[1] = goalState[1];
ob::ProblemDefinitionPtr pdef(new ob::ProblemDefinition(si));
pdef->setStartAndGoalStates(start, goal);
pdef->setOptimizationObjective(ob::OptimizationObjectivePtr(new ClearanceObjective(si)));
og::RRTstar *plan_pt=new og::RRTstar(si);
plan_pt->setGoalBias(0.05);
plan_pt->setRange(1.0);
ob::PlannerPtr optimizingPlanner(plan_pt);
optimizingPlanner->setProblemDefinition(pdef);
optimizingPlanner->setup();
ob::PlannerStatus solved;
int it=0;
while(solved!=ompl::base::PlannerStatus::StatusType::EXACT_SOLUTION && it<100){
it++;
solved = optimizingPlanner->solve(1.0);
}
if(solved==ompl::base::PlannerStatus::StatusType::EXACT_SOLUTION){
std::vector< ob::State * > sol = boost::static_pointer_cast<og::PathGeometric>(pdef->getSolutionPath())->getStates();
current_path_raw.resize(sol.size());
std::transform(sol.begin(), sol.end(), current_path_raw.begin(), &obstateState);
/*std::cout << "Path length: " << pdef->getSolutionPath()->length() << std::endl;
std::cout << "Path cost: " << pdef->getSolutionPath()->cost(pdef->getOptimizationObjective()) << std::endl;
std::cout << "Path :" << std::endl;
for(std::deque<State<2>>::iterator it=current_path_raw.begin(),end=current_path_raw.end();it!=end;++it){
std::cout << (*it) << " -- ";
}
std::cout << std::endl;*/
planned=true;
}else{
ROS_INFO("Planning failed");
planned=false;
}
}
pos& operator+= (int i)
{
if(i == 0)
throw std::logic_error("Cannot add zero to a pos class");
int index = abs(i) - 1;
int len = pos_vec.size();
if(index < len)
pos_vec[index] += Utility::sign(i);
else {
pos_vec.resize(index + 1, 0);
pos_vec[index] += Utility::sign(i);
}
return *this;
};
int main(int argc, char **argv) {
signal(SIGINT, handler);
while (1) {
std::string args;
args = readline();
if (args.length() < 1) break;
std::deque<std::string> tokens;
tokens = split(args, '|', tokens);
pids.resize(tokens.size());
std::deque<int *> ffd(tokens.size(), new int[2]);
for (int i = 0; i < tokens.size() - 1; ++i) {
pipe(ffd[i]);
}
for (int i = 0; i < tokens.size(); ++i) {
int id;
if ((id = fork()) > 0) {
pids[i] = id;
} else {
if (i != 0) {
//dup2(ffd[i - 1][1], ffd[i][0]);
dup2(ffd[i - 1][1], STDIN_FILENO);
close(ffd[i - 1][0]);
}
if (i != tokens.size() - 1) {
dup2(ffd[i][1], STDOUT_FILENO);
}
proceedCommand(tokens[i]);
close(ffd[i][1]);
close(ffd[i][0]);
}
if (id < 0) return 0;
}
for (int i = 0, j; i < tokens.size(); ++i) {
waitpid(pids[i], &j, 0);
}
for (int i = 0; i < tokens.size() - 1; ++i) {
close(ffd[i][0]);
close(ffd[i][1]);
}
pids.clear();
}
}
double shift_quanta()
{
double factor;
factor = 0; // return 0 is shift failure.
if (oscillating(m_past_length - 1) &&
(0 != (archive.back().date - archive.front().date))) {
Trace(context(), 7, "Oscillating, archive size = %zu (m_past_length = %u) ",
archive.size(), m_past_length);
double acc;
double local_estim;
int cnt;
acc = 0;
cnt = 0;
for (size_t i = 0; i < archive.size() - 2; ++i) {
if (0 != (archive[i + 2].date - archive[i].date)) {
if ((archive.back().value * archive[i + 1].value) >
0) // same direction as last move
{
local_estim =
1 -
(archive[i + 1].date - archive[i].date) /
(archive[i + 2].date - archive[i].date);
} else {
local_estim = (archive[i + 1].date - archive[i].date) /
(archive[i + 2].date - archive[i].date);
}
acc += local_estim;
cnt++;
}
}
acc = acc / cnt;
factor = acc;
archive.resize(0);
}
return factor;
}
// 产生uRandNum个不相同的随机数，并添加到queRand末尾
void Rand(std::deque<unsigned int> &queRand, unsigned int uRandNum)
{
if (uRandNum <= 0)
{
return;
}
unsigned uSizeOld = queRand.size();
unsigned uSizeNew = uSizeOld + uRandNum;
queRand.resize(uSizeNew);
srand(unsigned(time(NULL)));
for(unsigned i = uSizeOld; i < uSizeNew; i++)
{
queRand[i] = i;
}
for(unsigned i = uSizeOld; i < uSizeNew; i++)
{
std::swap(queRand[i], queRand[rand() % uSizeNew]);
}
}
void pop_values(int nbValues)
{
valueStack.resize(valueStack.size() - nbValues);
}
in_order(T_arg_list args = {})
: super(args)
{
parse(args, false);
inter_pkt_delays.resize(max_inter_pkt_delays);
}
void resize_back(size_type new_size)
{
pos_vec.resize(new_size,0);
};
void CorrectPairedReads(AssemblyInfo &assembly_info, std::deque<Sequence> &contigs,
std::deque<ContigInfo> &contig_infos, const std::string &align_file, double mean, double sd, int max_mismatch, int min_match)
{
int read_length = assembly_info.read_length();
vector<vector<uint32_t> > read_counts(contigs.size());
deque<vector<Score> > scores_table(contigs.size());
deque<vector<short> > overlap(contigs.size());
#pragma omp parallel for
for (int64_t i = 0; i < (int64_t)contigs.size(); ++i)
{
scores_table[i].resize(contigs[i].size());
overlap[i].resize(contigs[i].size(), 0);
read_counts[i].resize(omp_get_max_threads(), 0);
}
FILE *falign = OpenFile(align_file, "rb");
deque<ShortSequence> &reads = assembly_info.reads;
int64_t buffer_size = (1 << 20) * omp_get_max_threads();
for (int64_t offset = 0; offset < (int64_t)reads.size(); offset += buffer_size)
{
int64_t size = min((int64_t)buffer_size, (int64_t)(reads.size() - offset));
vector<HashAlignerRecord> all_records(size);
ReadHashAlignerRecordBlock(falign, all_records);
#pragma omp parallel for
for (int64_t i = 0; i < size; ++i)
{
HashAlignerRecord &record = all_records[i];
if (record.match_length != 0)
//if (record.match_length != 0 && record.query_length - record.match_length <= max_mismatch)
{
//#pragma omp atomic
++read_counts[record.ref_id][omp_get_thread_num()];
Sequence seq(reads[offset + i]);
bool is_reverse = record.is_reverse;
if (is_reverse)
{
record.ReverseComplement();
seq.ReverseComplement();
}
vector<Score> &scores = scores_table[record.ref_id];
int id = record.ref_id;
for (unsigned j = 0; j < seq.size(); ++j)
{
int index = record.ref_from + j;
#pragma omp atomic
scores[index][seq[j]]++;
if (min(int(j), int(seq.size() - j)) >= read_length/3)
{
#pragma omp atomic
overlap[id][index]++;
}
}
if (is_reverse)
{
record.ReverseComplement();
seq.ReverseComplement();
}
}
}
}
fclose(falign);
int64_t conformed_bases = 0;
// int64_t t1 = 0;
// int64_t t2 = 0;
// int64_t t3 = 0;
#pragma omp parallel for reduction(+: conformed_bases)
for (int64_t i = 0; i < (int64_t)contigs.size(); ++i)
{
for (unsigned j = 0; j < contigs[i].size(); ++j)
{
int aux[4];
for (int k = 0; k < 4; ++k)
aux[k] = scores_table[i][j][k];
sort(aux, aux + 4);
if (aux[3] > aux[2] * 4 && aux[3] >= min_match && overlap[i][j] >= 1)
{
++conformed_bases;
int best = -1;
for (int k = 0; k < 4; ++k)
{
if (scores_table[i][j][k] == aux[3])
best = k;
}
contigs[i][j] = best;
scores_table[i][j].best = best;
}
}
}
falign = OpenFile(align_file, "rb");
int64_t corrected_reads = 0;
int64_t corrected_bases = 0;
for (int64_t offset = 0; offset < (int64_t)reads.size(); offset += buffer_size)
{
int64_t size = min((int64_t)buffer_size, (int64_t)(reads.size() - offset));
vector<HashAlignerRecord> all_records(size);
vector<int> flags(size, 1);
ReadHashAlignerRecordBlock(falign, all_records);
#pragma omp parallel for reduction(+: corrected_reads, corrected_bases)
for (int64_t i = 0; i < size; i += 2)
{
HashAlignerRecord r1 = all_records[i];
HashAlignerRecord r2 = all_records[i+1];
HashAlignerRecord tmp = r2;
tmp.ReverseComplement();
double d = fabs(tmp.ref_to - r1.ref_from - mean);
if (r1.match_length != 0 && r2.match_length != 0
&& r1.ref_id == r2.ref_id
&& r1.is_reverse != r2.is_reverse
&& d < 2*sd)
{
Sequence seq1(reads[offset + i]);
Sequence seq2(reads[offset + i+1]);
seq2.ReverseComplement();
r2.ReverseComplement();
bool is_reverse = r1.is_reverse;
if (is_reverse)
{
seq1.ReverseComplement();
r1.ReverseComplement();
seq2.ReverseComplement();
r2.ReverseComplement();
}
vector<Score> &scores = scores_table[r1.ref_id];
Sequence &contig = contigs[r1.ref_id];
bool is_all_confirmed = true;
for (unsigned j = 0; j < seq1.size(); ++j)
{
if (scores[r1.ref_from + j].best == -1)
is_all_confirmed = false;
}
for (unsigned j = 0; j < seq2.size(); ++j)
{
if (scores[r2.ref_from + j].best == -1)
is_all_confirmed = false;
}
if (!is_all_confirmed)
{
goto SINGLE_CORRECT;
}
corrected_reads += 2;
for (unsigned j = 0; j < seq1.size(); ++j)
{
if (seq1[j] != scores[r1.ref_from+j].best)
{
seq1[j] = contig[r1.ref_from+j];
++corrected_bases;
}
}
for (unsigned j = 0; j < seq2.size(); ++j)
{
if (seq2[j] != scores[r2.ref_from+j].best)
{
seq2[j] = contig[r2.ref_from+j];
++corrected_bases;
}
}
if (is_reverse)
{
seq1.ReverseComplement();
r1.ReverseComplement();
seq2.ReverseComplement();
r2.ReverseComplement();
}
seq2.ReverseComplement();
r2.ReverseComplement();
reads[offset + i] = seq1;
reads[offset + i+1] = seq2;
}
else
{
SINGLE_CORRECT:
r1 = all_records[i];
r2 = all_records[i+1];
if (r1.match_length != 0)
{
int id = i;
Sequence seq(reads[offset + i]);
HashAlignerRecord record = r1;
bool is_reverse = record.is_reverse;
if (is_reverse)
{
record.ReverseComplement();
seq.ReverseComplement();
}
int match_count = 0;
bool is_all_confirmed = true;
vector<Score> &scores = scores_table[record.ref_id];
Sequence &contig = contigs[record.ref_id];
for (unsigned j = 0; j < seq.size(); ++j)
{
if (seq[j] == contig[record.ref_from + j])
++match_count;
if (scores[record.ref_from + j].best == -1)
is_all_confirmed = false;
}
if (match_count >= (int)seq.size() - max_mismatch && is_all_confirmed)
{
++corrected_reads;
for (unsigned j = 0; j < seq.size(); ++j)
{
if (scores[record.ref_from + j].best != -1
&& seq[j] != contig[record.ref_from + j])
{
seq[j] = scores[record.ref_from + j].best;
++corrected_bases;
}
}
}
if (is_reverse)
{
record.ReverseComplement();
seq.ReverseComplement();
}
reads[offset + id] = seq;
}
if (r2.match_length != 0)
{
int id = i+1;
Sequence seq(reads[offset + i+1]);
HashAlignerRecord record = r2;
bool is_reverse = record.is_reverse;
if (is_reverse)
{
record.ReverseComplement();
seq.ReverseComplement();
}
int match_count = 0;
bool is_all_confirmed = true;
vector<Score> &scores = scores_table[record.ref_id];
Sequence &contig = contigs[record.ref_id];
for (unsigned j = 0; j < seq.size(); ++j)
{
if (seq[j] == contig[record.ref_from + j])
++match_count;
if (scores[record.ref_from + j].best == -1)
is_all_confirmed = false;
}
if (match_count >= (int)seq.size() - max_mismatch && is_all_confirmed)
{
++corrected_reads;
for (unsigned j = 0; j < seq.size(); ++j)
{
if (scores[record.ref_from + j].best != -1
&& seq[j] != contig[record.ref_from + j])
{
seq[j] = scores[record.ref_from + j].best;
++corrected_bases;
}
}
}
if (is_reverse)
{
record.ReverseComplement();
seq.ReverseComplement();
}
reads[offset + id] = seq;
}
}
}
copy(flags.begin(), flags.end(), assembly_info.read_flags.begin() + offset);
}
fclose(falign);
contig_infos.resize(contigs.size());
#pragma omp parallel for
for (int64_t i = 0; i < (int64_t)contigs.size(); ++i)
{
uint32_t sum = 0;
for (unsigned j = 0; j < read_counts[i].size(); ++j)
sum += read_counts[i][j];
contig_infos[i].set_kmer_count(sum);
}
cout << "confirmed bases: " << conformed_bases << " correct reads: " << corrected_reads << " bases: " << corrected_bases << endl;
//cout << "t1 t2 t3: " << t1 << " " << t2 << " " << t3 << endl;
}
void Astar_planning(){
ROS_INFO("A star planning");
std::priority_queue<PQItem> open_list;
const unsigned num_cells = local_map->info.height*local_map->info.width;
std::vector<bool> seen(num_cells); // Default initialized to all false
const occupancy_grid_utils::index_t dest_ind = occupancy_grid_utils::pointIndex(local_map->info,goal.point);
const occupancy_grid_utils::index_t src_ind = occupancy_grid_utils::pointIndex(local_map->info,statePoint(startState));
open_list.push(PQItem(src_ind, 0, h(src_ind),src_ind));
std::vector<occupancy_grid_utils::index_t> parent(num_cells,0);
while (!open_list.empty()) {
const PQItem current = open_list.top();
open_list.pop();
const occupancy_grid_utils::Cell c = occupancy_grid_utils::indexCell(local_map->info, current.ind);
if (seen[current.ind])
continue;
parent[current.ind] = current.parent_ind;
seen[current.ind] = true;
ROS_DEBUG_STREAM_NAMED ("shortest_path_internal", " Considering " << c << " with cost " <<
current.g_cost << " + " << current.h_cost);
if (current.ind == dest_ind) {
std::cout << "solution found" << std::endl;
std::cout << "Visited " << std::count(seen.begin(), seen.end(), true) << " states out of " << num_cells << std::endl;;
std::deque<occupancy_grid_utils::index_t> path;
path.push_back(dest_ind);
occupancy_grid_utils::index_t last = dest_ind;
while (parent[last]!=src_ind){
path.push_front(parent[last]);
last=parent[last];
}
/*for(std::deque<occupancy_grid_utils::index_t>::iterator it=path.begin(),end=path.end();it!=end;++it){
std::cout << occupancy_grid_utils::indexCell(local_map->info, *it) << " -- ";
}
std::cout << std::endl;*/
current_path_raw.resize(path.size());
std::transform(path.begin(), path.end(), current_path_raw.begin(), &indexState);
/*std::cout << "Path length: " << current_path_raw.size() << std::endl;
std::cout << "Path cost: " << current.g_cost << std::endl;
std::cout << "Path :" << std::endl;
for(std::deque<State<2>>::iterator it=current_path_raw.begin(),end=current_path_raw.end();it!=end;++it){
std::cout << (*it) << " -- ";
}
std::cout << std::endl;*/
planned=true;
return;
}
for (int d=-1; d<=1; d+=2) {
for (int vertical=0; vertical<2; vertical++) {
const int cx = c.x + d*(1-vertical);
const int cy = c.y + d*vertical;
if (cx>=0 && cy>=0) {
const occupancy_grid_utils::Cell c2((occupancy_grid_utils::coord_t) cx, (occupancy_grid_utils::coord_t) cy);
if (withinBounds(local_map->info, c2)) {
const occupancy_grid_utils::index_t ind = cellIndex(local_map->info, c2);
if (!isObstacle(pointState(indexPoint(ind))) && !seen[ind]) {
open_list.push(PQItem(ind, current.g_cost + g(ind), h(ind), current.ind));
}
//ROS_DEBUG_STREAM_COND_NAMED (g.data[ind]!=UNOCCUPIED, "shortest_path_internal",
// " Skipping cell " << indexCell(g.info, ind) <<
// " with cost " << (unsigned) g.data[ind]);
}
}
}
}
}
planning=false;
ROS_INFO("Planning failed");
}
inline void custom_resize(std::size_t new_size) { m_hidden_deque.resize(new_size); }
void ConsumerStateTable::pops(std::deque<KeyOpFieldsValuesTuple> &vkco, std::string /*prefix*/)
{
static std::string luaScript =
"local ret = {}\n"
"local keys = redis.call('SPOP', KEYS[1], ARGV[1])\n"
"local n = table.getn(keys)\n"
"for i = 1, n do\n"
"local key = keys[i]\n"
"local values = redis.call('HGETALL', KEYS[2] .. key)\n"
"table.insert(ret, {key, values})\n"
"end\n"
"return ret\n";
static std::string sha = loadRedisScript(m_db, luaScript);
RedisCommand command;
command.format(
"EVALSHA %s 2 %s %s: %d ''",
sha.c_str(),
getKeySetName().c_str(),
getTableName().c_str(),
POP_BATCH_SIZE);
RedisReply r(m_db, command);
auto ctx0 = r.getContext();
vkco.clear();
// if the set is empty, return an empty kco object
if (ctx0->type == REDIS_REPLY_NIL)
{
return;
}
assert(ctx0->type == REDIS_REPLY_ARRAY);
size_t n = ctx0->elements;
vkco.resize(n);
for (size_t ie = 0; ie < n; ie++)
{
auto& kco = vkco[ie];
auto& values = kfvFieldsValues(kco);
assert(values.empty());
auto& ctx = ctx0->element[ie];
assert(ctx->elements == 2);
assert(ctx->element[0]->type == REDIS_REPLY_STRING);
std::string key = ctx->element[0]->str;
kfvKey(kco) = key;
assert(ctx->element[1]->type == REDIS_REPLY_ARRAY);
auto ctx1 = ctx->element[1];
for (size_t i = 0; i < ctx1->elements / 2; i++)
{
FieldValueTuple e;
fvField(e) = ctx1->element[i * 2]->str;
fvValue(e) = ctx1->element[i * 2 + 1]->str;
values.push_back(e);
}
// if there is no field-value pair, the key is already deleted
if (values.empty())
{
kfvOp(kco) = DEL_COMMAND;
}
else
{
kfvOp(kco) = SET_COMMAND;
}
}
}