src/rrts_wr.cc

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