Dubins Paths и RRT Connect на OMPL [закрыто] ⇐ C++

[Anonymous]

Я пытаюсь использовать RRTconnect в пространстве состояний Оуэна с использованием библиотеки OMPL.
RRTconnect — это метод поиска, основанный на RRT, который вместо того, чтобы получать одну начальную точку для достижения цели с помощью растущих деревьев, он выращивает деревья как с самого начала, так и от цели, ожидая, что оба будут соединяться в середине.
Пространство состояний Оуэна — это пространство состояний, которое заставляет движение между точками следовать по путям Дубина, которые представляют собой минимальные кривые, позволяющие перемещаться между позами (позиция+ориентация).
По какой-то причине в моем коде я получаю внезапный поворот на 180° в месте пересечения начальной и целевой ветвей.



Я думаю, что это может быть проблема, при которой можно бежать назад. В моем случае это невозможно. Я пробовал убедиться, что валидатор движения использует проверку Оуэнса, так как она не должна допускать движения назад, но это ничего не изменило.
В чем может быть проблема?
Этот код выполнен на C++ с использованием ROS2.
Также не совсем актуально, но для препятствий используются Bounding Boxes, а для карты используется библиотека GridMapGeo.

Код: Выделить всё

  std::shared_ptr space; // In here it is stated to use owen paths
std::shared_ptr terrain_map; // Uses GridMapGeo for the map
auto si(std::make_shared(space));

// It is possible for obstacles to exist, composed of bounding boxes
if (obstaclesSet)
{
validity_checker = std::make_shared(si, terrain_map->getGridMap(), Path);
si->setStateValidityChecker(ob::StateValidityCheckerPtr(validity_checker));
}

// If there are no obstacles
else
{
validity_checker = std::make_shared(si, terrain_map->getGridMap(), Path, Obstacles);
si->setStateValidityChecker(ob::StateValidityCheckerPtr(validity_checker));
}
si->setMotionValidator(std::make_shared(si));

auto probDef(std::make_shared(si));
probDef->setStartAndGoalStates(*start, *goal);
probDef->setOptimizationObjective(getOptObj(si));

auto planner(std::make_shared(si));// In here it is stated to use RRTConnect
planner->setRange(Range);
// planner->setTreePruning(false);
// planner->setDelayCC(true);
planner->setProblemDefinition(probDef);
planner->setup();
printf("..................PLANNER HAS THE FOLLOWING SETTINGS..................\n");
si->printSettings(std::cout);
printf("......................................................................\n");
probDef->print(std::cout);
printf("......................................................................\n");

ob::PlannerStatus solved = planner->ob::Planner::solve(time);

nav_msgs::msg::Path return_path;

if (solved == ob::PlannerStatus::EXACT_SOLUTION || solved == ob::PlannerStatus::APPROXIMATE_SOLUTION)
{
return_path = extractPath(probDef.get());
std::cout 

Подробнее здесь: [url]https://stackoverflow.com/questions/79886874/dubins-paths-and-rrt-connect-on-ompl[/url]