void do_magic( msg *m ) { unsigned int channel; #ifdef DEBUG char buf[32]; if ( fds[0] == -1 ) { for ( channel=0; channel < NR_CHANNELS; ++channel ) { sprintf( buf, "mic%d.txt", channel ); fds[channel] = open( buf, O_RDWR | O_CREAT | O_TRUNC ); } } #endif for ( channel = 0; channel < NR_CHANNELS; ++channel ) { // Operate on input m->data[channel] and write the result // as floats in channels[channel] stage1( m->data[channel], channels[channel] ); #ifdef DEBUG sprintf( buf, "%f\n", channels[channel][SAMPLES_PER_MSG - 1] ); write( fds[channel], buf, strlen( buf ) ); #endif } }
int main(int argc, char *argv[]) { if (argc < 2) { printf("Supply the disk dump as a parameter!\n"); return -1; } char* filename = argv[1]; FILE *fp = fopen(filename, "rb"); if (fp == NULL) { printf("Cannot open file\n"); return -1; } if (is_infected(fp)) { printf("[+] Petya FOUND on the disk!\n"); } else { printf("[-] Petya not found on the disk!\n"); } printf("---\n"); if (stage1(fp) == 0) { printf("[OK] Stage 1 key recovered!\n"); fclose(fp); return 0; } printf("Invalid Stage1 key! Probably the key has been already erased!\n"); printf("Try to recover from Stage2 by third-party decoder!\n"); printf("Paste the data you got below on one of the following sites:\n"); printf("+ https://petya-pay-no-ransom.herokuapp.com/\n"); printf("+ https://petya-pay-no-ransom-mirror1.herokuapp.com/\n"); stage2(fp); fclose(fp); return 0; }
int main() { printf("HaCkMe by Whistlepig\n"); printf("[*] Starting Stage 1\n"); printf("\tEnter your password\n"); #define STAGE1_KEY_SIZE 4 char stage1_key[STAGE1_KEY_SIZE]; for(int i = 0; i < STAGE1_KEY_SIZE; i++) { stage1_key[i] = getchar(); if(stage1_key[i] == 0 || stage1_key[i] == 10) break; } unsigned char stage1_result = stage1(stage1_key, strlen(stage1_key), stage1_data, 4); //printf("Stage 1 result: 0x%X\n", (int)(stage1_result & 0xFF)); if(stage1_result == (unsigned char)0xA1) // "Sam!" printf("[*] Stage 1 passed!\n"); else printf("[X] Stage 1 failed.\n"); return 0; }
int main( int argc, char* argv[] ) { if ( !strcmp( argv[ 1 ], "stage1" ) ) { stage1( argv[ 2 ], atoi( argv[ 3 ] ), atoi( argv[ 4 ] ) ); } else if( !strcmp( argv[ 1 ], "stage2" ) ) { stage2( argv[ 2 ], argv[ 3 ] ); } else if( !strcmp( argv[ 1 ], "stage3" ) ) { stage3( argv[ 2 ], argv[ 3 ], argv[ 4 ] ); } else if( !strcmp( argv[ 1 ], "stage4" ) ) { stage4( argv[ 2 ], argv[ 3 ], argv[ 4 ] ); } else if( !strcmp( argv[ 1 ], "stage5" ) ) { stage5( argv[ 2 ], argv + 3, argc - 3 ); } return 0; }
int main(int argc, char **argv) { int i, nbufs = 100; int packetsize = 1500; int iters = 1000 * 1000; if (argc == 2) { packetsize = atoi(argv[1]); } sz = packetsize; char** bufs = malloc(sizeof(char*) * nbufs); for (i = 0; i < nbufs; ++i) { bufs[i] = malloc(packetsize); memset(bufs[i], 0, packetsize); } char** bufs2 = malloc(sizeof(char*) * nbufs); for (i = 0; i < nbufs; ++i) { bufs2[i] = malloc(packetsize); memset(bufs2[i], 0, packetsize); } struct timespec ts; struct timespec te; clock_gettime(CLOCK_REALTIME, &ts); for (i = 0; i < iters; ++i) { void* ba = bufs[i % nbufs]; void* bb = bufs2[i % nbufs]; stage1(ba, bb); } clock_gettime(CLOCK_REALTIME, &te); time_t ds = te.tv_sec - ts.tv_sec; long dn = te.tv_nsec - ts.tv_nsec; double s = (double) ds + (((double) dn) / 1e9); printf("c-fn nocopy throughput in 'packets' of size %d per second: %.2lf\n", packetsize, (double) iters / s); return 0; }
bool execute() { schifra::utils::timer timer; timer.start(); bool result = stage1() && stage2() && stage3() && stage4() && stage5() && stage6() && stage7() && stage8() && stage9() && stage10() && stage11() && stage12(); timer.stop(); double time = timer.time(); print_codec_properties(); std::cout << "Blocks decoded: " << blocks_processed_ << "\tDecoding Failures: " << block_failures_ << "\tRate: " << ((blocks_processed_ * data_length) * 8.0) / (1048576.0 * time) << "Mbps" << std::endl; /* Note: The throughput rate is not only the throughput of reed solomon encoding and decoding, but also that of the steps needed to add simulated transmission errors to the reed solomon block such as the calculation of the positions and additions of errors and erasures to the reed solomon block, which normally in a true data transmission medium would not be taken into consideration. */ return result; }
int main (int argc, char *argv[]) { double *A,*A2,*L,*U, temp2; int i,j,k; int temp=0; int offset = 0; double t1,t2; if( argc > 1 ) N = atoi(argv[1]); if( argc > 2 ) //Block = atoi(argv[2]); M = atoi(argv[2]); A = (double *)malloc (N*N*sizeof(double)); A2 = (double *)malloc (N*N*sizeof(double)); L = (double *)malloc (N*N*sizeof(double)); U = (double *)malloc (N*N*sizeof(double)); if( A==NULL || A2==NULL || L==NULL || U==NULL) { printf("Can't allocate memory\n"); exit(1); } /* INITIALIZATION */ //InitMatrix(A,N); InitMatrix3(A,N); for(i=0; i<N*N; i++) { A2[i] = A[i]; // Copy of A for verification of correctness L[i] = 0; U[i] = 0; } /* /\* LU DECOMPOSITION *\/ */ /* for (k=0;k<N-1;k++){ */ /* for (i=k+1;i<N;i++){ */ /* A[i*N+k] = A[i*N+k]/A[k*N+k]; */ /* /\* for (i=k+1;i<N;i++) *\/ */ /* for (j=k+1;j<N;j++) */ /* A[i*N+j] = A[i*N+j] - A[i*N+k]*A[k*N+j]; */ /* } */ /* } */ int *sizedim; int *start; int R; //Remain int itr = 0; sizedim = (int*)malloc(M*sizeof(int)); start = (int*)malloc(M*sizeof(int)); R = N; t1 = GetTickCount(); #pragma omp parallel { //printf("The number of thread: %d\n", omp_get_num_threads()); #pragma omp master { while (N-offset>M){ // printf(" Iteration: %d\n", itr++); for (i=0;i<M;i++){ if (i<R%M){ sizedim[i]=R/M+1; start[i]=(R/M+1)*i; } else{ sizedim[i]=R/M; start[i]=(R/M+1)*(R%M)+(R/M)*(i-R%M); } //printf("%i,%i \n",sizedim[i],start[i]); } //Print_Matrix(sizedim,1,M); stage1(A, offset, sizedim, start, N, M); //Print_Matrix(A,N,N); stage2(A, offset, sizedim, start, N, M); //Print_Matrix(A,N,N); stage3(A, offset, sizedim, start, N, M); offset+=sizedim[0]; R=R-sizedim[0]; //Print_Matrix(A,N,N); } //while } //master } //omp parallel ProcessDiagonalBlock(&A[offset*N+offset], N-offset, N); t2 = GetTickCount(); printf("Time for LU-decomposition in secs: %f \n", (t2-t1)/1000000); //Print_Matrix(A,N,N); /* while (N-offset>Block){ */ /* stepLU(A,Block,offset,N); */ /* offset+=Block; */ /* } */ /* ProcessDiagonalBlock(&A[offset*N+offset], N-offset, N); */ //Print_Matrix(A,N,N); #ifdef CHECK /* PROOF OF CORRECTNESS */ for (i=0;i<N;i++) for (j=0;j<N;j++) if (i>j) L[i*N+j] = A[i*N+j]; else U[i*N+j] = A[i*N+j]; for (i=0;i<N;i++) L[i*N+i] = 1; //printf("L=\n"); //Print_Matrix(L,N,N); //printf("U=\n"); //Print_Matrix(U,N,N); for (i=0;i<N;i++) for (j=0;j<N;j++){ temp2=0; for (k=0;k<N;k++) temp2+=L[i*N+k]*U[k*N+j]; if ((A2[i*N+j]-temp2)/A2[i*N+j] >0.1 || (A2[i*N+j]-temp2)/A2[i*N+j] <-0.1) { temp++; printf("Error at: [%d, %d\n]",i,j); } } printf("Errors = %d \n", temp); #endif return 0; }
int main (int argc, char *argv[]) { double *A,*A2,*L,*U, temp2; int i,j,k; int temp=0; int offset = 0; double t1,t2; if (argc < 3) { printf("Usage: ./lu <Matrix size> <number of blocks per dimension>\n"); exit(1); } if( argc > 1 ) N = atoi(argv[1]); if( argc > 2 ) M = atoi(argv[2]); A = (double *)malloc (N*N*sizeof(double)); A2 = (double *)malloc (N*N*sizeof(double)); L = (double *)malloc (N*N*sizeof(double)); U = (double *)malloc (N*N*sizeof(double)); if( A==NULL || A2==NULL || L==NULL || U==NULL) { printf("Can't allocate memory\n"); exit(1); } /* INITIALIZATION */ InitMatrix3(A,N); for(i=0; i<N*N; i++) { A2[i] = A[i]; // Copy of A for verification of correctness L[i] = 0; U[i] = 0; } int *sizedim; int *start; int R; //Remain sizedim = (int*)malloc(M*sizeof(int)); start = (int*)malloc(M*sizeof(int)); R = N; t1 = GetTickCount(); #pragma omp parallel { #pragma omp master { while (N-offset>M){ for (i=0;i<M;i++){ if (i<R%M){ sizedim[i]=R/M+1; start[i]=(R/M+1)*i; } else{ sizedim[i]=R/M; start[i]=(R/M+1)*(R%M)+(R/M)*(i-R%M); } } stage1(A, offset, sizedim, start, N, M); stage2(A, offset, sizedim, start, N, M); stage3(A, offset, sizedim, start, N, M); offset+=sizedim[0]; R=R-sizedim[0]; } //end of while } //end of master } //end of parallel region ProcessDiagonalBlock(&A[offset*N+offset], N-offset, N); t2 = GetTickCount(); printf("Time for LU-decomposition in secs: %f \n", (t2-t1)/1000000); #ifdef CHECK /* PROOF OF CORRECTNESS */ for (i=0;i<N;i++) for (j=0;j<N;j++) if (i>j) L[i*N+j] = A[i*N+j]; else U[i*N+j] = A[i*N+j]; for (i=0;i<N;i++) L[i*N+i] = 1; for (i=0;i<N;i++) for (j=0;j<N;j++){ temp2=0; for (k=0;k<N;k++) temp2+=L[i*N+k]*U[k*N+j]; if ((A2[i*N+j]-temp2)/A2[i*N+j] >0.1 || (A2[i*N+j]-temp2)/A2[i*N+j] <-0.1) temp++; } printf("Errors = %d \n", temp); #endif return; }
bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr &planning_scene, const moveit_msgs::PlaceGoal &goal) { double timeout = goal.allowed_planning_time; ros::WallTime endtime = ros::WallTime::now() + ros::WallDuration(timeout); std::string attached_object_name = goal.attached_object_name; const robot_model::JointModelGroup *jmg = NULL; const robot_model::JointModelGroup *eef = NULL; // if the group specified is actually an end-effector, we use it as such if (planning_scene->getRobotModel()->hasEndEffector(goal.group_name)) { eef = planning_scene->getRobotModel()->getEndEffector(goal.group_name); if (eef) { // if we correctly found the eef, then we try to find out what the planning group is const std::string &eef_parent = eef->getEndEffectorParentGroup().first; if (eef_parent.empty()) { ROS_ERROR_STREAM_NAMED("manipulation", "No parent group to plan in was identified based on end-effector '" << goal.group_name << "'. Please define a parent group in the SRDF."); error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME; return false; } else jmg = planning_scene->getRobotModel()->getJointModelGroup(eef_parent); } } else { // if a group name was specified, try to use it jmg = goal.group_name.empty() ? NULL : planning_scene->getRobotModel()->getJointModelGroup(goal.group_name); if (jmg) { // we also try to find the corresponding eef const std::vector<std::string> &eef_names = jmg->getAttachedEndEffectorNames(); if (eef_names.empty()) { ROS_ERROR_STREAM_NAMED("manipulation", "There are no end-effectors specified for group '" << goal.group_name << "'"); error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME; return false; } else // check to see if there is an end effector that has attached objects associaded, so we can complete the place for (std::size_t i = 0 ; i < eef_names.size() ; ++i) { std::vector<const robot_state::AttachedBody*> attached_bodies; const robot_model::JointModelGroup *eg = planning_scene->getRobotModel()->getEndEffector(eef_names[i]); if (eg) { // see if there are objects attached to links in the eef planning_scene->getCurrentState().getAttachedBodies(attached_bodies, eg); // is is often possible that the objects are attached to the same link that the eef itself is attached, // so we check for attached bodies there as well const robot_model::LinkModel *attached_link_model = planning_scene->getRobotModel()->getLinkModel(eg->getEndEffectorParentGroup().second); if (attached_link_model) { std::vector<const robot_state::AttachedBody*> attached_bodies2; planning_scene->getCurrentState().getAttachedBodies(attached_bodies2, attached_link_model); attached_bodies.insert(attached_bodies.end(), attached_bodies2.begin(), attached_bodies2.end()); } } // if this end effector has attached objects, we go on if (!attached_bodies.empty()) { // if the user specified the name of the attached object to place, we check that indeed // the group contains this attachd body if (!attached_object_name.empty()) { bool found = false; for (std::size_t j = 0 ; j < attached_bodies.size() ; ++j) if (attached_bodies[j]->getName() == attached_object_name) { found = true; break; } // if the attached body this group has is not the same as the one specified, // we cannot use this eef if (!found) continue; } // if we previoulsy have set the eef it means we have more options we could use, so things are ambiguous if (eef) { ROS_ERROR_STREAM_NAMED("manipulation", "There are multiple end-effectors for group '" << goal.group_name << "' that are currently holding objects. It is ambiguous which end-effector to use. Please specify it explicitly."); error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME; return false; } // set the end effector (this was initialized to NULL above) eef = planning_scene->getRobotModel()->getEndEffector(eef_names[i]); } } } } // if we know the attached object, but not the eef, we can try to identify that if (!attached_object_name.empty() && !eef) { const robot_state::AttachedBody *attached_body = planning_scene->getCurrentState().getAttachedBody(attached_object_name); if (attached_body) { // get the robot model link this attached body is associated to const robot_model::LinkModel *link = attached_body->getAttachedLink(); // check to see if there is a unique end effector containing the link const std::vector<const robot_model::JointModelGroup*> &eefs = planning_scene->getRobotModel()->getEndEffectors(); for (std::size_t i = 0 ; i < eefs.size() ; ++i) if (eefs[i]->hasLinkModel(link->getName())) { if (eef) { ROS_ERROR_STREAM_NAMED("manipulation", "There are multiple end-effectors that include the link '" << link->getName() << "' which is where the body '" << attached_object_name << "' is attached. It is unclear which end-effector to use."); error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME; return false; } eef = eefs[i]; } } // if the group is also unknown, but we just found out the eef if (!jmg && eef) { const std::string &eef_parent = eef->getEndEffectorParentGroup().first; if (eef_parent.empty()) { ROS_ERROR_STREAM_NAMED("manipulation", "No parent group to plan in was identified based on end-effector '" << goal.group_name << "'. Please define a parent group in the SRDF."); error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME; return false; } else jmg = planning_scene->getRobotModel()->getJointModelGroup(eef_parent); } } if (!jmg || !eef) { error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME; return false; } // try to infer attached body name if possible int loop_count = 0; while (attached_object_name.empty() && loop_count < 2) { // in the first try, look for objects attached to the eef, if the eef is known; // otherwise, look for attached bodies in the planning group itself std::vector<const robot_state::AttachedBody*> attached_bodies; planning_scene->getCurrentState().getAttachedBodies(attached_bodies, loop_count == 0 ? eef : jmg); loop_count++; if (attached_bodies.size() > 1) { ROS_ERROR_NAMED("manipulation", "Multiple attached bodies for group '%s' but no explicit attached object to place was specified", goal.group_name.c_str()); error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME; return false; } else attached_object_name = attached_bodies[0]->getName(); } const robot_state::AttachedBody *attached_body = planning_scene->getCurrentState().getAttachedBody(attached_object_name); if (!attached_body) { ROS_ERROR_NAMED("manipulation", "There is no object to detach for place action"); error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME; return false; } ros::WallTime start_time = ros::WallTime::now(); // construct common data for possible manipulation plans ManipulationPlanSharedDataPtr plan_data(new ManipulationPlanSharedData()); ManipulationPlanSharedDataConstPtr const_plan_data = plan_data; plan_data->planning_group_ = jmg; plan_data->end_effector_group_ = eef; plan_data->ik_link_ = planning_scene->getRobotModel()->getLinkModel(eef->getEndEffectorParentGroup().second); plan_data->timeout_ = endtime; plan_data->path_constraints_ = goal.path_constraints; plan_data->planner_id_ = goal.planner_id; plan_data->minimize_object_distance_ = false; plan_data->max_goal_sampling_attempts_ = std::max(2u, jmg->getDefaultIKAttempts()); moveit_msgs::AttachedCollisionObject &detach_object_msg = plan_data->diff_attached_object_; // construct the attached object message that will change the world to what it would become after a placement detach_object_msg.link_name = attached_body->getAttachedLinkName(); detach_object_msg.object.id = attached_object_name; detach_object_msg.object.operation = moveit_msgs::CollisionObject::REMOVE; collision_detection::AllowedCollisionMatrixPtr approach_place_acm(new collision_detection::AllowedCollisionMatrix(planning_scene->getAllowedCollisionMatrix())); // we are allowed to touch certain other objects with the gripper approach_place_acm->setEntry(eef->getLinkModelNames(), goal.allowed_touch_objects, true); // we are allowed to touch the target object slightly while retreating the end effector std::vector<std::string> touch_links(attached_body->getTouchLinks().begin(), attached_body->getTouchLinks().end()); approach_place_acm->setEntry(attached_object_name, touch_links, true); if (!goal.support_surface_name.empty()) { // we are allowed to have contact between the target object and the support surface before the place approach_place_acm->setEntry(goal.support_surface_name, attached_object_name, true); // optionally, it may be allowed to touch the support surface with the gripper if (goal.allow_gripper_support_collision) approach_place_acm->setEntry(goal.support_surface_name, eef->getLinkModelNames(), true); } // configure the manipulation pipeline pipeline_.reset(); ManipulationStagePtr stage1(new ReachableAndValidPoseFilter(planning_scene, approach_place_acm, pick_place_->getConstraintsSamplerManager())); ManipulationStagePtr stage2(new ApproachAndTranslateStage(planning_scene, approach_place_acm)); ManipulationStagePtr stage3(new PlanStage(planning_scene, pick_place_->getPlanningPipeline())); pipeline_.addStage(stage1).addStage(stage2).addStage(stage3); initialize(); pipeline_.start(); // add possible place locations for (std::size_t i = 0 ; i < goal.place_locations.size() ; ++i) { ManipulationPlanPtr p(new ManipulationPlan(const_plan_data)); const moveit_msgs::PlaceLocation &pl = goal.place_locations[i]; if (goal.place_eef) p->goal_pose_ = pl.place_pose; else // The goals are specified for the attached body // but we want to transform them into goals for the end-effector instead if (!transformToEndEffectorGoal(pl.place_pose, attached_body, p->goal_pose_)) { p->goal_pose_ = pl.place_pose; ROS_ERROR_NAMED("manipulation", "Unable to transform the desired pose of the object to the pose of the end-effector"); } p->approach_ = pl.pre_place_approach; p->retreat_ = pl.post_place_retreat; p->retreat_posture_ = pl.post_place_posture; p->id_ = i; if (p->retreat_posture_.joint_names.empty()) p->retreat_posture_ = attached_body->getDetachPosture(); pipeline_.push(p); } ROS_INFO_NAMED("manipulation", "Added %d place locations", (int) goal.place_locations.size()); // wait till we're done waitForPipeline(endtime); pipeline_.stop(); last_plan_time_ = (ros::WallTime::now() - start_time).toSec(); if (!getSuccessfulManipulationPlans().empty()) error_code_.val = moveit_msgs::MoveItErrorCodes::SUCCESS; else { if (last_plan_time_ > timeout) error_code_.val = moveit_msgs::MoveItErrorCodes::TIMED_OUT; else { error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED; if (goal.place_locations.size() > 0) { ROS_WARN_NAMED("manipulation", "All supplied place locations failed. Retrying last location in verbose mode."); // everything failed. we now start the pipeline again in verbose mode for one grasp initialize(); pipeline_.setVerbose(true); pipeline_.start(); pipeline_.reprocessLastFailure(); waitForPipeline(ros::WallTime::now() + ros::WallDuration(1.0)); pipeline_.stop(); pipeline_.setVerbose(false); } } } ROS_INFO_NAMED("manipulation", "Place planning completed after %lf seconds", last_plan_time_); return error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS; }
// // 함수: WndProc(HWND, UINT, WPARAM, LPARAM) // // 목적: 주 창의 메시지를 처리합니다. // // WM_COMMAND - 응용 프로그램 메뉴를 처리합니다. // WM_PAINT - 주 창을 그립니다. // WM_DESTROY - 종료 메시지를 게시하고 반환합니다. // // LRESULT CALLBACK WndProc(HWND hWnd, UINT message, WPARAM wParam, LPARAM lParam) { static int ac=0,j_flag=0,j_not=0; static float j_count1=0; static Player player[2]; //player[0]는 현재위치 player[1]은 전위치 PAINTSTRUCT ps; static HANDLE hTimer; static char map[HEIGHT][WIDTH]={}; static int stage[1]={MENU}, trapKey[10]; static TRAP trap[10]; static MapBox mapbox[HEIGHT][WIDTH] = {0}; int save[3] = {0}; //save[0] = ac, save[1] = j_count1, save[2] = j_not HDC hdc, hBitDC, mapDC, backDC, charDC, BulletDC; HBITMAP hBit, mapbit, Bulletbit; HBITMAP backbitmap; //기존에 dc에 저장된 BitMap을 다른곳에 보관 해주면서 새 BitMap을 dc에 저장한다. RECT rt={0,0,900,700}; static int player_bullet_direction; static Bullet player_bullet[P_BULLET_MAX]; static int player_bullet_count[1] = {0}; static int enemy_count[1] = {0}; static int reset=0; static int menu_select = 0; char B[7] = "bullet"; static int menu_arrow[1] = {1}; //1 = 처음하기, 2 = 이어하기, 3 = 끝내기 static int die_check = 0; SetTimer(hWnd, MOVE_TIMER_ID, 10, NULL); SetTimer(hWnd, BULLET_TIMER_ID, 200, NULL); //총알 타이머 save[0] = ac; save[1] = j_count1; save[2] = j_not; switch(stage[0]) { case MENU: menu(menu_arrow, player, reset, stage, menu_select); break; if(die_check == 1){ die_check = 0; menu_arrow[0] = 1; menu_select = 0; stage[0] = MENU; } case TUTORIAL1: tuto(player, save, map,trap,stage, mapbox,&reset); break; case TUTORIAL2: tuto2(player,save,map,trap, stage, mapbox, &reset); break; case STAGE1_1: stage1(player,save,map,trap, stage, mapbox, &reset); break; } ac = save[0]; j_count1 = save[1]; j_not = save[2]; switch (message) { case WM_CREATE : hTimer=(HANDLE)SetTimer(hWnd,AC_TIMER_ID,50,NULL); return 0; case WM_KEYDOWN: switch(wParam) { if(stage[0] == MENU){ case VK_LEFT: if(menu_arrow[0] > 1){ menu_arrow[0]--; } return FALSE; case VK_RIGHT: if(menu_arrow[0] < 3){ menu_arrow[0]++; } return FALSE; case VK_RETURN: menu_select = menu_arrow[0]; return FALSE; } case 'z': //위누르면 점프 2단까지 허용 case 'Z': if(player[0].life==1 && j_count1<2 && j_not<1.1) { SetTimer(hWnd,JUMP_TIMER_ID,50,NULL); j_not++; ac=0; j_flag=0; j_count1+=1; // InvalidateRect(hWnd,NULL,false); return false; } } if(stage[0] != MENU){ case WM_TIMER: switch(wParam) { case AC_TIMER_ID: //중력처리 if(ac<17) ac+=2; player[1].top = player[0].top; player[0].top+=ac; player[1].bottom = player[0].bottom; player[0].bottom+=ac; InvalidateRect(hWnd,NULL,FALSE); return false; case JUMP_TIMER_ID: player[1].top = player[0].top; player[0].top-=(20-j_flag*3); //점프 올라갈수록 느려짐 player[1].bottom = player[0].bottom; player[0].bottom-=(20-j_flag*3); j_flag++; if(j_flag==4) KillTimer(hWnd,2); return false; case MOVE_TIMER_ID: if(player[0].life!=0) { if(GetAsyncKeyState(VK_LEFT) < 0) //왼쪽ㄱㄱ { if(LR_Crash(map, player, mapbox, LEFT)) // if 문 추가로 입력 { player_bullet_direction = WW; player[1].left = player[0].left; player[0].left -= 3; player[1].right = player[0].right; player[0].right -= 3; } } if(GetAsyncKeyState(VK_RIGHT) < 0) //오른쪽 ㄱㄱ { if(LR_Crash(map, player, mapbox, RIGHT))//if 문 추가로 입력 (이동 불가하게 만듬) { player_bullet_direction = EE; player[1].left = player[0].left; player[0].left += 3; player[1].right = player[0].right; player[0].right += 3; } } } return false; case BULLET_TIMER_ID: if(GetAsyncKeyState(0x58) < 0) // X = Bullet { player_bullet[player_bullet_count[0]].direction = player_bullet_direction; if(player_bullet[player_bullet_count[0]].direction == WW){ player_bullet[player_bullet_count[0]].right = player[0].left; player_bullet[player_bullet_count[0]].left = player_bullet[player_bullet_count[0]].right - P_BULLETSIZE; } else if(player_bullet[player_bullet_count[0]].direction == EE){ player_bullet[player_bullet_count[0]].left = player[0].right; player_bullet[player_bullet_count[0]].right = player_bullet[player_bullet_count[0]].left + P_BULLETSIZE; } player_bullet[player_bullet_count[0]].top = player[0].top + 11; player_bullet[player_bullet_count[0]].bottom = player_bullet[player_bullet_count[0]].top + P_BULLETSIZE; player_bullet_count[0]++; } for(int i=0; i<player_bullet_count[0]; i++){ if(player_bullet[i].direction == WW){ player_bullet[i].right -= 10; player_bullet[i].left = player_bullet[i].right - P_BULLETSIZE; } else if(player_bullet[i].direction == EE){ player_bullet[i].left += 10; player_bullet[i].right = player_bullet[i].left + P_BULLETSIZE; } } return false; } } case WM_PAINT: hdc = BeginPaint(hWnd, &ps); backDC=CreateCompatibleDC(hdc); backbitmap = CreateCompatibleBitmap(hdc,rt.right,rt.bottom); hBitDC = CreateCompatibleDC(hdc); mapDC = CreateCompatibleDC(hdc); charDC = CreateCompatibleDC(hdc); BulletDC = CreateCompatibleDC(hdc); if(player[0].life==1) mapbit=LoadBitmap(hInst,MAKEINTRESOURCE(IDB_BITMAP1)); else{ mapbit=LoadBitmap(hInst,MAKEINTRESOURCE(IDB_BITMAP3)); die_check = 1; } if(stage[0]/10==TUTO) hBit = LoadBitmap(hInst, MAKEINTRESOURCE(IDB_BITMAP2)); else if(stage[0]/10==STAGE1) hBit = LoadBitmap(hInst, MAKEINTRESOURCE(IDB_BITMAP7)); Bulletbit = LoadBitmap(hInst, MAKEINTRESOURCE(IDB_BITMAP15)); SelectObject(backDC, backbitmap); SelectObject(mapDC,hBit); SelectObject(charDC,mapbit); SelectObject(BulletDC, Bulletbit); SelectObject(backDC,hBit); FillRect(backDC, &rt, (HBRUSH)GetStockObject(WHITE_BRUSH)); switch(stage[0]/10) { case MENU: DrawMenu(hdc, backDC, hInst, menu_arrow); break; case TUTO: DrawBlockTuto(hdc,backDC,mapDC,trap,stage,hInst,map); break; case STAGE1: DrawBlockStage1(hdc,backDC,mapDC,trap,stage,hInst,map); break; } for(int i=0; i<player_bullet_count[0]; i++){ //BitBlt(backDC, player_bullet[i].left, player_bullet[i].top, P_BULLETSIZE, P_BULLETSIZE, BulletDC , 0, 0, SRCCOPY); TextOut(backDC, player_bullet[i].left, player_bullet[i].top, B, strlen(B)); } TransparentBlt(backDC, player[0].left-BOXSIZE, player[0].top-BOXSIZE, PLAYERSIZE, PLAYERSIZE, charDC, 0, 0,PLAYERSIZE,PLAYERSIZE, RGB(255,255,255)); BitBlt(hdc,0,0,rt.right,rt.bottom,backDC,0,0,SRCCOPY); // TODO: 여기에 그리기 코드를 추가합니다. // SelectObject(hBitDC, hOldBit); // SelectObject(mapDC, holdmap); DeleteObject(backbitmap); DeleteObject(hBit); DeleteObject(Bulletbit); DeleteObject(mapbit); DeleteDC(hBitDC); DeleteDC(backDC); DeleteDC(BulletDC); DeleteDC(hdc); DeleteDC(mapDC); DeleteDC(charDC); EndPaint(hWnd, &ps); return FALSE; case WM_DESTROY: KillTimer(hWnd, AC_TIMER_ID); KillTimer(hWnd, MOVE_TIMER_ID); KillTimer(hWnd, BULLET_TIMER_ID); PostQuitMessage(0); break; default: return DefWindowProc(hWnd, message, wParam, lParam); } return 0; }
//alias int exponentiation(int a, int e, int _n) { return stage1(a, e, _n); }
int bootup_main(int argc, char **argv) { FILE *fp; int t=0; char cmd[1024], buf[1024], name[1024]; time_t curtime; struct tm *loctime; signal(SIGINT,SIG_IGN); putenv("PATH=/bin"); putenv("TERM=linux"); umask(0770); chdir("/"); print_file("/etc/banner"); xsystem("mount -t proc -o ro virtual /proc"); xsystem("mount -t sysfs -o ro virtual /sys"); stage1(); stage2(); // STAGE 3 chdir("/"); read_cmdline(); xsystem("mdev -s"); xsystem("mount -t devpts -o \"rw,gid=0,mode=620\""); mk_dev("/dev/ppp",108,0); if(LCD_PROG==1 && file_exists("/tmp/tools/lcd/lcd.bz2")) { mk_dev("/dev/parport0",99,0); mk_dev("/dev/lp0",6,0); xsystem("tar -C / -jxf /tmp/tools/lcd/lcd.bz2"); } else { LCD_PROG=0; } rename("/dev/random","/dev/random-block"); symlink("/dev/urandom","/dev/random"); xmkdir("/strg"); if(LCD_DEV[0]!='\0') { snprintf(buf,sizeof(buf),"/dev/%s",LCD_DEV); if(file_exists(buf)) { save_to_file("/var/sys/lcd_dev",buf); if(LCD_PROG==1) save_to_file("/var/sys/lcd_proc","%d",LCD_PROG); symlink(buf,"/dev/lcd"); lcd_msg(LCD_PROG,"SYSTEM LOADING..","-> STORAGE ON"); } } memset(buf,0x0,sizeof(buf)); snprintf(buf,sizeof(buf),"mount -t %s -o \"rw,noatime\" %s /strg",STRG_FS,STRG_DEV); if(xsystem("mount -t %s -o \"rw,noatime\" %s /strg",STRG_FS,STRG_DEV)==0) { if(file_exists("/strg/.mount_strg")) { unlink("/strg/.mount_strg"); if(xsystem("umount /strg")==0) { fprintf_stdout("**** MYBOX SYSTEM APPEARS TO HAVE SHUT DOWN UNCLEANLY ****\n"); lcd_msg(LCD_PROG,"SYSTEM LOADING..","-> FIX STORAGE"); xsystem("e2fsck -y %s",STRG_DEV); t=1; } } if(t==0) xsystem("umount /strg"); } else { fprintf_stdout("**** MOUNTING STORAGE DISK FAILED! ****\n"); lcd_msg(LCD_PROG,"SYSTEM LOADING..","STORAGE FAILED !"); xtouch("/strg/.nostrg"); xtouch("/var/sys/nolog"); } if(!file_exists("/strg/.nostrg")) { if(xsystem("mount -t %s -o \"rw,noatime\" %s /strg",STRG_FS,STRG_DEV)==0) { memset(buf,0x0,sizeof(buf)); snprintf(buf,sizeof(buf),"%s:%s\n",STRG_DEV,STRG_FS); save_to_file("/strg/.mount_strg",buf); save_to_file("/var/sys/.mount_strg",buf); save_to_file("/var/sys/.mount_strg","%s:%s\n",BOOT_DEV,BOOT_FS); } } if(xsystem("swapon %s",SWAP_DEV)==0) { memset(buf,0x0,sizeof(buf)); snprintf(buf,sizeof(buf),"%s:swap\n",SWAP_DEV); save_to_file("/var/sys/.mount_swap",buf); } xsystem("chmod 700 *"); if((fp=fopen("/etc/inittab","w"))!=NULL) { fprintf(fp,"::sysinit:/bin/initrc\n"); fprintf(fp,"tty1::respawn:/bin/getty -h -n -L tty1 9600 linux\n"); fprintf(fp,"ttyS0::respawn:/bin/getty -h -n -L ttyS0 9600 vt100\n"); fprintf(fp,"null::respawn:/bin/chkprog\n"); fprintf(fp,"::restart:/bin/init\n"); fprintf(fp,"::ctrlaltdel:/bin/bootdown\n"); fprintf(fp,"::ctrlaltdel:/bin/reset\n"); fprintf(fp,"::ctrlaltdel:/bin/reboot\n"); fprintf(fp,"::shutdown:/bin/bootdown\n"); fclose(fp); } if((fp=fopen("/etc/profile","a"))!=NULL) { fprintf(fp,"xexit() {\n"); fprintf(fp," if [ \"$PPID\" = \"1\" ]; then\n"); fprintf(fp," local logname=\"/strg/mybox/logs/auth-$(date \"+%s\").log\"\n","%Y%m%d"); fprintf(fp," local msg=\"[$(date \"+%s\")] TYPE=console USER=console IP=$(basename $(tty)) MSG=Session logout.\"\n","%d/%m/%Y %H:%M:%S"); fprintf(fp," echo \"$msg\" >> $logname\n"); fprintf(fp," [ ! -z \"$ME\" -a -f \"$ME\" ] && rm -f /tmp/console.session/console_*\n"); fprintf(fp," fi\n"); fprintf(fp," exit\n"); fprintf(fp,"}\n"); fprintf(fp,"alias exit='xexit'\n"); fprintf(fp,"export HISTFILE=/.consolehistory\n"); fprintf(fp,"lcdd_msg() {\n"); fprintf(fp," if [ -f \"/bin/lcdd\" -a -c \"/dev/lcd\" ]; then\n"); fprintf(fp," if [ -f \"/var/sys/lcd_proc\" ]; then\n"); fprintf(fp," echo \"$2\" > /var/sys/lcd_msg\n"); fprintf(fp," else\n"); fprintf(fp," /bin/lcdd \"$1\" \"$2\"\n"); fprintf(fp," fi\n"); fprintf(fp," fi\n"); fprintf(fp,"}\n"); fprintf(fp,"if [ -z $DO_SINGLE ]; then\n"); fprintf(fp," if [ -f \"/bin/iosh\" ]; then\n"); fprintf(fp," XTTY=\"SSL\";\n"); fprintf(fp," if [ -f \"/var/sys/init_start\" ]; then\n"); fprintf(fp," trap : 1 2 3 15\n"); fprintf(fp," echo \"System loading in progress..please wait or login back in a minute\"\n"); fprintf(fp," while [ -f \"/var/sys/init_start\" ]; do sleep 1;done\n"); fprintf(fp," trap 1 2 3 15\n"); fprintf(fp," fi\n"); fprintf(fp," if [ \"$PPID\" = \"1\" ]; then\n"); fprintf(fp," export ME=\"/tmp/console.session/console_${PPID}_$(basename $(tty))_$(date \"+%s\")\";\n","%d:%m:%Y_%H:%M:%S"); fprintf(fp," touch $ME\n"); fprintf(fp," XTTY=\"console\";\n"); fprintf(fp," fi\n"); fprintf(fp," /bin/iosh $XTTY\n"); fprintf(fp," if [ $? != 5 ]; then\n"); fprintf(fp," clear;reset\n"); fprintf(fp," exit\n"); fprintf(fp," fi\n"); fprintf(fp," else\n"); fprintf(fp," echo \"** FAILED TO RUN IO SHELL **\"\n"); fprintf(fp," read io\n"); fprintf(fp," exit\n"); fprintf(fp," fi\n"); fprintf(fp,"else \n"); fprintf(fp," echo \"** MAINTENANCE MODE **\"\n"); fprintf(fp," lcdd_msg \"SYSTEM LOADING.." "-> MAINTENANCE\"\n"); fprintf(fp," read io\n"); fprintf(fp,"fi\n"); fclose(fp); } unlink("/strg/mybox/debug.log"); curtime=time(NULL); loctime=localtime(&curtime); memset(name,0x0,sizeof(name)); strftime(name, sizeof(name),"system-%Y%m%d.log",loctime); memset(buf,0x0,sizeof(buf)); snprintf(buf,sizeof(buf),"%s/%s",LOGPATH,name); memset(cmd,0x0,sizeof(cmd)); strftime(cmd, sizeof(cmd), "[%d/%m/%Y %T] TYPE=INFO MSG=****** SYSTEM LOADING ******\n",loctime); append_to_file(buf,cmd); if(file_exists("/bin/getkey")) { if(system("getkey -c 3 -m \"-> Starting Init: %d\" R")==0) { fprintf_stdout("\r*** BYPASS CONSOLE LOGIN ***\n"); lcd_msg(LCD_PROG,"SYSTEM LOADING..","BYPASS CONSOLE !"); xtouch("/etc/noconsole"); } else { fprintf_stdout("\r* Starting Init. Done.\n"); } } memset(buf,0x0,sizeof(buf)); snprintf(buf,sizeof(buf),"%s\n","/bin/mdev"); save_to_file("/proc/sys/kernel/hotplug",buf); if(NUM_NET!=0) save_to_file("/var/sys/numnet_veto","%d",NUM_NET); do_chroot(); signal(SIGINT,SIG_DFL); lcd_msg(LCD_PROG,"SYSTEM LOADING..","ERROR !"); fprintf_stdout("You are not supposed to be here, something went wrong!\n"); fprintf_stdout("Press Ctrl+Alt+Del or switch off/on for reboot.\n"); while(1); exit(0); }
int main() { // initialize IRQ (interrupts) // this must come before everything else IRQ_INIT(); // Initialize global pointers GameStateManager gameStateMan; OamManager oamMan; AudioManager audioMan; PlayState playState(&gameStateMan); TitleScreenState titleState(&gameStateMan); PauseState pauseState(&gameStateMan); GameOverState gameOverState(&gameStateMan); StoreState storeState(&gameStateMan); StageEndState stageEndState(&gameStateMan); g_gameStateMan = &gameStateMan; g_oamMan = &oamMan; g_playState = &playState; g_titleState = &titleState; g_pauseState = &pauseState; g_gameOverState = &gameOverState; g_storeState = &storeState; g_stageEndState = &stageEndState; g_audioMan = &audioMan; // create stage events StageEvent endEvent; StageEvent event1; StageEvent event2; StageEvent event3; StageEvent event4; StageEvent event5; StageEvent event6; StageEvent firePowerPowerUpEvent; StageEvent invinciblePowerUpEvent; StageEvent bombPowerUpEvent; g_endEvent = &endEvent; g_event1 = &event1; g_event2 = &event2; g_event3 = &event3; g_event4 = &event4; g_event5 = &event5; g_event6 = &event6; g_firePowerPowerUpEvent = &firePowerPowerUpEvent; g_invinciblePowerUpEvent = &invinciblePowerUpEvent; g_bombPowerUpEvent = &bombPowerUpEvent; initializeEvents(); StageEvent * stage1Events[24]; int stage1Timing[24]; int stage1yOffset[24]; fillEventsStage1(stage1Events, stage1Timing, stage1yOffset); Stage stage1(&playState, stage1Events, stage1Timing, stage1yOffset, 24); g_stage1 = &stage1; StageEvent * stage2Events[20]; int stage2Timing[20]; int stage2yOffset[20]; fillEventsStage2(stage2Events, stage2Timing, stage2yOffset); Stage stage2(&playState, stage2Events, stage2Timing, stage2yOffset, 20); g_stage2 = &stage2; StageEvent * stage3Events[20]; int stage3Timing[20]; int stage3yOffset[20]; fillEventsStage3(stage3Events, stage3Timing, stage3yOffset); Stage stage3(&playState, stage3Events, stage3Timing, stage3yOffset, 20); g_stage3 = &stage3; videoInit(); g_gameStateMan->pushState(g_titleState); #ifdef DEBUG // timers used for debug display REG_TM1D = 0x10000 - 2808; // overflow into timer 2 every 2808 cycles, approx. 1% of a screen refresh REG_TM2D = 0; REG_TM2CNT = TM_CASCADE | TM_ENABLE; REG_TM1CNT = TM_FREQ_1 | TM_ENABLE; int oldPercent, diffPercent, oldFrac, diffFrac; char buf[15]; #endif // DEBUG while(true) { // wait until next VBlank // prefer this over vid_vsync() - it's // better for power consumption VBlankIntrWait(); #ifdef DEBUG // grab current percentage oldPercent = REG_TM2D; oldFrac = REG_TM1D; #endif // DEBUG // update shadow OAM to real OAM g_oamMan->update(); // mix the next frame's audio g_audioMan->sndMix(); // poll keys - do not do this in other places key_poll(); // update the game state g_gameStateMan->update(); #ifdef DEBUG // grab current percentage, and write it out diffPercent = REG_TM2D - oldPercent; diffFrac = REG_TM1D - oldFrac; // round the percent based on the fractional amount if (diffFrac > 1404) { diffPercent++; } else if (diffFrac < -1404) { diffPercent--; } gba_itoa(diffPercent, buf); // clear out characters from the last write write(" ", Vec2(0, 0)); write(buf, Vec2(0, 0)); // reset timer 2 to 0 REG_TM2CNT = 0; // disable timer REG_TM2D = 0; // set new value to 0 REG_TM2CNT = TM_CASCADE | TM_ENABLE; // reenable timer #endif // DEBUG } }
int main (int argc, char *argv[]) { double *A,*A2,*L,*U, temp2; int i,j,k; int temp=0; int offset = 0; double t1,t2,t3,t4; int N = 100; int Block = 1; int M=1; //number of blocks per dimension if( argc > 1 ) N = atoi(argv[1]); if( argc > 2 ) M = atoi(argv[2]); A = (double *)malloc (N*N*sizeof(double)); A2 = (double *)malloc (N*N*sizeof(double)); L = (double *)malloc (N*N*sizeof(double)); U = (double *)malloc (N*N*sizeof(double)); if( A==NULL || A2==NULL || L==NULL || U==NULL ) { printf("Can't allocate memory\n"); exit(1); } int *sizedim; int *start; int R; //Remain sizedim = (int*)malloc(M*sizeof(int)); start = (int*)malloc(M*sizeof(int)); R = N; t1 = GetTickCount(); #pragma omp parallel { #pragma omp master { while (N-offset>M){ for (i=0;i<M;i++){ if (i<R%M){ sizedim[i]=R/M+1; start[i]=(R/M+1)*i; } else { sizedim[i]=R/M; start[i]=(R/M+1)*(R%M)+(R/M)*(i-R%M); } } stage1(A, offset, sizedim, start, N, M); t3 = GetTickCount(); stage2(A, offset, sizedim, start, N, M); stage3(A, offset, sizedim, start, N, M); t4 = GetTickCount(); total += (t4-t3); offset+=sizedim[0]; R=R-sizedim[0]; } } } ProcessDiagonalBlock(&A[offset*N+offset], N-offset, N); t2 = GetTickCount(); printf("Time for LU-decomposition in secs: %f \n", (t2-t1)/1000000); printf("Time for the task region in secs: %f \n", (total)/1000000); printf("Time for inside tasks in secs: %f \n", (task_total)/1000000); printf("Time spent in taskwait in secs: %f \n", (wait_total)/1000000); #ifdef CHECK for (i=0;i<N;i++) for (j=0;j<N;j++) if (i>j) L[i*N+j] = A[i*N+j]; else U[i*N+j] = A[i*N+j]; for (i=0;i<N;i++) L[i*N+i] = 1; for (i=0;i<N;i++) for (j=0;j<N;j++){ temp2=0; for (k=0;k<N;k++) temp2+=L[i*N+k]*U[k*N+j]; if ((A2[i*N+j]-temp2)/A2[i*N+j] >0.1 || (A2[i*N+j]-temp2)/A2[i*N+j] <-0.1) temp++; } printf("Errors = %d \n", temp); #endif return 0; }
bool PickPlan::plan(const planning_scene::PlanningSceneConstPtr &planning_scene, const moveit_msgs::PickupGoal &goal) { double timeout = goal.allowed_planning_time; ros::WallTime endtime = ros::WallTime::now() + ros::WallDuration(timeout); std::string planning_group = goal.group_name; std::string end_effector = goal.end_effector; if (end_effector.empty() && !planning_group.empty()) { const robot_model::JointModelGroup *jmg = planning_scene->getRobotModel()->getJointModelGroup(planning_group); if (!jmg) { error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME; return false; } const std::vector<std::string> &eefs = jmg->getAttachedEndEffectorNames(); if (!eefs.empty()) { end_effector = eefs.front(); if (eefs.size() > 1) ROS_WARN_STREAM_NAMED("manipulation", "Choice of end-effector for group '" << planning_group << "' is ambiguous. Assuming '" << end_effector << "'"); } } else if (!end_effector.empty() && planning_group.empty()) { const robot_model::JointModelGroup *jmg = planning_scene->getRobotModel()->getEndEffector(end_effector); if (!jmg) { error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME; return false; } planning_group = jmg->getEndEffectorParentGroup().first; if (planning_group.empty()) { ROS_ERROR_STREAM_NAMED("manipulation", "No parent group to plan in was identified based on end-effector '" << end_effector << "'. Please define a parent group in the SRDF."); error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME; return false; } else ROS_INFO_STREAM_NAMED("manipulation", "Assuming the planning group for end effector '" << end_effector << "' is '" << planning_group << "'"); } const robot_model::JointModelGroup *eef = end_effector.empty() ? NULL : planning_scene->getRobotModel()->getEndEffector(end_effector); if (!eef) { ROS_ERROR_NAMED("manipulation", "No end-effector specified for pick action"); error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME; return false; } const std::string &ik_link = eef->getEndEffectorParentGroup().second; ros::WallTime start_time = ros::WallTime::now(); // construct common data for possible manipulation plans ManipulationPlanSharedDataPtr plan_data(new ManipulationPlanSharedData()); ManipulationPlanSharedDataConstPtr const_plan_data = plan_data; plan_data->planning_group_ = planning_scene->getRobotModel()->getJointModelGroup(planning_group); plan_data->end_effector_group_ = eef; plan_data->ik_link_ = planning_scene->getRobotModel()->getLinkModel(ik_link); plan_data->timeout_ = endtime; plan_data->path_constraints_ = goal.path_constraints; plan_data->planner_id_ = goal.planner_id; plan_data->minimize_object_distance_ = goal.minimize_object_distance; plan_data->max_goal_sampling_attempts_ = std::max(2u, plan_data->planning_group_->getDefaultIKAttempts()); moveit_msgs::AttachedCollisionObject &attach_object_msg = plan_data->diff_attached_object_; // construct the attached object message that will change the world to what it would become after a pick attach_object_msg.link_name = ik_link; attach_object_msg.object.id = goal.target_name; attach_object_msg.object.operation = moveit_msgs::CollisionObject::ADD; attach_object_msg.touch_links = goal.attached_object_touch_links.empty() ? eef->getLinkModelNames() : goal.attached_object_touch_links; collision_detection::AllowedCollisionMatrixPtr approach_grasp_acm(new collision_detection::AllowedCollisionMatrix(planning_scene->getAllowedCollisionMatrix())); // we are allowed to touch the target object approach_grasp_acm->setEntry(goal.target_name, attach_object_msg.touch_links, true); // we are allowed to touch certain other objects with the gripper approach_grasp_acm->setEntry(eef->getLinkModelNames(), goal.allowed_touch_objects, true); if (!goal.support_surface_name.empty()) { // we are allowed to have contact between the target object and the support surface before the grasp approach_grasp_acm->setEntry(goal.support_surface_name, goal.target_name, true); // optionally, it may be allowed to touch the support surface with the gripper if (goal.allow_gripper_support_collision) approach_grasp_acm->setEntry(goal.support_surface_name, eef->getLinkModelNames(), true); } // configure the manipulation pipeline pipeline_.reset(); ManipulationStagePtr stage1(new ReachableAndValidPoseFilter(planning_scene, approach_grasp_acm, pick_place_->getConstraintsSamplerManager())); ManipulationStagePtr stage2(new ApproachAndTranslateStage(planning_scene, approach_grasp_acm)); ManipulationStagePtr stage3(new PlanStage(planning_scene, pick_place_->getPlanningPipeline())); pipeline_.addStage(stage1).addStage(stage2).addStage(stage3); initialize(); pipeline_.start(); // order the grasps by quality std::vector<std::size_t> grasp_order(goal.possible_grasps.size()); for (std::size_t i = 0 ; i < goal.possible_grasps.size() ; ++i) grasp_order[i] = i; OrderGraspQuality oq(goal.possible_grasps); std::sort(grasp_order.begin(), grasp_order.end(), oq); // feed the available grasps to the stages we set up for (std::size_t i = 0 ; i < goal.possible_grasps.size() ; ++i) { ManipulationPlanPtr p(new ManipulationPlan(const_plan_data)); const moveit_msgs::Grasp &g = goal.possible_grasps[grasp_order[i]]; p->approach_ = g.pre_grasp_approach; p->retreat_ = g.post_grasp_retreat; p->goal_pose_ = g.grasp_pose; p->id_ = grasp_order[i]; // if no frame of reference was specified, assume the transform to be in the reference frame of the object if (p->goal_pose_.header.frame_id.empty()) p->goal_pose_.header.frame_id = goal.target_name; p->approach_posture_ = g.pre_grasp_posture; p->retreat_posture_ = g.grasp_posture; pipeline_.push(p); } // wait till we're done waitForPipeline(endtime); pipeline_.stop(); last_plan_time_ = (ros::WallTime::now() - start_time).toSec(); if (!getSuccessfulManipulationPlans().empty()) error_code_.val = moveit_msgs::MoveItErrorCodes::SUCCESS; else { if (last_plan_time_ > timeout) error_code_.val = moveit_msgs::MoveItErrorCodes::TIMED_OUT; else { error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED; if (goal.possible_grasps.size() > 0) { ROS_WARN_NAMED("manipulation", "All supplied grasps failed. Retrying last grasp in verbose mode."); // everything failed. we now start the pipeline again in verbose mode for one grasp initialize(); pipeline_.setVerbose(true); pipeline_.start(); pipeline_.reprocessLastFailure(); waitForPipeline(ros::WallTime::now() + ros::WallDuration(1.0)); pipeline_.stop(); pipeline_.setVerbose(false); } } } ROS_INFO_NAMED("manipulation", "Pickup planning completed after %lf seconds", last_plan_time_); return error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS; }