plan_path(const std::shared_ptr<nav_msgs::srv::GetPlan::Request> req,
std::shared_ptr<nav_msgs::srv::GetPlan::Response> res)
{
- rrts::P39 p;
+ rrts::P40 p;
double sx = req->start.pose.position.x;
double sy = req->start.pose.position.y;
tf2::Quaternion qh;