bool computePlan(moveit_msgs::GetMotionPlan::Request& req, moveit_msgs::GetMotionPlan::Response& res) { ROS_INFO("Received new planning request..."); if (debug_) pub_request_.publish(req.motion_plan_request); planning_interface::MotionPlanResponse response; ompl_interface::ModelBasedPlanningContextPtr context = ompl_interface_.getPlanningContext(psm_.getPlanningScene(), req.motion_plan_request); if (!context) { ROS_ERROR_STREAM_NAMED("computePlan", "No planning context found"); return false; } context->clear(); bool result = context->solve(response); if (debug_) { if (result) displaySolution(res.motion_plan_response); std::stringstream ss; ROS_INFO("%s", ss.str().c_str()); } return result; }
void place2Queens() { //2»Êºó¼ôÖ¦Ëã·¨ int solu[2]; //½â·¨ for ( solu[0] = 0; solu[0] < 2; solu[0]++ ) if ( !collide ( solu, 0 ) ) for ( solu[1] = 0; solu[1] < 2; solu[1]++ ) if ( !collide ( solu, 1 ) ) { nSolu++; displaySolution ( solu, 2 ); } }
void place3Queens() { //3»Êºó¼ôÖ¦Ëã·¨ int solu[3]; //½â·¨ for (solu[0] = 0; solu[0] < 3; solu[0]++) if (!collide(solu, 0)) for (solu[1] = 0; solu[1] < 3; solu[1]++) if (!collide(solu, 1)) for (solu[2] = 0; solu[2] < 3; solu[2]++) if (!collide(solu, 2)) { nSolu++; displaySolution(solu, 3); } }
/****************************************************************************************** * n皇后算法(递归版) * 初始启动:placeQueens(nQueen, 0); ******************************************************************************************/ void placeQueens(int nQueen, int k) { //放置n个皇后中的第k个(假设0至k-1均已就位) static int solu[QUEEN_MAX]; //解法 if (nQueen <= k) { //若所有皇后都已就位,则 nSolu++; displaySolution(solu, nQueen); //输出可行解 } else //否则 for (int i = 0; i < nQueen; i++) { //依次 solu[k] = i; //试着将当前皇后放在(当前行的)各列上 if (!collide(solu, k)) //若没有冲突,则 placeQueens(nQueen, k+1); //进而考虑下一皇后 } //思考:这里的回溯,通过什么机制实现? }
void place5Queens() { //5»Êºó¼ôÖ¦Ëã·¨ int solu[5]; //½â·¨ for (solu[0] = 0; solu[0] < 5; solu[0]++) if (!collide(solu, 0)) for (solu[1] = 0; solu[1] < 5; solu[1]++) if (!collide(solu, 1)) for (solu[2] = 0; solu[2] < 5; solu[2]++) if (!collide(solu, 2)) for (solu[3] = 0; solu[3] < 5; solu[3]++) if (!collide(solu, 3)) for (solu[4] = 0; solu[4] < 5; solu[4]++) if (!collide(solu, 4)) { nSolu++; displaySolution(solu, 5); } }
bool computePlan(moveit_msgs::GetMotionPlan::Request &req, moveit_msgs::GetMotionPlan::Response &res) { ROS_INFO("Received new planning request..."); if (debug_) pub_request_.publish(req.motion_plan_request); planning_interface::MotionPlanResponse response; bool result = ompl_interface_.solve(psm_.getPlanningScene(), req.motion_plan_request, response); res.motion_plan_response.error_code = response.error_code_; if (debug_) { if (result) displaySolution(res.motion_plan_response); std::stringstream ss; ROS_INFO("%s", ss.str().c_str()); } return result; }
void place5Queens_BruteForce() { //5皇后蛮力算法 int solu[5]; //解法 int nCandidate = 0; //候选解总数 for (solu[0] = 0; solu[0] < 5; solu[0]++) for (solu[1] = 0; solu[1] < 5; solu[1]++) for (solu[2] = 0; solu[2] < 5; solu[2]++) for (solu[3] = 0; solu[3] < 5; solu[3]++) for (solu[4] = 0; solu[4] < 5; solu[4]++) { nCandidate++; if (collide(solu, 0)) continue; if (collide(solu, 1)) continue; if (collide(solu, 2)) continue; if (collide(solu, 3)) continue; if (collide(solu, 4)) continue; nSolu++; displaySolution(solu, 5); } printf("%d Queens, %d Solution(s), %d Candidate(s), %dCheck(s)\n\a\a", 5, nSolu, nCandidate, nCheck); //输出解的总数 }