base/ros.cc

   1 /*
   2 This file is part of I am car.
   3
   4 I am car is free software: you can redistribute it and/or modify
   5 it under the terms of the GNU General Public License as published by
   6 the Free Software Foundation, either version 3 of the License, or
   7 (at your option) any later version.
   8
   9 I am car is distributed in the hope that it will be useful,
  10 but WITHOUT ANY WARRANTY; without even the implied warranty of
  11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12 GNU General Public License for more details.
  13
  14 You should have received a copy of the GNU General Public License
  15 along with I am car. If not, see <http://www.gnu.org/licenses/>.
  16 */
  17
  18 #include "ros/ros.h"
  19 #include "iamcar/Park.h"
  20 #include "obstacle_msgs/Obstacles.h"
  21
  22 #include "compile.h"
  23 #include "obstacle.h"
  24 #include "rrtplanner.h"
  25 #include "slotplanner.h"
  26
  27 #define USE_TMAX
  28 //#define USE_SLOTPLANNER
  29
  30 bool park(
  31         iamcar::Park::Request &req,
  32         iamcar::Park::Response &res
  33 )
  34 {
  35         PLANNER p(
  36                 new RRTNode(), // TODO get req.init, req.goal
  37                 new RRTNode()
  38         );
  39         p.ocost(p.root());
  40         p.ocost(p.goal());
  41
  42         ParallelSlot ps = ParallelSlot();
  43         ps.slot().add_bnode(new RRTNode()); // TODO req.slot
  44         ps.setAll();
  45         p.samplingInfo_ = ps.getSamplingInfo();
  46         if (pi.x == pg.x && pi.y == pi.y && ps.slot().bnodes().size() > 0) {
  47                 p.goal(ps.getFP());
  48                 ps.fipr(ps.getFP());
  49         }
  50         if (ps.cusp().size() > 0) {
  51                 p.goal(ps.cusp().front().front());
  52                 p.slot_cusp(ps.cusp().front());
  53         }
  54         p.tstart();
  55         p.tend();
  56         while (!p.goal_found() && p.elapsed() < TMAX) {
  57                 p.next();
  58                 p.tend();
  59                 if (p.opt_path()) {
  60                         if (ps.cusp().size() > 0)
  61                                 p.tlog(p.findt(p.slot_cusp().back()));
  62                         else
  63                                 p.tlog(p.findt());
  64                 }
  65         }
  66         // TODO res.path
  67         return true;
  68 }
  69
  70 int main(int argc, char **argv)
  71 {
  72         ros::init(argc, argv, "iamcar");
  73         ros::NodeHandle nh;
  74         ros::ServiceServer park_srv = nh.advertiseService("park", park);
  75         ros::spin();
  76         return 0;
  77 }