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

[Anonymous]

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



Я думаю, что это может быть проблема, при которой можно бежать назад. В моем случае это невозможно. Я пытался убедиться, что валидатор движения использует проверку Оуэнса, поскольку она не должна разрешать движение назад, но это ничего не меняет.
Я бы искал что-то вроде этого

В приведенном выше примере путь непрерывен, без каких-либо резких поворотов или движения назад в любом сегмент. Я получил его, используя RRT* вместо RRT Connect. Однако в качестве демонстрации я хотел также запустить код для этого второго алгоритма.
Как правильно запустить Dubins Paths в алгоритме RRT Connect с использованием библиотеки OMPL?
Этот код выполнен на C++ с использованием ROS2.
Кроме того, это не совсем актуально, но для препятствий используются ограничивающие рамки, а для карты — библиотека GridMapGeo. б/у.

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

#include 
#include 
#include 

#include 

#include 

#include 

#include 

#include 
#include 

#include 
#include 

#include 

#include 

#include 

#include "ompl_planner_owen_path_cost/ompl_planner.hpp"

#include 

#include 
#include "grid_map_core/GridMapMath.hpp"

namespace og = ompl::geometric;
namespace ob = ompl::base;

#define DIM 3                         // Dimension of the state space

/**
* @brief Planner class
*/
class OMPL_Planner
{
std::shared_ptr space;
std::shared_ptr terrain_map;
std::shared_ptr validity_checker;
ob::ValidStateSamplerPtr ge_sampler;

double Range = 100;
double Max_wave = 2.0; // Size of the biggest wave allowed to go over
double Cord = 1.0;     // Cord of the wing [m]

std::shared_ptr start, goal;

bool boundsSet = false, startSet = false, goalSet = false, mapSet = false, obstaclesSet = false, pathSet = false;

std::vector
 Path;

std::vector Obstacles;

// Functions

/**
* @brief Path extraction from ProblemDefinition
*/
nav_msgs::msg::Path extractPath(ob::ProblemDefinition *probDef);

/**
* @brief Use terrain map to set bounds
*/
void setBoundsFromMap(const grid_map::GridMap &map);

/**
* @brief Calculate distance to objective path
*/
ob::Cost distanceToPath();

public:
/**
* @brief Basic constructor
*/
OMPL_Planner()
{
setBounds(0, 0, 0, 0, 0, 0);
}

/**
* @brief Constructor
* @param map GridMap of the area, may include layers to show maximum and minimum allowable height (see param setLayers)
* @param start Start position
* @param goal Goal position
* @param setLayers false if GridMap already comes with bounding layers, true if it doesn't
*/
OMPL_Planner(const grid_map::GridMap &map, geometry_msgs::msg::PoseStamped start, geometry_msgs::msg::PoseStamped goal, bool setLayers = false);

/**
* @brief Constructor
* @param range Desired range of the planner
* @param width width of the map (X coordinates)
* @param height height of the map (Y coordinates), not to mistake with elevation
* @param start Start position
* @param goal Goal position
*/
OMPL_Planner(double range, int width, int height, geometry_msgs::msg::PoseStamped start, geometry_msgs::msg::PoseStamped goal);

~OMPL_Planner()
{
}

/**
* @brief Path planning function using gridmap
*/
nav_msgs::msg::Path pathPlanner(double time = 1.0);

/**
* @brief Set Start
* @param poseMsg start position as a ROS2 message
*/
void setStart(geometry_msgs::msg::PoseStamped poseMsg);

/**
* @brief Set Goal
* @param poseMsg start position as a ROS2 message
*/
void setGoal(geometry_msgs::msg::PoseStamped poseMsg);

/**
* @brief Set Bounds
*/
void setBounds(double X_min, double X_max, double Y_min, double Y_max, double Z_min, double Z_max);

/**
* @brief Set Obstacles
* @param obstacles_ Pointer to the list of obstacles as a BoundingBox3DArray
*/
void setObstacles(const vision_msgs::msg::BoundingBox3DArray &obstacles_);

/**
* @brief Set Map
*/
void setMap(const grid_map::GridMap &map, bool setLayers = false);

/**
* @brief Get Map
* @return Map as a GridMap
*/
grid_map::GridMap getMap() { return terrain_map->getGridMap(); }

/**
* \brief Set Path
*/
void setPath(std::vector  Path_msg)
{
Path = Path_msg;
pathSet = true;
}

/**
* @brief Get Path
*/
std::vector getPath()
{
return Path;
}
};

// Constructor
OMPL_Planner::OMPL_Planner(const grid_map::GridMap &map, geometry_msgs::msg::PoseStamped start, geometry_msgs::msg::PoseStamped goal, bool setLayers)
{
grid_map::GridMap temp_map_ = map;
if (Range > map.getResolution())
Range = map.getResolution();
setMap(map, setLayers);
setStart(start);
setGoal(goal);
}

// Constructor
OMPL_Planner::OMPL_Planner(double range, int width, int height, geometry_msgs::msg::PoseStamped start, geometry_msgs::msg::PoseStamped goal)
{
Range = range;
setBounds(-range * width / 2, range * width / 2, -range * height / 2, range * height / 2, 0.0, Max_wave + Cord);
setStart(start);
setGoal(goal);
}

// Path planning function using gridmap
nav_msgs::msg::Path OMPL_Planner::pathPlanner(double time)
{
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 AddLayerDistanceTransform(15.0 + Cord, "max_elevation");

terrain_map->AddLayerDistanceTransform(0.25 * Cord, "distance_surface");

double radius = 10.0;

terrain_map->AddLayerHorizontalDistanceTransform(radius, "ics_+", "distance_surface");

terrain_map->AddLayerHorizontalDistanceTransform(-radius, "ics_-", "max_elevation");
}

if (Range > map.getResolution())
Range = map.getResolution();

setBoundsFromMap(terrain_map->getGridMap());

mapSet = true;
}

// Use terrain map to set bounds
void OMPL_Planner::setBoundsFromMap(const grid_map::GridMap &map)
{
double max_point = std::numeric_limits::min(), min_point = std::numeric_limits::max();

for (grid_map::GridMapIterator it(map); !it.isPastEnd(); ++it)
{
const grid_map::Index map_index = *it;
const double min_point_it = map.at("ics_+", map_index), max_point_it = map.at("ics_-", map_index);
if (min_point_it < min_point)
min_point = min_point_it;
if (max_point_it > max_point)
max_point = max_point_it;
}
setBounds(-map.getLength().x() / 2, map.getLength().x() / 2, -map.getLength().y() / 2, map.getLength().y() / 2, min_point, max_point);
}

Проверка действительности состояния (библиотека PathValidityChecker.hpp внутри приведенного выше кода) следует этому коду:

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

#include 
#include 
#include 
#include 

#include 

#include 

#include 

#include 

#include 
#include "grid_map_core/GridMapMath.hpp"
#include 

// Structs

// Struct for a signle obstacle
struct Obstacle
{
Eigen::Matrix4d transformation_mat;
Eigen::Vector3d boundaries;
};

// Struct for position and orientation
struct Pose
{
Eigen::Vector3d position;
Eigen::Vector4d orientation;
};

struct Quaternion
{
double w, x, y, z;
};

// Function definitions

/**
* @brief Convert Euler angles to quaternion
* @param roll Bank (rad)
* @param pitch Attitude (rad)
* @param yaw Heading (rad)
* @return Quaternion struct
*/
Quaternion Euler2Quaternion(double roll, double pitch, double yaw)
{
double cr = cos(roll * 0.5);
double sr = sin(roll * 0.5);
double cp = cos(pitch * 0.5);
double sp = sin(pitch * 0.5);
double cy = cos(yaw * 0.5);
double sy = sin(yaw * 0.5);

Quaternion q;
q.w = cr * cp * cy + sr * sp * sy;
q.x = sr * cp * cy - cr * sp * sy;
q.y = cr * sp * cy + sr * cp * sy;
q.z = cr * cp * sy - sr * sp * cy;

return q;
}

// Variables

grid_map::Length mapLength_;
grid_map::Position mapPosition_, itPos;
double resolution_;
grid_map::Size bufferSize_;
grid_map::Index bufferStartIndex_;

namespace ompl
{
class PathValidityChecker : public base::StateValidityChecker
{
public:
/**
* @brief Constructor of the owen validity checker
* @param Map Map of the area as a GridMap
*/
PathValidityChecker(const base::SpaceInformationPtr &space_info, const grid_map::GridMap &map_, const std::vector
  Path)
: base::StateValidityChecker(space_info), gridmap(map_), path(Path) {}

/**
* @brief Constructor of the owen validity checker including obstacles
* @param Map Map of the area as a GridMap
* @param Obstacles List of obstacles as an array
*/
PathValidityChecker(const base::SpaceInformationPtr &space_info, const grid_map::GridMap &map_, const std::vector Path, const std::vector Obstacles)
: base::StateValidityChecker(space_info), gridmap(map_), path(Path), obstacle_list(Obstacles) { existObstacles = true; }

/**
* @brief Check if state is valid
* @param state
* @return true if state is valid
* @return false if state is not valid
*/
virtual bool isValid(const base::State *state) const override;

/**
* @brief Check if that position is in collision
* @param state Position as an eigen 3D vector
* @return true if there is collision
* @return false if there is no collision
*/
bool checkCollision(const Eigen::Vector3d state) const;

/**
* @brief Check if that position is in collision
* @param state Position as a Pose
* @return true if there is collision
* @return false if there is no collision
*/
bool checkCollision(const Pose state) const;

/**
* @brief Check minimum distance from current state to the path
* @return Minimum distance to the path
*/
double clearance(const base::State *s) const;

protected:
const grid_map::GridMap &gridmap;
std::vector obstacle_list;
std::vector path;
bool existObstacles = false;
};

bool PathValidityChecker::isValid(const base::State *state) const
{
const auto &st = (*state->as());
Pose state_pos;
state_pos.position(0) = st[0];
state_pos.position(1) = st[1];
state_pos.position(2) = st[2];

Quaternion quat = Euler2Quaternion(0, 0, st.yaw());
state_pos.orientation(0) = quat.x;
state_pos.orientation(1) = quat.y;
state_pos.orientation(2) = quat.z;
state_pos.orientation(3) = quat.w;

// std::cout 

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