void *do_thread_io(void *) {
enum { MAX_EVENTS = 10 };
int err;
struct epoll_event *event;
struct epoll_event events[MAX_EVENTS];
int inx;
QE *qe;
while (!thread_io_done) {
if (verbose)
printf("epoll_wait start\n");
err = epoll_wait(epoll_fd, events, MAX_EVENTS, 1000);
if (verbose)
printf("epoll_wait rtn=%d\n", err);
assert(err != -1);
for (inx = 0; inx < err; inx++) {
event = &events[inx];
if (verbose)
printf("epoll_wait fd=%d, event=0x%x\n",
event->data.fd,
event->events);
qe = new QE();
memcpy(&qe->event, event, sizeof(*event));
io_q.add(qe);
XAWAKE(pin_main, PWU);
}
}
return NULL;
}
bool spfa(int &flow,int &cost){
memset(dist,0x3f,sizeof(dist));
dist[S]=0;
q.init(N);
q.push(S);
while(!q.empty()){
int u=q.front();
q.pop();
for(int i=le[u]; i!=-1; i=pe[i]){
Edge &e=edges[i];
if(e.data){
if(dist[e.v]>dist[u]+e.cost){
dist[e.v]=dist[u]+e.cost;
pre[e.v]=i;
q.push(e.v);
}
}
}
}
if(dist[T]>=INF)return false;
int u=T;
while(u!=S){
Edge &e=edges[pre[u]];
edges[pre[u]].data--;
edges[1^pre[u]].data++;
u=edges[pre[u]].u;
}
cost+=dist[T];
flow++;
return true;
}
void do_io_comp() {
QE *qe;
for (;;) {
qe = io_q.remove();
if (qe == NULL)
break;
if (verbose)
printf("completion fd=%d, event=0x%x\n",
qe->event.data.fd,
qe->event.events);
delete qe;
}
}
void posCallback(const pa10_controller::Positions::ConstPtr& msg){
//ROS_INFO("I HEARD: %0.2f %0.2f %0.2f %0.2f %0.2f %0.2f %0.2f", msg->positions[0],msg->positions[1],msg->positions[2],msg->positions[3],msg->positions[4],msg->positions[5],msg->positions[6]);
if (positions.size() == 0){
positions = Q(7,0,0,0,0,0,0,0);
for (int i = 0; i < (int) msg->positions.size(); i ++){
positions[i] = msg->positions[i]*Pi/180;
}
device->setQ(positions, state);
}else{
for (int i = 0; i < (int) msg->positions.size(); i ++){
positions[i] = msg->positions[i]*Pi/180;
}
}
calculatePose(positions);
}
bool pop_or_not_with_predicate (Predicate& p)
{
return !running || ( !queue.empty() && p(front_adapter<Q> ()) );
};
bool pop_or_not ()
{
return !running || !queue.empty();
};
typename std::enable_if<container_traits::has_push_back<S>::value, void>::type
push_adapter (const T& t) { queue.push_back (t); };
typename std::enable_if<container_traits::has_pop_front<S>::value, void>::type
pop_adapter () { queue.pop_front (); };
typename std::enable_if<container_traits::has_top<S>::value, const T&>::type
front_adapter () const { return queue.top (); };
bool empty ()
{
return queue.empty ();
};
void u() { q_.setM(m_); }
namespace nf{
struct Edge{
int u,v,data,cost;
void init(int a,int b,int c,int d){
u=a,v=b,data=c,cost=d;
}
};
struct Q{
int size,inq[N+10],q[N+10],fr,re,contain;
void init(int n){
fr=0,re=-1;
size=n+1;
contain=0;
memset(inq,0,sizeof(inq));
}
void push(int u){
if(inq[u])return;
contain++;
re++;
if(re==size)re=0;
q[re]=u;
inq[u]=1;
}
int front(){
return q[fr];
}
void pop(){
contain--;
inq[q[fr]]=0;
fr++;
if(fr==size)fr=0;
}
bool empty(){
return contain==0;
}
};
int S,T;
Edge edges[M];
int le[N],pe[M],ecnt;
Q q;
void addEdge(int u,int v,int cap,int cost){
pe[ecnt]=le[u];
edges[ecnt].init(u,v,cap,cost);
le[u]=ecnt++;
pe[ecnt]=le[v];
edges[ecnt].init(v,u,0,-cost);
le[v]=ecnt++;
}
int dist[N],pre[N];
bool spfa(int &flow,int &cost){
memset(dist,0x3f,sizeof(dist));
dist[S]=0;
q.init(N);
q.push(S);
while(!q.empty()){
int u=q.front();
q.pop();
for(int i=le[u]; i!=-1; i=pe[i]){
Edge &e=edges[i];
if(e.data){
if(dist[e.v]>dist[u]+e.cost){
dist[e.v]=dist[u]+e.cost;
pre[e.v]=i;
q.push(e.v);
}
}
}
}
if(dist[T]>=INF)return false;
int u=T;
while(u!=S){
Edge &e=edges[pre[u]];
edges[pre[u]].data--;
edges[1^pre[u]].data++;
u=edges[pre[u]].u;
}
cost+=dist[T];
flow++;
return true;
}
void mcmf(int &f,int &c){
f=0,c=0;
while(spfa(f,c));
}
void init(int s,int t){
S=s,T=t;
memset(le,-1,sizeof(le));
}
}