src/rrts_wr.cc

   1 #include "rclcpp/rclcpp.hpp"
   2 #include "nav_msgs/srv/get_plan.hpp"
   3 #include "rrtsp.hh"
   4
   5 void
   6 plan_path(const std::shared_ptr<nav_msgs::srv::GetPlan::Request> req,
   7                 std::shared_ptr<nav_msgs::srv::GetPlan::Response> res)
   8 {
   9         rrts::P39 p;
  10         double sx = req->start.pose.position.x;
  11         double sy = req->start.pose.position.y;
  12         double sh = 0.0;  // FIXME
  13         p.set_start(sx, sy, sh);
  14         double gx = req->goal.pose.position.x;
  15         double gy = req->goal.pose.position.y;
  16         double gb = 0.0;  // FIXME
  17         double ge = 0.0;  // FIXME
  18         p.set_goal(gx, gy, gb, ge);
  19         // TODO make 1000 settable over ROS (max iters before reset)
  20         p.set_imax_reset(1000);
  21         p.reset();
  22         unsigned icnt = 0;
  23         unsigned ncnt = 0;
  24         double best_cost = 0.0;
  25         auto best_path = p.get_path();
  26         // TODO make 1000 settable over ROS (max iters total)
  27         while (icnt < 1000) {
  28                 p.icnt(icnt);
  29                 while (p.next()) {}
  30                 icnt = p.icnt();
  31                 if (best_cost == 0.0 || p.get_path_cost() < best_cost) {
  32                         // TODO make 0.75 settable over ROS
  33                         if (p.get_path_cost() < 0.75 * best_cost) {
  34                                 ncnt = 0;
  35                         }
  36                         best_path = p.get_path();
  37                         best_cost = p.get_path_cost();
  38                 } else {
  39                         ncnt++;
  40                 }
  41                 // TODO make 5 settable over ROS
  42                 if (ncnt > 5) {
  43                         break;
  44                 }
  45                 p.reset();
  46         }
  47         RCLCPP_INFO(rclcpp::get_logger("rclcpp"),
  48                         "path planned in %lf s, path cost %lf m",
  49                         p.scnt(), best_cost);
  50         res->plan.poses.clear();
  51         for (auto pp: best_path) {
  52                 geometry_msgs::msg::PoseStamped p;
  53                 p.pose.position.x = pp.x();
  54                 p.pose.position.y = pp.y();
  55                 p.pose.position.z = 0.0;
  56                 // TODO use pp.h() to compute orientation
  57                 p.pose.orientation.x = 0.0;
  58                 p.pose.orientation.y = 0.0;
  59                 p.pose.orientation.z = 0.0;
  60                 p.pose.orientation.w = 0.0;
  61                 res->plan.poses.push_back(p);
  62         }
  63 }
  64
  65 int
  66 main(int argc, char **argv)
  67 {
  68         rclcpp::init(argc, argv);
  69         auto node = rclcpp::Node::make_shared("rrts");
  70         auto srv = node->create_service<nav_msgs::srv::GetPlan>("rrts/planner",
  71                         &plan_path);
  72         RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ready to plan paths.");
  73         rclcpp::spin(node);
  74         rclcpp::shutdown();
  75         return 0;
  76 }