diff --git a/dijkstra_2d_planner/CMakeLists.txt b/dijkstra_2d_planner/CMakeLists.txt new file mode 100644 index 0000000..8c73c9a --- /dev/null +++ b/dijkstra_2d_planner/CMakeLists.txt @@ -0,0 +1,48 @@ +cmake_minimum_required(VERSION 3.8) +project(dijkstra_2d_planner) + +add_compile_options(-Wall -g3) + +set (CMAKE_CXX_STANDARD 20) + +find_package(ament_cmake_ros REQUIRED) +find_package(mbf_octo_core REQUIRED) +find_package(mbf_msgs REQUIRED) +find_package(mbf_utility REQUIRED) +find_package(rclcpp REQUIRED) +find_package(pluginlib REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(pcl_conversions REQUIRED) + +pluginlib_export_plugin_description_file(mbf_octo_core dijkstra_2d_planner.xml) + + +add_library(${PROJECT_NAME} + src/dijkstra_2d_planner.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ + ${pluginlib_INCLUDE_DIRS} +) +target_compile_definitions(${PROJECT_NAME} PRIVATE "DIJKSTRA_2D_PLANNER_BUILDING_LIBRARY") +ament_target_dependencies(${PROJECT_NAME} mbf_octo_core mbf_msgs mbf_utility rclcpp pluginlib sensor_msgs pcl_conversions) +target_link_libraries(${PROJECT_NAME} + ${MPI_CXX_LIBRARIES} +) + +install(DIRECTORY include/ + DESTINATION include +) +install(TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets(export_${PROJECT_NAME}) +ament_export_dependencies(mbf_octo_core mbf_msgs mbf_utility rclcpp pluginlib) +ament_package() diff --git a/dijkstra_2d_planner/dijkstra_2d_planner.xml b/dijkstra_2d_planner/dijkstra_2d_planner.xml new file mode 100644 index 0000000..7f0695b --- /dev/null +++ b/dijkstra_2d_planner/dijkstra_2d_planner.xml @@ -0,0 +1,8 @@ + + + + An Dijkstra 2D costmap planner for mbf_octo_nav + + + diff --git a/dijkstra_2d_planner/include/dijkstra_2d_planner/dijkstra_2d_planner.h b/dijkstra_2d_planner/include/dijkstra_2d_planner/dijkstra_2d_planner.h new file mode 100644 index 0000000..76e1984 --- /dev/null +++ b/dijkstra_2d_planner/include/dijkstra_2d_planner/dijkstra_2d_planner.h @@ -0,0 +1,210 @@ +/* + * Copyright 2025, MASCOR + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * authors: + * MASCOR + * + */ + +#ifndef OCTO_NAVIGATION__DIJKSTRA_2D_PLANNER_H +#define OCTO_NAVIGATION__DIJKSTRA_2D_PLANNER_H + +#include +#include +#include +#include +#include +#include +#include + +namespace dijkstra_2d_planner +{ + +class Dijkstra2DPlanner : public mbf_octo_core::OctoPlanner +{ +public: + typedef std::shared_ptr Ptr; + + Dijkstra2DPlanner(); + + /** + * @brief Destructor + */ + virtual ~Dijkstra2DPlanner(); + + /** + * @brief Given a goal pose in the world, compute a plan + * + * @param start The start pose + * @param goal The goal pose + * @param tolerance If the goal is obstructed, how many meters the planner can + * relax the constraint in x and y before failing + * @param plan The plan... filled by the planner + * @param cost The cost for the the plan + * @param message Optional more detailed outcome as a string + * + * @return Result code as described on GetPath action result: + * SUCCESS = 0 + * 1..9 are reserved as plugin specific non-error results + * FAILURE = 50 # Unspecified failure, only used for old, + * non-mfb_core based plugins CANCELED = 51 INVALID_START = 52 + * INVALID_GOAL = 53 + * NO_PATH_FOUND = 54 + * PAT_EXCEEDED = 55 + * EMPTY_PATH = 56 + * TF_ERROR = 57 + * NOT_INITIALIZED = 58 + * INVALID_PLUGIN = 59 + * INTERNAL_ERROR = 60 + * 71..99 are reserved as plugin specific errors + */ + virtual uint32_t makePlan(const geometry_msgs::msg::PoseStamped& start, const geometry_msgs::msg::PoseStamped& goal, + double tolerance, std::vector& plan, double& cost, + std::string& message) override; + + /** + * @brief Requests the planner to cancel, e.g. if it takes too much time. + * + * @return True if a cancel has been successfully requested, false if not + * implemented. + */ + virtual bool cancel() override; + + /** + * @brief initializes this planner with the given plugin name and map + * + * @param name name of this plugin + * + * @return true if initialization was successul; else false + */ + virtual bool initialize(const std::string& plugin_name, const rclcpp::Node::SharedPtr& node) override; + + +protected: + /** + * @brief runs dijkstra path planning and stores the resulting distances and predecessors to the fields potential and + * predecessors of this class + * + * @param start[in] 3D starting position of the requested path + * @param goal[in] 3D goal position of the requested path + * @param path[out] optimal path from the given starting position to tie goal position + * + * @return result code in form of GetPath action result: SUCCESS, NO_PATH_FOUND, INVALID_START, INVALID_GOAL, and + * CANCELED are possible + */ + //uint32_t astar(const mesh_map::Vector& start, const mesh_map::Vector& goal, std::list& path); + + /** + * @brief runs dijkstra path planning + *src/octo_navigation/astar_2D_planner/include/astar_2D_planner/astar_2D_planner.h + */ + // uint32_t dijkstra(const mesh_map::Vector& start, const mesh_map::Vector& goal, + // const lvr2::DenseEdgeMap& edge_weights, const lvr2::DenseVertexMap& costs, + // std::list& path, lvr2::DenseVertexMap& distances, + // lvr2::DenseVertexMap& predecessors); + + + /** + * @brief gets called whenever the node's parameters change + + * @param parameters vector of changed parameters. + * Note that this vector will also contain parameters not related to the dijkstra mesh planner. + */ + rcl_interfaces::msg::SetParametersResult reconfigureCallback(std::vector parameters); + +private: + // // current map + // mesh_map::MeshMap::Ptr mesh_map_; + // name of this plugin + std::string name_; + // node handle + rclcpp::Node::SharedPtr node_; + // true if the abort of the current planning was requested; else false + std::atomic_bool cancel_planning_; + // publisher of resulting path + rclcpp::Publisher::SharedPtr path_pub_; + // tf frame of the map + std::string map_frame_; + // handle of callback for changing parameters dynamically + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr reconfiguration_callback_handle_; + // config determined by ROS params; Init values defined here are used as default ROS param value + struct { + // publisher of resulting vector fiels + bool publish_vector_field = false; + // publisher of per face vectorfield + bool publish_face_vectors = false; + // offset of maximum distance from goal position + double goal_dist_offset = 0.3; + // defines the vertex cost limit with which it can be accessed + double cost_limit = 1.0; + } config_; + // Utility functions of the 3D Planner. + // // Callback for point cloud subscription. + void ugvCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr& msg); + rclcpp::Subscription::SharedPtr pointcloud_sub_; + // Occupancy grid represented as a 2D vector. + std::vector> occupancy_grid_; + // geometry_msgs::msg::Point find_nearest_3d_point(geometry_msgs::msg::Point point, const std::vector>>& array_3d); + geometry_msgs::msg::Point worldToGrid(const geometry_msgs::msg::Point & point); + std::array gridToWorld(const std::tuple& grid_pt); + // bool isWithinBounds(const std::tuple& pt); + // bool isWithinBounds(const geometry_msgs::msg::Point & pt); + bool isOccupied(const std::tuple& pt); + // bool hasNoOccupiedCellsAbove(const std::tuple& coord, + // double vertical_min, double vertical_range); + // bool isCylinderCollisionFree(const std::tuple& coord, double radius); + // std::vector> astar(const std::tuple& start, + // const std::tuple& goal); + // Voxel grid parameters. + double voxel_size_; + double z_threshold_; + double robot_radius_ = 0.35; + double min_vertical_clearance_ = 0.4; + double max_vertical_clearance_ = 0.6; + + // // Minimum bound for the occupancy grid. + std::array min_bound_; + std::array max_bound_; + + // // Hash function for tuple to track unique occupied voxels + struct TupleHash { + template + std::size_t operator()(const std::tuple& t) const { + auto [a, b, c] = t; + return std::hash()(a) ^ std::hash()(b) ^ std::hash()(c); + } + }; +}; + +} // namespace dijkstra_2D_planner + +#endif // OCTO_NAVIGATION__DIJKSTRA_2D_PLANNER_H diff --git a/dijkstra_2d_planner/package.xml b/dijkstra_2d_planner/package.xml new file mode 100644 index 0000000..c4ee88f --- /dev/null +++ b/dijkstra_2d_planner/package.xml @@ -0,0 +1,21 @@ + + + + dijkstra_2d_planner + 1.0.0 + Creates a 2D plan using dijkstra for mbf_octo_core in ROS2 + Jonathan Behrends + CC BY-NC + + ament_cmake + + rclcpp + mbf_octo_core + mbf_utility + mbf_msgs + pluginlib + + + ament_cmake + + diff --git a/dijkstra_2d_planner/src/dijkstra_2d_planner.cpp b/dijkstra_2d_planner/src/dijkstra_2d_planner.cpp new file mode 100644 index 0000000..4c740d6 --- /dev/null +++ b/dijkstra_2d_planner/src/dijkstra_2d_planner.cpp @@ -0,0 +1,428 @@ +/* + * Copyright 2025, MASCOR + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * authors: + * MASCOR + * + */ + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +PLUGINLIB_EXPORT_CLASS(dijkstra_2d_planner::Dijkstra2DPlanner, mbf_octo_core::OctoPlanner); + +namespace dijkstra_2d_planner +{ + +// Structure for A* search nodes in the grid. +struct GridNode { +public: + std::tuple coord; + double f; // f-score = g + h + double g; // cost from start to this node + std::tuple prev; //prev == coord -> null, except for (0,0) + bool visited; // To check if neighbour is in unvisited in O(1) + + bool operator>(const GridNode& other) const { + return f > other.f; + } + + GridNode() {} + GridNode(std::tuple coord, double f, double g, std::tuple prev, bool visited) : coord(coord), f(f), g(g), prev(prev), visited(visited) {} +}; + +Dijkstra2DPlanner::Dijkstra2DPlanner() +: voxel_size_(0.1), // double than that of octomap resolution + z_threshold_(0.3) // same as Z_THRESHOLD in Python +{ + // The occupancy grid will be populated by the point cloud callback. + occupancy_grid_.clear(); + + // Set a default minimum bound; this will be updated when processing point clouds. + min_bound_ = {0.0, 0.0, 0.0}; + max_bound_ = {0.0, 0.0, 0.0}; +} + +Dijkstra2DPlanner::~Dijkstra2DPlanner() {} + +bool Dijkstra2DPlanner::isOccupied(const std::tuple& pt) +{ + int x, y, z; + std::tie(x, y, z) = pt; + + //RCLCPP_INFO(node_->get_logger(), "isOccupied(x = %i, y = %i, z = %i)", x, y, z); + //RCLCPP_INFO(node_->get_logger(), "Occupancy Grid = %i", occupancy_grid_[x][y]); + + return occupancy_grid_[x][y] > 0; + //return false; +} + +geometry_msgs::msg::Point Dijkstra2DPlanner::worldToGrid(const geometry_msgs::msg::Point& point) +{ + geometry_msgs::msg::Point grid_pt; + grid_pt.x = static_cast((point.x - min_bound_[0]) / voxel_size_); + grid_pt.y = static_cast((point.y - min_bound_[1]) / voxel_size_); + grid_pt.z = static_cast((point.z - min_bound_[2]) / voxel_size_); + return grid_pt; +} + +std::array Dijkstra2DPlanner::gridToWorld(const std::tuple& grid_pt) +{ + int x, y, z; + std::tie(x, y, z) = grid_pt; + double wx = x * voxel_size_ + min_bound_[0]; + double wy = y * voxel_size_ + min_bound_[1]; + double wz = z * voxel_size_ + min_bound_[2]; + return {wx, wy, wz}; +} + +uint32_t Dijkstra2DPlanner::makePlan(const geometry_msgs::msg::PoseStamped& start, + const geometry_msgs::msg::PoseStamped& goal, + double tolerance, + std::vector& plan, + double& cost, + std::string& message) +{ + RCLCPP_INFO(node_->get_logger(), "Start dijkstra 2D planner."); + RCLCPP_INFO(node_->get_logger(), "Start position: x = %f, y = %f, z = %f, frame_id = %s", + start.pose.position.x, start.pose.position.y, start.pose.position.z, + start.header.frame_id.c_str()); + + // Convert world coordinates to grid indices using the helper function. + geometry_msgs::msg::Point start_grid = worldToGrid(start.pose.position); + geometry_msgs::msg::Point goal_grid = worldToGrid(goal.pose.position); + + //RCLCPP_INFO(node_->get_logger(), "Start position after conversion: start_x = %f, start_y = %f, goal_x = %f, goal_y = %f", start_grid.x, start_grid.y, goal_grid.x, goal_grid.y); + + int grid_size_x = max_bound_[0]; //std::floor(std::abs(goal_grid.x - start_grid.x)) + 1; + int grid_size_y = max_bound_[1]; //std::floor(std::abs(goal_grid.y - start_grid.y)) + 1; + int grid_bottom_left_x = 0; //std::round(std::min(start_grid.x, goal_grid.x)); + int grid_bottom_left_y = 0; //std::round(std::min(start_grid.y, goal_grid.y)); + int nrGridNodes = grid_size_x*grid_size_y; + int start_x = std::round(start_grid.x); + int start_y = std::round(start_grid.y); + int goal_x = std::round(goal_grid.x); + int goal_y = std::round(goal_grid.y); + + //RCLCPP_INFO(node_->get_logger(), "GridSize x: %i, GridSize y: %i", grid_size_x, grid_size_y); + + std::vector> grid; + grid.reserve(grid_size_x); + + std::tuple gridOrder[nrGridNodes]; //gridOrder[0] is the grid node with minimum distance from start. Rest is ordered in ascending distance. + + //Initialize grid + for(int x = 0; x column; + column.reserve(grid_size_y); + + for(int y = 0; y(grid_bottom_left_x + x, grid_bottom_left_y + y), std::numeric_limits::infinity(), std::numeric_limits::infinity(), std::tuple(grid_bottom_left_x + x, grid_bottom_left_y + y), false)); + gridOrder[y+grid_size_y*x] = std::tuple(x, y); + } + grid.emplace_back(column); + } + + //RCLCPP_INFO(node_->get_logger(), "init grid: start_x = %i, start_y = %i", start_x, start_y); + //RCLCPP_INFO(node_->get_logger(), "grid empty?: %i", (int)grid.empty()); + //RCLCPP_INFO(node_->get_logger(), "size: %i", (int)grid.size()); + + //RCLCPP_INFO(node_->get_logger(), "Inner size: %i", (int)grid[0].size()); + + /*for(const auto& column : grid) { + for(const auto & node : column) { + RCLCPP_INFO(node_->get_logger(), "node= (x: %i, y: %i, f: %f, g: %f, prevX: %i, prevY: %i, visited: %i)", get<0>(node.coord), get<1>(node.coord), node.f, node.g, get<0>(node.prev), get<1>(node.prev), (int)node.visited); + } + }*/ + + //Initialize starting node + grid[start_x][start_y].f = 0.0; + grid[start_x][start_y].g = 0.0; + grid[start_x][start_y].prev = std::tuple(start_x, start_y); + + RCLCPP_INFO(node_->get_logger(), "node(%i, %i) g: %f", start_x, start_y, grid.at(start_x).at(start_y).g); + + // Swap order with node (0, 0) + std::get<0>(gridOrder[start_y+grid_size_y*start_x]) = 0; + std::get<1>(gridOrder[start_y+grid_size_y*start_x]) = 0; + std::get<0>(gridOrder[0]) = start_x; + std::get<1>(gridOrder[0]) = start_y; + + RCLCPP_INFO(node_->get_logger(), "isOccupied(%i,%i) = %i", start_x, start_y, (int)isOccupied(std::tuple(start_x, start_y, 0))); + RCLCPP_INFO(node_->get_logger(), "gridOrder(0) = %i, %i, gridOrder(grid) = (%i, %i, %f)", start_x, start_y, std::get<0>(grid[std::get<0>(gridOrder[0])][std::get<1>(gridOrder[0])].coord), std::get<1>(grid[std::get<0>(gridOrder[0])][std::get<1>(gridOrder[0])].coord), grid[std::get<0>(gridOrder[0])][std::get<1>(gridOrder[0])].g); + + std::vector collisionRadiusPointsX; + std::vector collisionRadiusPointsY; + double r = robot_radius_/voxel_size_; //robot radius in grid units + const int nrCollisionRadiusPoints = 12; + for(int i = 0; i < nrCollisionRadiusPoints; i++) { //Check for colllision on collision radius + double phi = i*M_PI/6; + collisionRadiusPointsX.push_back(std::round(r*std::cos(phi))); + collisionRadiusPointsY.push_back(std::round(r*std::sin(phi))); + } + + for(int vIndex = 0; vIndexget_logger(), "vIndex = %i", vIndex); + + GridNode* current = &grid[std::get<0>(gridOrder[vIndex])][std::get<1>(gridOrder[vIndex])]; + current->visited = true; + + //RCLCPP_INFO(node_->get_logger(), "Current node: x = %i, y = %i, g=%f", get<0>(current->coord), get<1>(current->coord), current->g); + //RCLCPP_INFO(node_->get_logger(), "Current node: x = %i, y = %i, g=%f, vIndex=%i, gridOrderX=%i, gridOrderY=%i", get<0>(current->coord), get<1>(current->coord), current->g, vIndex, std::get<0>(gridOrder[vIndex]), std::get<1>(gridOrder[vIndex])); + + if(grid[std::get<0>(current->coord)][std::get<1>(current->coord)].g > 2*grid_size_x*grid_size_y) { + RCLCPP_INFO(node_->get_logger(), "Costs = infinity. Terminating search..."); + break; + } + + if(std::get<0>(current->coord) == goal_x && std::get<1>(current->coord) == goal_y) { + RCLCPP_ERROR(node_->get_logger(), "Goal reached. Terminating search..."); + break; + } + + //RCLCPP_INFO(node_->get_logger(), "After break"); + + for(int x = -1; x<=1; x++) { //Get all 8 grid neighbours + for(int y = -1; y<=1; y++) { + //RCLCPP_INFO(node_->get_logger(), "x = %i, y = %i", x, y); + if(get<0>(current->coord) + x < 0 || get<1>(current->coord) + y < 0 || std::get<0>(current->coord) + x >= grid_size_x || std::get<1>(current->coord) + y >= grid_size_y || grid[std::get<0>(current->coord)+x][std::get<1>(current->coord)+y].visited || isOccupied(std::tuple(std::get<0>(current->coord)+x, std::get<1>(current->coord)+y, 0))) { + //RCLCPP_INFO(node_->get_logger(), "Continue at x = %i, y = %i", x, y); // crash at (1, 0) in isOccupied + continue; // neighbour not in unvisited or neighbour is occupied or part of border + } + + bool radiusCollision = false; + for(int i = 0; i < nrCollisionRadiusPoints; i++) { //Check for colllision on collision radius + if(isOccupied(std::tuple(std::get<0>(current->coord)+collisionRadiusPointsX[i]+x, std::get<1>(current->coord)+collisionRadiusPointsY[i]+y, 0))) { + radiusCollision = true; + //RCLCPP_ERROR(node_->get_logger(), "Radius collision detected in (%i, %i)", std::get<0>(current->coord)+std::round(r*std::cos(phi))+x, std::get<1>(current->coord)+std::round(r*std::sin(phi))+y); + break; + } + } + if(radiusCollision) { + break; + } + + double newF = current->g + std::sqrt(x*x + y*y); // dist(current) + cost(current, neighbour) + + if(newF < grid[std::get<0>(current->coord)+x][std::get<1>(current->coord)+y].g) { // Is dist(current) + cost(current, neighbour) < dist(neighbour) + grid[std::get<0>(current->coord)+x][std::get<1>(current->coord)+y].g = newF; // dist(neighbour) = dist(current) + cost(current, neighbour) + grid[std::get<0>(current->coord)+x][std::get<1>(current->coord)+y].f = newF; //TODO: Add Heuristic, if enabled + + // pred(neighbour) = current + std::get<0>(grid[std::get<0>(current->coord)+x][std::get<1>(current->coord)+y].prev) = std::get<0>(current->coord); + std::get<1>(grid[std::get<0>(current->coord)+x][std::get<1>(current->coord)+y].prev) = std::get<1>(current->coord); + + //Sort gridOrder, by moving the neigbour down until the next element now longer has a higher dist + for(int i = 0; i(grid[get<0>(gridOrder[i])][get<1>(gridOrder[i])].coord) == std::get<0>(current->coord)+x && get<1>(grid[get<0>(gridOrder[i])][get<1>(gridOrder[i])].coord) == std::get<1>(current->coord)+y) { + //RCLCPP_INFO(node_->get_logger(), "i = %i, gridOrder[i] = (%i, %i)", i, get<0>(gridOrder[i]), get<1>(gridOrder[i])); + + while(--i > vIndex && grid[get<0>(gridOrder[i])][get<1>(gridOrder[i])].g > newF) { + //std::get<0>(grid[get<0>(gridOrder[i+1])][get<1>(gridOrder[i+1])].coord) = std::get<0>(grid[get<0>(gridOrder[i])][get<1>(gridOrder[i])].coord); + //std::get<1>(grid[get<0>(gridOrder[i+1])][get<1>(gridOrder[i+1])].coord) = std::get<1>(grid[get<0>(gridOrder[i])][get<1>(gridOrder[i])].coord); + + gridOrder[i+1] = gridOrder[i]; + } + gridOrder[i+1] = grid[std::get<0>(current->coord)+x][std::get<1>(current->coord)+y].coord; + + std::get<0>(grid[get<0>(gridOrder[i+1])][get<1>(gridOrder[i+1])].prev) = std::get<0>(current->coord); + std::get<1>(grid[get<0>(gridOrder[i+1])][get<1>(gridOrder[i+1])].prev) = std::get<1>(current->coord); + break; + } + } + + /*RCLCPP_ERROR(node_->get_logger(), "Grid Order:"); + for(int i = 0; iget_logger(), "%i: (%i, %i)", i, get<0>(gridOrder[i]), get<1>(gridOrder[i])); + }*/ + } + } + } + } + + //TODO: return failure case + if(!grid[goal_x - grid_bottom_left_x][goal_y - grid_bottom_left_y].visited) { + RCLCPP_ERROR(node_->get_logger(), "No path found using Dijkstra."); + return mbf_msgs::action::GetPath::Result::FAILURE; + } + + std::vector grid_path; + std::tuple pt = grid[std::round(goal_grid.x) - grid_bottom_left_x][std::round(goal_grid.y) - grid_bottom_left_y].coord; + while(!(pt == grid[std::get<0>(pt)][std::get<1>(pt)].prev)) { + + geometry_msgs::msg::Point newP; + newP.set__x(std::get<0>(pt)); + newP.set__y(std::get<1>(pt)); + newP.set__z(0.0); + + grid_path.push_back(newP); + + cost += std::sqrt(std::pow(std::get<0>(grid[std::get<0>(pt)][std::get<1>(pt)].coord) - std::get<0>(grid[std::get<0>(grid[std::get<0>(pt)][std::get<1>(pt)].prev)][std::get<1>(grid[std::get<0>(pt)][std::get<1>(pt)].prev)].coord), 2) + + std::pow(std::get<1>(grid[std::get<0>(pt)][std::get<1>(pt)].coord) - std::get<1>(grid[std::get<0>(grid[std::get<0>(pt)][std::get<1>(pt)].prev)][std::get<1>(grid[std::get<0>(pt)][std::get<1>(pt)].prev)].coord), 2)); + + pt = grid[std::get<0>(pt)][std::get<1>(pt)].prev; + } + + //Todo: fill plan with poses + plan.clear(); + for (const auto& grid_pt : std::ranges::reverse_view(grid_path)) { + auto world_pt = gridToWorld(std::tuple(grid_pt.x, grid_pt.y, grid_pt.z)); + geometry_msgs::msg::PoseStamped pose; + pose.header = start.header; // Use same frame and timestamp + pose.pose.position.x = world_pt[0]; + pose.pose.position.y = world_pt[1]; + pose.pose.position.z = world_pt[2]; + pose.pose.orientation.w = 1.0; // Default orientation + plan.push_back(pose); + } + + // Publish the path for visualization. + nav_msgs::msg::Path path_msg; + path_msg.header.stamp = node_->now(); + path_msg.header.frame_id = "odom"; + path_msg.poses = plan; + path_pub_->publish(path_msg); + + RCLCPP_INFO_STREAM(node_->get_logger(), "Path found with length: " << cost << " steps"); + + return mbf_msgs::action::GetPath::Result::SUCCESS; +} + + + +bool Dijkstra2DPlanner::cancel() +{ + cancel_planning_ = true; + return true; +} + +bool Dijkstra2DPlanner::initialize(const std::string& plugin_name, const rclcpp::Node::SharedPtr& node) +{ + name_ = plugin_name; + node_ = node; + RCLCPP_INFO(node_->get_logger(), "init start"); + + config_.publish_vector_field = node_->declare_parameter(name_ + ".publish_vector_field", config_.publish_vector_field); + config_.publish_face_vectors = node_->declare_parameter(name_ + ".publish_face_vectors", config_.publish_face_vectors); + config_.goal_dist_offset = node_->declare_parameter(name_ + ".goal_dist_offset", config_.goal_dist_offset); + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "Defines the vertex cost limit with which it can be accessed."; + rcl_interfaces::msg::FloatingPointRange range; + range.from_value = 0.0; + range.to_value = 10.0; + descriptor.floating_point_range.push_back(range); + config_.cost_limit = node_->declare_parameter(name_ + ".cost_limit", config_.cost_limit); + } + + path_pub_ = node_->create_publisher("~/path", rclcpp::QoS(1).transient_local()); + + pointcloud_sub_ = node_->create_subscription( + "/mapUGV", + 10, + [this](const nav_msgs::msg::OccupancyGrid::SharedPtr msg) { + this->ugvCallback(msg); + }); + + reconfiguration_callback_handle_ = node_->add_on_set_parameters_callback( + std::bind(&Dijkstra2DPlanner::reconfigureCallback, this, std::placeholders::_1)); + + RCLCPP_INFO(node_->get_logger(), "init complete"); + + return true; +} + +void Dijkstra2DPlanner::ugvCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr& msg) +{ + int width = (int)msg->info.width; + int height = (int)msg->info.height; + int origin_x = (int)msg->info.origin.position.x; + int origin_y = (int)msg->info.origin.position.y; + + RCLCPP_INFO(node_->get_logger(), "OccupancyGrid recieved: width = %i, height = %i, orig_x = %i, orig_y = %i", width, height, origin_x, origin_y); + + min_bound_[0] = msg->info.origin.position.x; + min_bound_[1] = msg->info.origin.position.y; + max_bound_[0] = msg->info.width; + max_bound_[1] = msg->info.height; + + /*RCLCPP_INFO(node_->get_logger(), "OccupancyGrid recieved: %s, (%i, %i), Data =", std::string(msg->header.frame_id), width, height); + for (int i = 0; iget_logger(), "%i", msg->data[i]); + }*/ + + // Initialize 3D occupancy grid with -1 (unknown) + occupancy_grid_.clear(); + occupancy_grid_.resize(width, + std::vector(height, -1)); + + // Track occupied voxel count + std::unordered_set, TupleHash> occupied_voxels; + + // Populate the occupancy grid + for(int y = 0; ydata[x + y*width]; // <- bugged, alles -1 + occupied_voxels.insert({x, y, 0}); + } + } +} + +rcl_interfaces::msg::SetParametersResult Dijkstra2DPlanner::reconfigureCallback(std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + for (auto& parameter : parameters) { + if (parameter.get_name() == name_ + ".cost_limit") { + config_.cost_limit = parameter.as_double(); + RCLCPP_INFO_STREAM(node_->get_logger(), "New cost limit parameter received via dynamic reconfigure."); + } + } + result.successful = true; + return result; +} + +} // namespace dijkstra_2D_planner