Ejemplo n.º 1
0
  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;
  }
Ejemplo n.º 2
0
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 );
            }
}
Ejemplo n.º 3
0
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);
         }
}
Ejemplo n.º 4
0
/******************************************************************************************
 * 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); //进而考虑下一皇后
      } //思考:这里的回溯,通过什么机制实现?
}
Ejemplo n.º 5
0
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);
               }
}
Ejemplo n.º 6
0
  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;
  }
Ejemplo n.º 7
0
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); //输出解的总数
}