OMPL не меняет ориентацию ⇐ C++

[Anonymous]

Я использую OMPL для получения путей для экраноплана с помощью OwenStateSpace.
Дело в том, что по какой-то причине он не принимает во внимание ориентацию каждого состояния при создании промежуточных состояний, поэтому, когда я показываю его на RVIZ, это всегда расположение по умолчанию, как вы можете видеть на этих рисунках.




Это особенно проблема при использовании RRTConnect, поскольку соединение в середине вызывает внезапный поворот на 180°, поскольку конец одной ветви точно так же, как начало другого, а не противоположно, как вы можете видеть на этом другом рисунке.


Код будет следующим:

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

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

auto planner(std::make_shared(si));
planner->setRange(Range);
planner->setProblemDefinition(probDef);
planner->setup();
ob::PlannerStatus solved = planner->ob::Planner::solve(time);
return_path = extractPath(probDef.get());

extractPath() — это просто функция, которая преобразует путь в сообщение для темы ROS2. Но ошибки быть не может, потому что проблема возникает раньше.
При настройке начала и цели, как вы можете видеть, он получает правильные ориентации, поэтому просто игнорирует ориентацию промежуточных состояний.
Этот код cpp выполняется внутри узла ROS2 на виртуальной машине Ubuntu 22.
Изменить: проблема с ориентациями заключалась в настройке рыскания как состояния [3] вместо состояние.рыскание(). Итак, этот вопрос был решен. Однако проблема с RRTConnect все еще существует.

Подробнее здесь: https://stackoverflow.com/questions/798 ... rientation