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); //输出解的总数
}