From e0b849bc21b7c7a33f10dbb671bd0ce65ea042f3 Mon Sep 17 00:00:00 2001 From: Matthias Holoch Date: Tue, 23 Jan 2024 17:03:53 +0100 Subject: [PATCH 01/19] port dijkstra planner pkg info to ROS2 --- dijkstra_mesh_planner/CMakeLists.txt | 73 +++++++++++----------------- dijkstra_mesh_planner/COLCON_IGNORE | 0 dijkstra_mesh_planner/package.xml | 7 ++- 3 files changed, 31 insertions(+), 49 deletions(-) delete mode 100644 dijkstra_mesh_planner/COLCON_IGNORE diff --git a/dijkstra_mesh_planner/CMakeLists.txt b/dijkstra_mesh_planner/CMakeLists.txt index 9c68a59f..d62b6cd5 100644 --- a/dijkstra_mesh_planner/CMakeLists.txt +++ b/dijkstra_mesh_planner/CMakeLists.txt @@ -1,58 +1,41 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.8) project(dijkstra_mesh_planner) -set(CMAKE_CXX_STANDARD 14) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) +find_package(mbf_mesh_core REQUIRED) +find_package(mbf_msgs REQUIRED) +find_package(mbf_utility REQUIRED) +find_package(mesh_map REQUIRED) +find_package(rclcpp REQUIRED) -find_package(catkin REQUIRED COMPONENTS - roscpp - mbf_mesh_core - mbf_utility - mbf_msgs - mesh_map - dynamic_reconfigure -) - -find_package(PkgConfig REQUIRED) -pkg_check_modules(JSONCPP jsoncpp) - -generate_dynamic_reconfigure_options( - cfg/DijkstraMeshPlanner.cfg -) - -catkin_package( - LIBRARIES dijkstra_mesh_planner - CATKIN_DEPENDS roscpp mbf_mesh_core mbf_msgs mesh_map dynamic_reconfigure -) +pluginlib_export_plugin_description_file(mbf_mesh_core dijkstra_mesh_planner.xml) -include_directories( - include - ${catkin_INCLUDE_DIRS} -) +#find_package(PkgConfig REQUIRED) +#pkg_check_modules(JSONCPP jsoncpp) add_library(${PROJECT_NAME} src/dijkstra_mesh_planner.cpp ) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $) +ament_target_dependencies(${PROJECT_NAME} ${dependencies}) +target_compile_definitions(${PROJECT_NAME} PRIVATE "FTC_CONTROLLER_BUILDING_LIBRARY") +ament_target_dependencies(${PROJECT_NAME} mbf_mesh_core mbf_msgs mbf_utility mesh_map rclcpp) -add_dependencies(${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} - ${PROJECT_NAME}_gencfg -) - -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${JSONCPP_LIBRARIES} +install(DIRECTORY include/ + DESTINATION include ) - install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install(FILES dijkstra_mesh_planner.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + 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_mesh_core mbf_msgs mbf_utility mesh_map rclcpp) +ament_package() \ No newline at end of file diff --git a/dijkstra_mesh_planner/COLCON_IGNORE b/dijkstra_mesh_planner/COLCON_IGNORE deleted file mode 100644 index e69de29b..00000000 diff --git a/dijkstra_mesh_planner/package.xml b/dijkstra_mesh_planner/package.xml index d87d4416..2fa68f07 100644 --- a/dijkstra_mesh_planner/package.xml +++ b/dijkstra_mesh_planner/package.xml @@ -1,5 +1,5 @@ - + dijkstra_mesh_planner 1.0.1 The dijkstra_mesh_planner package @@ -8,14 +8,13 @@ Sebastian Pütz catkin - roscpp + rclcpp mbf_mesh_core mbf_utility mbf_msgs mesh_map - dynamic_reconfigure - + ament_cmake From ab23356360c8129d4388ea8345abe41d8a291b1c Mon Sep 17 00:00:00 2001 From: Matthias Holoch Date: Tue, 23 Jan 2024 17:04:09 +0100 Subject: [PATCH 02/19] change plugin name to fit current naming scheme --- dijkstra_mesh_planner/dijkstra_mesh_planner.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dijkstra_mesh_planner/dijkstra_mesh_planner.xml b/dijkstra_mesh_planner/dijkstra_mesh_planner.xml index b1cb9eb6..43100f80 100644 --- a/dijkstra_mesh_planner/dijkstra_mesh_planner.xml +++ b/dijkstra_mesh_planner/dijkstra_mesh_planner.xml @@ -1,4 +1,4 @@ - + From f97e31494cc1f9279e390ddb29ca64b14b7dfad7 Mon Sep 17 00:00:00 2001 From: Matthias Holoch Date: Wed, 24 Jan 2024 09:46:09 +0100 Subject: [PATCH 03/19] port includes, member and function types to ROS2 --- .../dijkstra_mesh_planner.h | 37 ++++++++----------- .../src/dijkstra_mesh_planner.cpp | 15 ++++---- 2 files changed, 23 insertions(+), 29 deletions(-) diff --git a/dijkstra_mesh_planner/include/dijkstra_mesh_planner/dijkstra_mesh_planner.h b/dijkstra_mesh_planner/include/dijkstra_mesh_planner/dijkstra_mesh_planner.h index 8b1e0c7a..699f0233 100644 --- a/dijkstra_mesh_planner/include/dijkstra_mesh_planner/dijkstra_mesh_planner.h +++ b/dijkstra_mesh_planner/include/dijkstra_mesh_planner/dijkstra_mesh_planner.h @@ -39,17 +39,16 @@ #define MESH_NAVIGATION__DIJKSTRA_MESH_PLANNER_H #include -#include +#include #include -#include -#include +#include namespace dijkstra_mesh_planner { class DijkstraMeshPlanner : public mbf_mesh_core::MeshPlanner { public: - typedef boost::shared_ptr Ptr; + typedef std::shared_ptr Ptr; DijkstraMeshPlanner(); @@ -84,9 +83,9 @@ class DijkstraMeshPlanner : public mbf_mesh_core::MeshPlanner * INTERNAL_ERROR = 60 * 71..99 are reserved as plugin specific errors */ - virtual uint32_t makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, - double tolerance, std::vector& plan, double& cost, - std::string& message); + 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. @@ -94,7 +93,7 @@ class DijkstraMeshPlanner : public mbf_mesh_core::MeshPlanner * @return True if a cancel has been successfully requested, false if not * implemented. */ - virtual bool cancel(); + virtual bool cancel() override; /** * @brief initializes this planner with the given plugin name and map @@ -104,7 +103,7 @@ class DijkstraMeshPlanner : public mbf_mesh_core::MeshPlanner * * @return true if initialization was successul; else false */ - virtual bool initialize(const std::string& name, const boost::shared_ptr& mesh_map_ptr); + virtual bool initialize(const std::string& name, const std::shared_ptr& mesh_map_ptr, const rclcpp::Node::SharedPtr& node) override; /** * @brief delivers vector field which has been generated during the latest planning @@ -152,12 +151,12 @@ class DijkstraMeshPlanner : public mbf_mesh_core::MeshPlanner void computeVectorMap(); /** - * @brief gets called on new incoming reconfigure parameters + * @brief gets called whenever the node's parameters change * - * @param cfg new configuration - * @param level level + * @param parameters vector of changed parameters. + * Note that this vector will also contain parameters not related to the dijkstra mesh planner. */ - void reconfigureCallback(dijkstra_mesh_planner::DijkstraMeshPlannerConfig& cfg, uint32_t level); + rcl_interfaces::msg::SetParametersResult reconfigureCallback(std::vector parameters); private: // current map @@ -165,11 +164,11 @@ class DijkstraMeshPlanner : public mbf_mesh_core::MeshPlanner // name of this plugin std::string name; // node handle - ros::NodeHandle private_nh; + 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 - ros::Publisher path_pub; + rclcpp::Publisher path_pub; // publisher of resulting vector fiels bool publish_vector_field; // publisher of per face vectorfield @@ -178,12 +177,8 @@ class DijkstraMeshPlanner : public mbf_mesh_core::MeshPlanner std::string map_frame; // offset of maximum distance from goal position float goal_dist_offset; - // Server for Reconfiguration - boost::shared_ptr> - reconfigure_server_ptr; - dynamic_reconfigure::Server::CallbackType config_callback; - bool first_config; - DijkstraMeshPlannerConfig config; + // handle of callback for changing parameters dynamically + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr reconfiguration_callback_handle_; // predecessors while wave propagation lvr2::DenseVertexMap predecessors; diff --git a/dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp b/dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp index cc1525a0..18c78e86 100644 --- a/dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp +++ b/dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp @@ -37,9 +37,9 @@ #include #include -#include +#include #include -#include +#include PLUGINLIB_EXPORT_CLASS(dijkstra_mesh_planner::DijkstraMeshPlanner, mbf_mesh_core::MeshPlanner); @@ -53,9 +53,9 @@ DijkstraMeshPlanner::~DijkstraMeshPlanner() { } -uint32_t DijkstraMeshPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, - double tolerance, std::vector& plan, double& cost, - std::string& message) +uint32_t DijkstraMeshPlanner::makePlan(const geometry_msgs::msg::PoseStamped& start, const geometry_msgs::msg::PoseStamped& goal, + double tolerance, std::vector& plan, double& cost, + std::string& message) { const auto& mesh = mesh_map->mesh(); std::list path; @@ -126,8 +126,7 @@ bool DijkstraMeshPlanner::cancel() return true; } -bool DijkstraMeshPlanner::initialize(const std::string& plugin_name, - const boost::shared_ptr& mesh_map_ptr) +bool DijkstraMeshPlanner::initialize(const std::string& name, const std::shared_ptr& mesh_map_ptr, const rclcpp::Node::SharedPtr& node) { mesh_map = mesh_map_ptr; name = plugin_name; @@ -156,7 +155,7 @@ lvr2::DenseVertexMap DijkstraMeshPlanner::getVectorMap() return vector_map; } -void DijkstraMeshPlanner::reconfigureCallback(dijkstra_mesh_planner::DijkstraMeshPlannerConfig& cfg, uint32_t level) +rcl_interfaces::msg::SetParametersResult DijkstraMeshPlanner::reconfigureCallback(std::vector parameters) { ROS_INFO_STREAM("New height diff layer config through dynamic reconfigure."); if (first_config) From dbaa749e687a0c31505b4a80e47b7e5c2b26e80b Mon Sep 17 00:00:00 2001 From: Matthias Holoch Date: Wed, 24 Jan 2024 09:53:39 +0100 Subject: [PATCH 04/19] use std shared ptr for mesh_map --- mesh_map/include/mesh_map/mesh_map.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mesh_map/include/mesh_map/mesh_map.h b/mesh_map/include/mesh_map/mesh_map.h index 8fbed13d..ddfc30e6 100644 --- a/mesh_map/include/mesh_map/mesh_map.h +++ b/mesh_map/include/mesh_map/mesh_map.h @@ -61,7 +61,7 @@ class MeshMap { public: inline static const std::string MESH_MAP_NAMESPACE = "mesh_map"; - typedef boost::shared_ptr Ptr; + typedef std::shared_ptr Ptr; MeshMap(tf2_ros::Buffer& tf, const rclcpp::Node::SharedPtr& node); From 633a5f298035bd65c1ba44ac2f0748bb8955fe1c Mon Sep 17 00:00:00 2001 From: Matthias Holoch Date: Wed, 24 Jan 2024 09:53:50 +0100 Subject: [PATCH 05/19] refactor: use trailing underscore for class members --- .../dijkstra_mesh_planner.h | 28 +++---- .../src/dijkstra_mesh_planner.cpp | 76 +++++++++---------- 2 files changed, 52 insertions(+), 52 deletions(-) diff --git a/dijkstra_mesh_planner/include/dijkstra_mesh_planner/dijkstra_mesh_planner.h b/dijkstra_mesh_planner/include/dijkstra_mesh_planner/dijkstra_mesh_planner.h index 699f0233..233f6d4c 100644 --- a/dijkstra_mesh_planner/include/dijkstra_mesh_planner/dijkstra_mesh_planner.h +++ b/dijkstra_mesh_planner/include/dijkstra_mesh_planner/dijkstra_mesh_planner.h @@ -103,7 +103,7 @@ class DijkstraMeshPlanner : public mbf_mesh_core::MeshPlanner * * @return true if initialization was successul; else false */ - virtual bool initialize(const std::string& name, const std::shared_ptr& mesh_map_ptr, const rclcpp::Node::SharedPtr& node) override; + virtual bool initialize(const std::string& plugin_name, const std::shared_ptr& mesh_map_ptr, const rclcpp::Node::SharedPtr& node) override; /** * @brief delivers vector field which has been generated during the latest planning @@ -160,35 +160,35 @@ class DijkstraMeshPlanner : public mbf_mesh_core::MeshPlanner private: // current map - mesh_map::MeshMap::Ptr mesh_map; + mesh_map::MeshMap::Ptr mesh_map_; // name of this plugin - std::string name; + std::string name_; // node handle - rclcpp::Node::SharedPtr node; + rclcpp::Node::SharedPtr node_; // true if the abort of the current planning was requested; else false - std::atomic_bool cancel_planning; + std::atomic_bool cancel_planning_; // publisher of resulting path - rclcpp::Publisher path_pub; + rclcpp::Publisher path_pub_; // publisher of resulting vector fiels - bool publish_vector_field; + bool publish_vector_field_; // publisher of per face vectorfield - bool publish_face_vectors; + bool publish_face_vectors_; // tf frame of the map - std::string map_frame; + std::string map_frame_; // offset of maximum distance from goal position - float goal_dist_offset; + float goal_dist_offset_; // handle of callback for changing parameters dynamically rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr reconfiguration_callback_handle_; // predecessors while wave propagation - lvr2::DenseVertexMap predecessors; + lvr2::DenseVertexMap predecessors_; // the face which is cut by line to the source - lvr2::DenseVertexMap cutting_faces; + lvr2::DenseVertexMap cutting_faces_; // stores the current vector map containing vectors pointing to the source // (path goal) - lvr2::DenseVertexMap vector_map; + lvr2::DenseVertexMap vector_map_; // potential field or distance values to the source (path goal) - lvr2::DenseVertexMap potential; + lvr2::DenseVertexMap potential_; }; } // namespace dijkstra_mesh_planner diff --git a/dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp b/dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp index 18c78e86..b1da3e1d 100644 --- a/dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp +++ b/dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp @@ -57,7 +57,7 @@ uint32_t DijkstraMeshPlanner::makePlan(const geometry_msgs::msg::PoseStamped& st double tolerance, std::vector& plan, double& cost, std::string& message) { - const auto& mesh = mesh_map->mesh(); + const auto& mesh = mesh_map_->mesh(); std::list path; ROS_INFO("start dijkstra mesh planner."); @@ -71,13 +71,13 @@ uint32_t DijkstraMeshPlanner::makePlan(const geometry_msgs::msg::PoseStamped& st std_msgs::Header header; header.stamp = ros::Time::now(); - header.frame_id = mesh_map->mapFrame(); + header.frame_id = mesh_map_->mapFrame(); cost = 0; if (!path.empty()) { mesh_map::Vector& vec = start_vec; - const auto& vertex_normals = mesh_map->vertexNormals(); + const auto& vertex_normals = mesh_map_->vertexNormals(); mesh_map::Normal normal = vertex_normals[path.front()]; float dir_length; @@ -107,14 +107,14 @@ uint32_t DijkstraMeshPlanner::makePlan(const geometry_msgs::msg::PoseStamped& st path_msg.poses = plan; path_msg.header = header; - path_pub.publish(path_msg); - mesh_map->publishVertexCosts(potential, "Potential"); + path_pub_.publish(path_msg); + mesh_map_->publishVertexCosts(potential_, "Potential"); ROS_INFO_STREAM("Path length: " << cost << "m"); - if (publish_vector_field) + if (publish_vector_field_) { - mesh_map->publishVectorField("vector_field", vector_map, publish_face_vectors); + mesh_map_->publishVectorField("vector_field", vector_map_, publish_face_vectors_); } return outcome; @@ -122,23 +122,23 @@ uint32_t DijkstraMeshPlanner::makePlan(const geometry_msgs::msg::PoseStamped& st bool DijkstraMeshPlanner::cancel() { - cancel_planning = true; + cancel_planning_ = true; return true; } -bool DijkstraMeshPlanner::initialize(const std::string& name, const std::shared_ptr& mesh_map_ptr, const rclcpp::Node::SharedPtr& node) +bool DijkstraMeshPlanner::initialize(const std::string& plugin_name, const std::shared_ptr& mesh_map_ptr, const rclcpp::Node::SharedPtr& node) { - mesh_map = mesh_map_ptr; - name = plugin_name; - map_frame = mesh_map->mapFrame(); - private_nh = ros::NodeHandle("~/" + name); + mesh_map_ = mesh_map_ptr; + name_ = plugin_name; + map_frame_ = mesh_map_->mapFrame(); + node_ = node; - private_nh.param("publish_vector_field", publish_vector_field, false); - private_nh.param("publish_face_vectors", publish_face_vectors, false); - private_nh.param("goal_dist_offset", goal_dist_offset, 0.3f); + private_nh.param("publish_vector_field", publish_vector_field_, false); + private_nh.param("publish_face_vectors", publish_face_vectors_, false); + private_nh.param("goal_dist_offset", goal_dist_offset_, 0.3f); - path_pub = private_nh.advertise("path", 1, true); - const auto& mesh = mesh_map->mesh(); + path_pub_ = private_nh.advertise("path", 1, true); + const auto& mesh = mesh_map_->mesh(); reconfigure_server_ptr = boost::shared_ptr>( @@ -152,7 +152,7 @@ bool DijkstraMeshPlanner::initialize(const std::string& name, const std::shared_ lvr2::DenseVertexMap DijkstraMeshPlanner::getVectorMap() { - return vector_map; + return vector_map_; } rcl_interfaces::msg::SetParametersResult DijkstraMeshPlanner::reconfigureCallback(std::vector parameters) @@ -169,11 +169,11 @@ rcl_interfaces::msg::SetParametersResult DijkstraMeshPlanner::reconfigureCallbac void DijkstraMeshPlanner::computeVectorMap() { - const auto& mesh = mesh_map->mesh(); + const auto& mesh = mesh_map_->mesh(); for (auto v3 : mesh.vertices()) { - const lvr2::VertexHandle& v1 = predecessors[v3]; + const lvr2::VertexHandle& v1 = predecessors_[v3]; // if predecessor is pointing to it self, continue with the next vertex. if (v1 == v3) continue; @@ -184,15 +184,15 @@ void DijkstraMeshPlanner::computeVectorMap() // compute the direction vector and store it in the direction vertex map const auto dirVec = vec1 - vec3; // store the normalized rotated vector in the vector map - vector_map.insert(v3, dirVec.normalized()); + vector_map_.insert(v3, dirVec.normalized()); } - mesh_map->setVectorMap(vector_map); + mesh_map_->setVectorMap(vector_map_); } uint32_t DijkstraMeshPlanner::dijkstra(const mesh_map::Vector& start, const mesh_map::Vector& goal, std::list& path) { - return dijkstra(start, goal, mesh_map->edgeDistances(), mesh_map->vertexCosts(), path, potential, predecessors); + return dijkstra(start, goal, mesh_map_->edgeDistances(), mesh_map_->vertexCosts(), path, potential_, predecessors_); } uint32_t DijkstraMeshPlanner::dijkstra(const mesh_map::Vector& original_start, const mesh_map::Vector& original_goal, @@ -204,19 +204,19 @@ uint32_t DijkstraMeshPlanner::dijkstra(const mesh_map::Vector& original_start, c ROS_INFO_STREAM("Init wave front propagation."); ros::WallTime t_initialization_start = ros::WallTime::now(); - const auto& mesh = mesh_map->mesh(); - const auto& vertex_costs = mesh_map->vertexCosts(); + const auto& mesh = mesh_map_->mesh(); + const auto& vertex_costs = mesh_map_->vertexCosts(); - auto& invalid = mesh_map->invalid; + auto& invalid = mesh_map_->invalid; - mesh_map->publishDebugPoint(original_start, mesh_map::color(0, 1, 0), "start_point"); - mesh_map->publishDebugPoint(original_goal, mesh_map::color(0, 0, 1), "goal_point"); + mesh_map_->publishDebugPoint(original_start, mesh_map::color(0, 1, 0), "start_point"); + mesh_map_->publishDebugPoint(original_goal, mesh_map::color(0, 0, 1), "goal_point"); // Find the closest vertex handle of start and goal - const auto& start_opt = mesh_map->getNearestVertexHandle(original_start); - const auto& goal_opt = mesh_map->getNearestVertexHandle(original_goal); + const auto& start_opt = mesh_map_->getNearestVertexHandle(original_start); + const auto& goal_opt = mesh_map_->getNearestVertexHandle(original_goal); // reset cancel planning - cancel_planning = false; + cancel_planning_ = false; if (!start_opt) return mbf_msgs::GetPathResult::INVALID_START; @@ -238,7 +238,7 @@ uint32_t DijkstraMeshPlanner::dijkstra(const mesh_map::Vector& original_start, c lvr2::DenseVertexMap fixed(mesh.nextVertexIndex(), false); // clear vector field map - vector_map.clear(); + vector_map_.clear(); ros::WallTime t_start, t_end; t_start = ros::WallTime::now(); @@ -266,7 +266,7 @@ uint32_t DijkstraMeshPlanner::dijkstra(const mesh_map::Vector& original_start, c size_t fixed_set_cnt = 0; - while (!pq.isEmpty() && !cancel_planning) + while (!pq.isEmpty() && !cancel_planning_) { lvr2::VertexHandle current_vh = pq.popMin().key(); fixed[current_vh] = true; @@ -275,7 +275,7 @@ uint32_t DijkstraMeshPlanner::dijkstra(const mesh_map::Vector& original_start, c if (current_vh == goal_vertex) { ROS_INFO_STREAM("The Dijkstra Mesh Planner reached the goal."); - goal_dist = distances[current_vh] + goal_dist_offset; + goal_dist = distances[current_vh] + goal_dist_offset_; } if (distances[current_vh] > goal_dist) @@ -329,7 +329,7 @@ uint32_t DijkstraMeshPlanner::dijkstra(const mesh_map::Vector& original_start, c } } - if (cancel_planning) + if (cancel_planning_) { ROS_WARN_STREAM("Wave front propagation has been canceled!"); return mbf_msgs::GetPathResult::CANCELED; @@ -348,7 +348,7 @@ uint32_t DijkstraMeshPlanner::dijkstra(const mesh_map::Vector& original_start, c auto vH = goal_vertex; - while (vH != start_vertex && !cancel_planning) + while (vH != start_vertex && !cancel_planning_) { vH = predecessors[vH]; path.push_front(vH); @@ -361,7 +361,7 @@ uint32_t DijkstraMeshPlanner::dijkstra(const mesh_map::Vector& original_start, c computeVectorMap(); - if (cancel_planning) + if (cancel_planning_) { ROS_WARN_STREAM("Dijkstra has been canceled!"); return mbf_msgs::GetPathResult::CANCELED; From 6555738a8ed9bfee4c7c3b4181f54c0d59bba5cc Mon Sep 17 00:00:00 2001 From: Matthias Holoch Date: Wed, 24 Jan 2024 09:57:13 +0100 Subject: [PATCH 06/19] port logging to ROS2 --- .../src/dijkstra_mesh_planner.cpp | 35 ++++++++++--------- 1 file changed, 18 insertions(+), 17 deletions(-) diff --git a/dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp b/dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp index b1da3e1d..e0f65b50 100644 --- a/dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp +++ b/dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp @@ -40,6 +40,7 @@ #include #include #include +#include PLUGINLIB_EXPORT_CLASS(dijkstra_mesh_planner::DijkstraMeshPlanner, mbf_mesh_core::MeshPlanner); @@ -59,7 +60,7 @@ uint32_t DijkstraMeshPlanner::makePlan(const geometry_msgs::msg::PoseStamped& st { const auto& mesh = mesh_map_->mesh(); std::list path; - ROS_INFO("start dijkstra mesh planner."); + RCLCPP_INFO(node_->get_logger(), "start dijkstra mesh planner."); mesh_map::Vector goal_vec = mesh_map::toVector(goal.pose.position); mesh_map::Vector start_vec = mesh_map::toVector(start.pose.position); @@ -102,7 +103,7 @@ uint32_t DijkstraMeshPlanner::makePlan(const geometry_msgs::msg::PoseStamped& st plan.push_back(pose); } - ROS_INFO_STREAM("Path length: " << cost << "m"); + RCLCPP_INFO_STREAM(node_->get_logger(), "Path length: " << cost << "m"); nav_msgs::Path path_msg; path_msg.poses = plan; path_msg.header = header; @@ -110,7 +111,7 @@ uint32_t DijkstraMeshPlanner::makePlan(const geometry_msgs::msg::PoseStamped& st path_pub_.publish(path_msg); mesh_map_->publishVertexCosts(potential_, "Potential"); - ROS_INFO_STREAM("Path length: " << cost << "m"); + RCLCPP_INFO_STREAM(node_->get_logger(), "Path length: " << cost << "m"); if (publish_vector_field_) { @@ -157,7 +158,7 @@ lvr2::DenseVertexMap DijkstraMeshPlanner::getVectorMap() rcl_interfaces::msg::SetParametersResult DijkstraMeshPlanner::reconfigureCallback(std::vector parameters) { - ROS_INFO_STREAM("New height diff layer config through dynamic reconfigure."); + RCLCPP_INFO_STREAM(node_->get_logger(), "New height diff layer config through dynamic reconfigure."); if (first_config) { config = cfg; @@ -201,7 +202,7 @@ uint32_t DijkstraMeshPlanner::dijkstra(const mesh_map::Vector& original_start, c lvr2::DenseVertexMap& distances, lvr2::DenseVertexMap& predecessors) { - ROS_INFO_STREAM("Init wave front propagation."); + RCLCPP_INFO_STREAM(node_->get_logger(), "Init wave front propagation."); ros::WallTime t_initialization_start = ros::WallTime::now(); const auto& mesh = mesh_map_->mesh(); @@ -260,7 +261,7 @@ uint32_t DijkstraMeshPlanner::dijkstra(const mesh_map::Vector& original_start, c float goal_dist = std::numeric_limits::infinity(); - ROS_INFO_STREAM("Start Dijkstra"); + RCLCPP_INFO_STREAM(node_->get_logger(), "Start Dijkstra"); ros::WallTime t_propagation_start = ros::WallTime::now(); double initialization_duration = (t_propagation_start - t_initialization_start).toNSec() * 1e-6; @@ -274,7 +275,7 @@ uint32_t DijkstraMeshPlanner::dijkstra(const mesh_map::Vector& original_start, c if (current_vh == goal_vertex) { - ROS_INFO_STREAM("The Dijkstra Mesh Planner reached the goal."); + RCLCPP_INFO_STREAM(node_->get_logger(), "The Dijkstra Mesh Planner reached the goal."); goal_dist = distances[current_vh] + goal_dist_offset_; } @@ -331,15 +332,15 @@ uint32_t DijkstraMeshPlanner::dijkstra(const mesh_map::Vector& original_start, c if (cancel_planning_) { - ROS_WARN_STREAM("Wave front propagation has been canceled!"); + RCLCPP_WARN_STREAM(node_->get_logger(), "Wave front propagation has been canceled!"); return mbf_msgs::GetPathResult::CANCELED; } - ROS_INFO_STREAM("The Dijkstra Mesh Planner finished the propagation."); + RCLCPP_INFO_STREAM(node_->get_logger(), "The Dijkstra Mesh Planner finished the propagation."); if (goal_vertex == predecessors[goal_vertex]) { - ROS_WARN("Predecessor of the goal is not set! No path found!"); + RCLCPP_WARN(node_->get_logger(), "Predecessor of the goal is not set! No path found!"); return mbf_msgs::GetPathResult::NO_PATH_FOUND; } @@ -356,26 +357,26 @@ uint32_t DijkstraMeshPlanner::dijkstra(const mesh_map::Vector& original_start, c t_end = ros::WallTime::now(); double execution_time = (t_end - t_start).toNSec() * 1e-6; - ROS_INFO_STREAM("Execution time (ms): " << execution_time << " for " << mesh.numVertices() + RCLCPP_INFO_STREAM(node_->get_logger(), "Execution time (ms): " << execution_time << " for " << mesh.numVertices() << " num vertices in the mesh."); computeVectorMap(); if (cancel_planning_) { - ROS_WARN_STREAM("Dijkstra has been canceled!"); + RCLCPP_WARN_STREAM(node_->get_logger(), "Dijkstra has been canceled!"); return mbf_msgs::GetPathResult::CANCELED; } ros::WallTime t_path_backtracking = ros::WallTime::now(); double path_backtracking_duration = (t_path_backtracking - t_propagation_end).toNSec() * 1e-6; - ROS_INFO_STREAM("Processed " << fixed_set_cnt << " vertices in the fixed set."); - ROS_INFO_STREAM("Initialization duration (ms): " << initialization_duration); - ROS_INFO_STREAM("Execution time wavefront propagation (ms): "<< propagation_duration); - ROS_INFO_STREAM("Path backtracking duration (ms): " << path_backtracking_duration); + RCLCPP_INFO_STREAM(node_->get_logger(), "Processed " << fixed_set_cnt << " vertices in the fixed set."); + RCLCPP_INFO_STREAM(node_->get_logger(), "Initialization duration (ms): " << initialization_duration); + RCLCPP_INFO_STREAM(node_->get_logger(), "Execution time wavefront propagation (ms): "<< propagation_duration); + RCLCPP_INFO_STREAM(node_->get_logger(), "Path backtracking duration (ms): " << path_backtracking_duration); - ROS_INFO_STREAM("Successfully finished Dijkstra back tracking!"); + RCLCPP_INFO_STREAM(node_->get_logger(), "Successfully finished Dijkstra back tracking!"); return mbf_msgs::GetPathResult::SUCCESS; } From 6ec4a5b7aa59306d33ebbb89ffda5406aa14c2d2 Mon Sep 17 00:00:00 2001 From: Matthias Holoch Date: Wed, 24 Jan 2024 09:59:07 +0100 Subject: [PATCH 07/19] fix msg types for ROS2 --- .../src/dijkstra_mesh_planner.cpp | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp b/dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp index e0f65b50..8d417536 100644 --- a/dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp +++ b/dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp @@ -70,8 +70,8 @@ uint32_t DijkstraMeshPlanner::makePlan(const geometry_msgs::msg::PoseStamped& st path.reverse(); - std_msgs::Header header; - header.stamp = ros::Time::now(); + std_msgs::msg::Header header; + header.stamp = node_->now(); header.frame_id = mesh_map_->mapFrame(); cost = 0; @@ -82,7 +82,7 @@ uint32_t DijkstraMeshPlanner::makePlan(const geometry_msgs::msg::PoseStamped& st mesh_map::Normal normal = vertex_normals[path.front()]; float dir_length; - geometry_msgs::PoseStamped pose; + geometry_msgs::msg::PoseStamped pose; pose.header = header; while (!path.empty()) @@ -104,7 +104,7 @@ uint32_t DijkstraMeshPlanner::makePlan(const geometry_msgs::msg::PoseStamped& st } RCLCPP_INFO_STREAM(node_->get_logger(), "Path length: " << cost << "m"); - nav_msgs::Path path_msg; + nav_msgs::msg::Path path_msg; path_msg.poses = plan; path_msg.header = header; @@ -138,7 +138,7 @@ bool DijkstraMeshPlanner::initialize(const std::string& plugin_name, const std:: private_nh.param("publish_face_vectors", publish_face_vectors_, false); private_nh.param("goal_dist_offset", goal_dist_offset_, 0.3f); - path_pub_ = private_nh.advertise("path", 1, true); + path_pub_ = private_nh.advertise("path", 1, true); const auto& mesh = mesh_map_->mesh(); reconfigure_server_ptr = @@ -220,9 +220,9 @@ uint32_t DijkstraMeshPlanner::dijkstra(const mesh_map::Vector& original_start, c cancel_planning_ = false; if (!start_opt) - return mbf_msgs::GetPathResult::INVALID_START; + return mbf_msgs::action::GetPath::Result::INVALID_START; if (!goal_opt) - return mbf_msgs::GetPathResult::INVALID_GOAL; + return mbf_msgs::action::GetPath::Result::INVALID_GOAL; const auto& start_vertex = start_opt.unwrap(); const auto& goal_vertex = goal_opt.unwrap(); @@ -233,7 +233,7 @@ uint32_t DijkstraMeshPlanner::dijkstra(const mesh_map::Vector& original_start, c if (goal_vertex == start_vertex) { - return mbf_msgs::GetPathResult::SUCCESS; + return mbf_msgs::action::GetPath::Result::SUCCESS; } lvr2::DenseVertexMap fixed(mesh.nextVertexIndex(), false); @@ -333,7 +333,7 @@ uint32_t DijkstraMeshPlanner::dijkstra(const mesh_map::Vector& original_start, c if (cancel_planning_) { RCLCPP_WARN_STREAM(node_->get_logger(), "Wave front propagation has been canceled!"); - return mbf_msgs::GetPathResult::CANCELED; + return mbf_msgs::action::GetPath::Result::CANCELED; } RCLCPP_INFO_STREAM(node_->get_logger(), "The Dijkstra Mesh Planner finished the propagation."); @@ -341,7 +341,7 @@ uint32_t DijkstraMeshPlanner::dijkstra(const mesh_map::Vector& original_start, c if (goal_vertex == predecessors[goal_vertex]) { RCLCPP_WARN(node_->get_logger(), "Predecessor of the goal is not set! No path found!"); - return mbf_msgs::GetPathResult::NO_PATH_FOUND; + return mbf_msgs::action::GetPath::Result::NO_PATH_FOUND; } ros::WallTime t_propagation_end = ros::WallTime::now(); @@ -365,7 +365,7 @@ uint32_t DijkstraMeshPlanner::dijkstra(const mesh_map::Vector& original_start, c if (cancel_planning_) { RCLCPP_WARN_STREAM(node_->get_logger(), "Dijkstra has been canceled!"); - return mbf_msgs::GetPathResult::CANCELED; + return mbf_msgs::action::GetPath::Result::CANCELED; } ros::WallTime t_path_backtracking = ros::WallTime::now(); @@ -377,7 +377,7 @@ uint32_t DijkstraMeshPlanner::dijkstra(const mesh_map::Vector& original_start, c RCLCPP_INFO_STREAM(node_->get_logger(), "Path backtracking duration (ms): " << path_backtracking_duration); RCLCPP_INFO_STREAM(node_->get_logger(), "Successfully finished Dijkstra back tracking!"); - return mbf_msgs::GetPathResult::SUCCESS; + return mbf_msgs::action::GetPath::Result::SUCCESS; } } /* namespace dijkstra_mesh_planner */ From c555e0b94b10b3cf0365945bbe9707e4dcc155f8 Mon Sep 17 00:00:00 2001 From: Matthias Holoch Date: Wed, 24 Jan 2024 11:12:25 +0100 Subject: [PATCH 08/19] fix publisher and parameter handling --- .../dijkstra_mesh_planner.h | 21 +++++--- .../src/dijkstra_mesh_planner.cpp | 50 +++++++++++-------- 2 files changed, 41 insertions(+), 30 deletions(-) diff --git a/dijkstra_mesh_planner/include/dijkstra_mesh_planner/dijkstra_mesh_planner.h b/dijkstra_mesh_planner/include/dijkstra_mesh_planner/dijkstra_mesh_planner.h index 233f6d4c..7d2ee205 100644 --- a/dijkstra_mesh_planner/include/dijkstra_mesh_planner/dijkstra_mesh_planner.h +++ b/dijkstra_mesh_planner/include/dijkstra_mesh_planner/dijkstra_mesh_planner.h @@ -152,7 +152,7 @@ class DijkstraMeshPlanner : public mbf_mesh_core::MeshPlanner /** * @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. */ @@ -168,17 +168,22 @@ class DijkstraMeshPlanner : public mbf_mesh_core::MeshPlanner // true if the abort of the current planning was requested; else false std::atomic_bool cancel_planning_; // publisher of resulting path - rclcpp::Publisher path_pub_; - // publisher of resulting vector fiels - bool publish_vector_field_; - // publisher of per face vectorfield - bool publish_face_vectors_; + rclcpp::Publisher::SharedPtr path_pub_; // tf frame of the map std::string map_frame_; - // offset of maximum distance from goal position - float goal_dist_offset_; // 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_; // predecessors while wave propagation lvr2::DenseVertexMap predecessors_; diff --git a/dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp b/dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp index 8d417536..d2c11159 100644 --- a/dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp +++ b/dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp @@ -108,14 +108,14 @@ uint32_t DijkstraMeshPlanner::makePlan(const geometry_msgs::msg::PoseStamped& st path_msg.poses = plan; path_msg.header = header; - path_pub_.publish(path_msg); + path_pub_->publish(path_msg); mesh_map_->publishVertexCosts(potential_, "Potential"); RCLCPP_INFO_STREAM(node_->get_logger(), "Path length: " << cost << "m"); - if (publish_vector_field_) + if (config_.publish_vector_field) { - mesh_map_->publishVectorField("vector_field", vector_map_, publish_face_vectors_); + mesh_map_->publishVectorField("vector_field", vector_map_, config_.publish_face_vectors); } return outcome; @@ -134,19 +134,24 @@ bool DijkstraMeshPlanner::initialize(const std::string& plugin_name, const std:: map_frame_ = mesh_map_->mapFrame(); node_ = node; - private_nh.param("publish_vector_field", publish_vector_field_, false); - private_nh.param("publish_face_vectors", publish_face_vectors_, false); - private_nh.param("goal_dist_offset", goal_dist_offset_, 0.3f); + config_.publish_vector_field = node_->declare_parameter("publish_vector_field", config_.publish_vector_field); + config_.publish_face_vectors = node_->declare_parameter("publish_face_vectors", config_.publish_face_vectors); + config_.goal_dist_offset = node->declare_parameter("goal_dist_offset", config_.goal_dist_offset); + { // cost limit param + 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("cost_limit", config_.cost_limit); + } - path_pub_ = private_nh.advertise("path", 1, true); + path_pub_ = node_->create_publisher("~/path", rclcpp::QoS(1).transient_local()); const auto& mesh = mesh_map_->mesh(); - reconfigure_server_ptr = - boost::shared_ptr>( - new dynamic_reconfigure::Server(private_nh)); - - config_callback = boost::bind(&DijkstraMeshPlanner::reconfigureCallback, this, _1, _2); - reconfigure_server_ptr->setCallback(config_callback); + reconfiguration_callback_handle_ = node_->add_on_set_parameters_callback(std::bind( + &DijkstraMeshPlanner::reconfigureCallback, this, std::placeholders::_1)); return true; } @@ -158,14 +163,15 @@ lvr2::DenseVertexMap DijkstraMeshPlanner::getVectorMap() rcl_interfaces::msg::SetParametersResult DijkstraMeshPlanner::reconfigureCallback(std::vector parameters) { - RCLCPP_INFO_STREAM(node_->get_logger(), "New height diff layer config through dynamic reconfigure."); - if (first_config) - { - config = cfg; - first_config = false; - return; + 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 height diff layer config through dynamic reconfigure."); + } } - config = cfg; + result.successful = true; + return result; } void DijkstraMeshPlanner::computeVectorMap() @@ -276,13 +282,13 @@ uint32_t DijkstraMeshPlanner::dijkstra(const mesh_map::Vector& original_start, c if (current_vh == goal_vertex) { RCLCPP_INFO_STREAM(node_->get_logger(), "The Dijkstra Mesh Planner reached the goal."); - goal_dist = distances[current_vh] + goal_dist_offset_; + goal_dist = distances[current_vh] + config_.goal_dist_offset; } if (distances[current_vh] > goal_dist) continue; - if (vertex_costs[current_vh] > config.cost_limit) + if (vertex_costs[current_vh] > config_.cost_limit) continue; std::vector edges; From 1904839ef8f8e09fec5a02fe2918d4ef1acb5d8c Mon Sep 17 00:00:00 2001 From: Matthias Holoch Date: Wed, 24 Jan 2024 11:39:24 +0100 Subject: [PATCH 09/19] fix timing calculations --- .../src/dijkstra_mesh_planner.cpp | 32 +++++++++---------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp b/dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp index d2c11159..5c200ad6 100644 --- a/dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp +++ b/dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp @@ -35,6 +35,7 @@ * */ +#include #include #include #include @@ -209,7 +210,7 @@ uint32_t DijkstraMeshPlanner::dijkstra(const mesh_map::Vector& original_start, c lvr2::DenseVertexMap& predecessors) { RCLCPP_INFO_STREAM(node_->get_logger(), "Init wave front propagation."); - ros::WallTime t_initialization_start = ros::WallTime::now(); + const auto t_initialization_start = std::chrono::steady_clock::now(); const auto& mesh = mesh_map_->mesh(); const auto& vertex_costs = mesh_map_->vertexCosts(); @@ -247,8 +248,7 @@ uint32_t DijkstraMeshPlanner::dijkstra(const mesh_map::Vector& original_start, c // clear vector field map vector_map_.clear(); - ros::WallTime t_start, t_end; - t_start = ros::WallTime::now(); + const auto t_start = std::chrono::steady_clock::now(); // initialize distances with infinity // initialize predecessor of each vertex with itself @@ -268,8 +268,8 @@ uint32_t DijkstraMeshPlanner::dijkstra(const mesh_map::Vector& original_start, c float goal_dist = std::numeric_limits::infinity(); RCLCPP_INFO_STREAM(node_->get_logger(), "Start Dijkstra"); - ros::WallTime t_propagation_start = ros::WallTime::now(); - double initialization_duration = (t_propagation_start - t_initialization_start).toNSec() * 1e-6; + const auto t_propagation_start = std::chrono::steady_clock::now(); + const auto initialization_duration_ms = std::chrono::duration_cast(t_propagation_start - t_initialization_start); size_t fixed_set_cnt = 0; @@ -350,8 +350,8 @@ uint32_t DijkstraMeshPlanner::dijkstra(const mesh_map::Vector& original_start, c return mbf_msgs::action::GetPath::Result::NO_PATH_FOUND; } - ros::WallTime t_propagation_end = ros::WallTime::now(); - double propagation_duration = (t_propagation_end - t_propagation_start).toNSec() * 1e-6; + const auto t_propagation_end = std::chrono::steady_clock::now(); + const auto propagation_duration_ms = std::chrono::duration_cast(t_propagation_end - t_propagation_start); auto vH = goal_vertex; @@ -361,10 +361,10 @@ uint32_t DijkstraMeshPlanner::dijkstra(const mesh_map::Vector& original_start, c path.push_front(vH); }; - t_end = ros::WallTime::now(); - double execution_time = (t_end - t_start).toNSec() * 1e-6; - RCLCPP_INFO_STREAM(node_->get_logger(), "Execution time (ms): " << execution_time << " for " << mesh.numVertices() - << " num vertices in the mesh."); + const auto t_end = std::chrono::steady_clock::now(); + const auto execution_duration_ms = std::chrono::duration_cast(t_end - t_start); + RCLCPP_INFO_STREAM(node_->get_logger(), "Execution duration (ms): " << execution_duration_ms.count() + << " for " << mesh.numVertices() << " num vertices in the mesh."); computeVectorMap(); @@ -374,13 +374,13 @@ uint32_t DijkstraMeshPlanner::dijkstra(const mesh_map::Vector& original_start, c return mbf_msgs::action::GetPath::Result::CANCELED; } - ros::WallTime t_path_backtracking = ros::WallTime::now(); - double path_backtracking_duration = (t_path_backtracking - t_propagation_end).toNSec() * 1e-6; + const auto t_path_backtracking = std::chrono::steady_clock::now(); + const auto path_backtracking_duration_ms = std::chrono::duration_cast(t_path_backtracking - t_propagation_end); RCLCPP_INFO_STREAM(node_->get_logger(), "Processed " << fixed_set_cnt << " vertices in the fixed set."); - RCLCPP_INFO_STREAM(node_->get_logger(), "Initialization duration (ms): " << initialization_duration); - RCLCPP_INFO_STREAM(node_->get_logger(), "Execution time wavefront propagation (ms): "<< propagation_duration); - RCLCPP_INFO_STREAM(node_->get_logger(), "Path backtracking duration (ms): " << path_backtracking_duration); + RCLCPP_INFO_STREAM(node_->get_logger(), "Initialization duration (ms): " << initialization_duration_ms.count()); + RCLCPP_INFO_STREAM(node_->get_logger(), "Execution time wavefront propagation (ms): " << propagation_duration_ms.count()); + RCLCPP_INFO_STREAM(node_->get_logger(), "Path backtracking duration (ms): " << path_backtracking_duration_ms.count()); RCLCPP_INFO_STREAM(node_->get_logger(), "Successfully finished Dijkstra back tracking!"); return mbf_msgs::action::GetPath::Result::SUCCESS; From c8c549015e0fdfd328bfbd5c44306080dd07d95b Mon Sep 17 00:00:00 2001 From: Matthias Holoch Date: Wed, 24 Jan 2024 11:39:37 +0100 Subject: [PATCH 10/19] fix constructor and destructors --- .../dijkstra_mesh_planner/dijkstra_mesh_planner.h | 1 + dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp | 9 +++------ mbf_mesh_core/include/mbf_mesh_core/mesh_controller.h | 4 ++-- mbf_mesh_core/include/mbf_mesh_core/mesh_planner.h | 8 +++++--- mbf_mesh_core/include/mbf_mesh_core/mesh_recovery.h | 6 ++---- 5 files changed, 13 insertions(+), 15 deletions(-) diff --git a/dijkstra_mesh_planner/include/dijkstra_mesh_planner/dijkstra_mesh_planner.h b/dijkstra_mesh_planner/include/dijkstra_mesh_planner/dijkstra_mesh_planner.h index 7d2ee205..c4d45ab5 100644 --- a/dijkstra_mesh_planner/include/dijkstra_mesh_planner/dijkstra_mesh_planner.h +++ b/dijkstra_mesh_planner/include/dijkstra_mesh_planner/dijkstra_mesh_planner.h @@ -45,6 +45,7 @@ namespace dijkstra_mesh_planner { + class DijkstraMeshPlanner : public mbf_mesh_core::MeshPlanner { public: diff --git a/dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp b/dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp index 5c200ad6..c86978f0 100644 --- a/dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp +++ b/dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp @@ -47,13 +47,10 @@ PLUGINLIB_EXPORT_CLASS(dijkstra_mesh_planner::DijkstraMeshPlanner, mbf_mesh_core namespace dijkstra_mesh_planner { -DijkstraMeshPlanner::DijkstraMeshPlanner() -{ -} -DijkstraMeshPlanner::~DijkstraMeshPlanner() -{ -} +DijkstraMeshPlanner::DijkstraMeshPlanner() {} + +DijkstraMeshPlanner::~DijkstraMeshPlanner() {} uint32_t DijkstraMeshPlanner::makePlan(const geometry_msgs::msg::PoseStamped& start, const geometry_msgs::msg::PoseStamped& goal, double tolerance, std::vector& plan, double& cost, diff --git a/mbf_mesh_core/include/mbf_mesh_core/mesh_controller.h b/mbf_mesh_core/include/mbf_mesh_core/mesh_controller.h index 7ef630f2..02736bab 100644 --- a/mbf_mesh_core/include/mbf_mesh_core/mesh_controller.h +++ b/mbf_mesh_core/include/mbf_mesh_core/mesh_controller.h @@ -53,12 +53,12 @@ class MeshController : public mbf_abstract_core::AbstractController public: typedef std::shared_ptr Ptr; - MeshController() = delete; + MeshController() {}; /** * @brief Destructor */ - virtual ~MeshController(){}; + virtual ~MeshController() {}; /** * @brief Given the current position, orientation, and velocity of the robot, diff --git a/mbf_mesh_core/include/mbf_mesh_core/mesh_planner.h b/mbf_mesh_core/include/mbf_mesh_core/mesh_planner.h index 1be7cbdc..a4aedeb6 100644 --- a/mbf_mesh_core/include/mbf_mesh_core/mesh_planner.h +++ b/mbf_mesh_core/include/mbf_mesh_core/mesh_planner.h @@ -52,12 +52,10 @@ class MeshPlanner : public mbf_abstract_core::AbstractPlanner public: typedef std::shared_ptr Ptr; - MeshPlanner() = delete; - /** * @brief Destructor */ - virtual ~MeshPlanner(){}; + virtual ~MeshPlanner() {}; /** * @brief Given a goal pose in the world, compute a plan @@ -88,7 +86,11 @@ class MeshPlanner : public mbf_abstract_core::AbstractPlanner * @return true if the plugin has been initialized successfully */ virtual bool initialize(const std::string& name, const std::shared_ptr& mesh_map_ptr, const rclcpp::Node::SharedPtr& node) = 0; + +protected: + MeshPlanner() {}; }; + } /* namespace mbf_mesh_core */ #endif /* MESH_MESH_CORE__MESH_PLANNER_H */ diff --git a/mbf_mesh_core/include/mbf_mesh_core/mesh_recovery.h b/mbf_mesh_core/include/mbf_mesh_core/mesh_recovery.h index 1f13746a..7c59b7a4 100644 --- a/mbf_mesh_core/include/mbf_mesh_core/mesh_recovery.h +++ b/mbf_mesh_core/include/mbf_mesh_core/mesh_recovery.h @@ -64,14 +64,12 @@ class MeshRecovery : public mbf_abstract_core::AbstractRecovery */ virtual uint32_t runBehavior(std::string& message) = 0; - MeshRecovery() = delete; + MeshRecovery() {}; /** * @brief Virtual destructor for the interface */ - virtual ~MeshRecovery() - { - } + virtual ~MeshRecovery() {}; /** * @brief Requests the recovery behavior to cancel, e.g. if it takes too much From 79bd475beba7373f0006d1ccf801c95f4a475530 Mon Sep 17 00:00:00 2001 From: Matthias Holoch Date: Wed, 24 Jan 2024 11:41:18 +0100 Subject: [PATCH 11/19] change: build shared object Needed for use in plugins, e.g. dijkstra mesh planner --- mesh_map/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/mesh_map/CMakeLists.txt b/mesh_map/CMakeLists.txt index 2fa72c84..09e143fa 100644 --- a/mesh_map/CMakeLists.txt +++ b/mesh_map/CMakeLists.txt @@ -6,6 +6,7 @@ if(NOT CMAKE_CXX_STANDARD) endif() find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) set(dependencies geometry_msgs LVR2 From f3de363e7d34b64ce53505e7155edbf7fc8833b0 Mon Sep 17 00:00:00 2001 From: Matthias Holoch Date: Wed, 24 Jan 2024 12:18:09 +0100 Subject: [PATCH 12/19] use ament_cmake_ros --- dijkstra_mesh_planner/CMakeLists.txt | 1 - mbf_mesh_core/CMakeLists.txt | 2 +- mbf_mesh_nav/CMakeLists.txt | 2 +- mesh_client/CMakeLists.txt | 2 +- mesh_map/CMakeLists.txt | 5 ----- 5 files changed, 3 insertions(+), 9 deletions(-) diff --git a/dijkstra_mesh_planner/CMakeLists.txt b/dijkstra_mesh_planner/CMakeLists.txt index d62b6cd5..02dddfb9 100644 --- a/dijkstra_mesh_planner/CMakeLists.txt +++ b/dijkstra_mesh_planner/CMakeLists.txt @@ -1,7 +1,6 @@ cmake_minimum_required(VERSION 3.8) project(dijkstra_mesh_planner) -find_package(ament_cmake REQUIRED) find_package(ament_cmake_ros REQUIRED) find_package(mbf_mesh_core REQUIRED) find_package(mbf_msgs REQUIRED) diff --git a/mbf_mesh_core/CMakeLists.txt b/mbf_mesh_core/CMakeLists.txt index 43b0c37f..b9e8233b 100644 --- a/mbf_mesh_core/CMakeLists.txt +++ b/mbf_mesh_core/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(mbf_mesh_core) -find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) set(dependencies mbf_abstract_core mesh_map diff --git a/mbf_mesh_nav/CMakeLists.txt b/mbf_mesh_nav/CMakeLists.txt index b5fc314d..e1e3ddc1 100644 --- a/mbf_mesh_nav/CMakeLists.txt +++ b/mbf_mesh_nav/CMakeLists.txt @@ -5,7 +5,7 @@ if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) endif() -find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) set(dependencies geometry_msgs LVR2 diff --git a/mesh_client/CMakeLists.txt b/mesh_client/CMakeLists.txt index 40881bff..6b4fd04f 100644 --- a/mesh_client/CMakeLists.txt +++ b/mesh_client/CMakeLists.txt @@ -5,7 +5,7 @@ if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) endif() -find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) set(dependencies rclcpp LVR2 diff --git a/mesh_map/CMakeLists.txt b/mesh_map/CMakeLists.txt index 09e143fa..910b2a4a 100644 --- a/mesh_map/CMakeLists.txt +++ b/mesh_map/CMakeLists.txt @@ -1,11 +1,6 @@ cmake_minimum_required(VERSION 3.5) project(mesh_map) -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -find_package(ament_cmake REQUIRED) find_package(ament_cmake_ros REQUIRED) set(dependencies geometry_msgs From b57f5391399215c2b724537aff44d08871489544 Mon Sep 17 00:00:00 2001 From: Matthias Holoch Date: Wed, 24 Jan 2024 12:18:16 +0100 Subject: [PATCH 13/19] add missing linker call --- mesh_client/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/mesh_client/CMakeLists.txt b/mesh_client/CMakeLists.txt index 6b4fd04f..209a3a99 100644 --- a/mesh_client/CMakeLists.txt +++ b/mesh_client/CMakeLists.txt @@ -27,6 +27,7 @@ add_library(${PROJECT_NAME} src/mesh_client.cpp ) ament_target_dependencies(${PROJECT_NAME} ${dependencies}) +target_link_libraries(${PROJECT_NAME} jsoncpp) install(DIRECTORY include/ DESTINATION include From 77bb3878a0589ee9316e471b85f2c4cd578ba3b9 Mon Sep 17 00:00:00 2001 From: Matthias Holoch Date: Wed, 24 Jan 2024 12:20:32 +0100 Subject: [PATCH 14/19] remove pkgconfig --- dijkstra_mesh_planner/CMakeLists.txt | 3 --- 1 file changed, 3 deletions(-) diff --git a/dijkstra_mesh_planner/CMakeLists.txt b/dijkstra_mesh_planner/CMakeLists.txt index 02dddfb9..5ae3e337 100644 --- a/dijkstra_mesh_planner/CMakeLists.txt +++ b/dijkstra_mesh_planner/CMakeLists.txt @@ -10,9 +10,6 @@ find_package(rclcpp REQUIRED) pluginlib_export_plugin_description_file(mbf_mesh_core dijkstra_mesh_planner.xml) -#find_package(PkgConfig REQUIRED) -#pkg_check_modules(JSONCPP jsoncpp) - add_library(${PROJECT_NAME} src/dijkstra_mesh_planner.cpp ) From 4d93f5b56dffbf38abdf3d1aacc9ee9138871910 Mon Sep 17 00:00:00 2001 From: Matthias Holoch Date: Wed, 24 Jan 2024 14:14:36 +0100 Subject: [PATCH 15/19] fix compile defs --- dijkstra_mesh_planner/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dijkstra_mesh_planner/CMakeLists.txt b/dijkstra_mesh_planner/CMakeLists.txt index 5ae3e337..c1bf6c09 100644 --- a/dijkstra_mesh_planner/CMakeLists.txt +++ b/dijkstra_mesh_planner/CMakeLists.txt @@ -17,7 +17,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC $ $) ament_target_dependencies(${PROJECT_NAME} ${dependencies}) -target_compile_definitions(${PROJECT_NAME} PRIVATE "FTC_CONTROLLER_BUILDING_LIBRARY") +target_compile_definitions(${PROJECT_NAME} PRIVATE "DIJKSTRA_MESH_PLANNER_BUILDING_LIBRARY") ament_target_dependencies(${PROJECT_NAME} mbf_mesh_core mbf_msgs mbf_utility mesh_map rclcpp) install(DIRECTORY include/ From c1cca45062f56dd217128cfcafc764c21cbaa204 Mon Sep 17 00:00:00 2001 From: Matthias Holoch Date: Wed, 24 Jan 2024 14:16:00 +0100 Subject: [PATCH 16/19] remove obsolete catkin buildtool dep --- dijkstra_mesh_planner/package.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/dijkstra_mesh_planner/package.xml b/dijkstra_mesh_planner/package.xml index 2fa68f07..8332cefa 100644 --- a/dijkstra_mesh_planner/package.xml +++ b/dijkstra_mesh_planner/package.xml @@ -7,7 +7,6 @@ BSD-3 Sebastian Pütz - catkin rclcpp mbf_mesh_core mbf_utility From b43ea53c91f12d8689beda3c69f309698e8e55ad Mon Sep 17 00:00:00 2001 From: Matthias Holoch Date: Wed, 24 Jan 2024 14:30:21 +0100 Subject: [PATCH 17/19] remove ineffectual line --- dijkstra_mesh_planner/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/dijkstra_mesh_planner/CMakeLists.txt b/dijkstra_mesh_planner/CMakeLists.txt index c1bf6c09..74befb44 100644 --- a/dijkstra_mesh_planner/CMakeLists.txt +++ b/dijkstra_mesh_planner/CMakeLists.txt @@ -16,7 +16,6 @@ add_library(${PROJECT_NAME} target_include_directories(${PROJECT_NAME} PUBLIC $ $) -ament_target_dependencies(${PROJECT_NAME} ${dependencies}) target_compile_definitions(${PROJECT_NAME} PRIVATE "DIJKSTRA_MESH_PLANNER_BUILDING_LIBRARY") ament_target_dependencies(${PROJECT_NAME} mbf_mesh_core mbf_msgs mbf_utility mesh_map rclcpp) From f7e40de7a4f5f94c96a93920f8076000cea83425 Mon Sep 17 00:00:00 2001 From: Matthias Holoch Date: Wed, 24 Jan 2024 14:34:25 +0100 Subject: [PATCH 18/19] remove obsolete cfg file --- dijkstra_mesh_planner/cfg/DijkstraMeshPlanner.cfg | 9 --------- 1 file changed, 9 deletions(-) delete mode 100755 dijkstra_mesh_planner/cfg/DijkstraMeshPlanner.cfg diff --git a/dijkstra_mesh_planner/cfg/DijkstraMeshPlanner.cfg b/dijkstra_mesh_planner/cfg/DijkstraMeshPlanner.cfg deleted file mode 100755 index 4cfcab07..00000000 --- a/dijkstra_mesh_planner/cfg/DijkstraMeshPlanner.cfg +++ /dev/null @@ -1,9 +0,0 @@ -#!/usr/bin/env python - -from dynamic_reconfigure.parameter_generator_catkin import * - -gen = ParameterGenerator() - -gen.add("cost_limit", double_t, 0, "Defines the vertex cost limit with which it can be accessed.", 1.0, 0, 10.0) - -exit(gen.generate("dijkstra_mesh_planner", "dijkstra_mesh_planner", "DijkstraMeshPlanner")) From c0c0927bbe66d7188ad726925f3897ab8f127700 Mon Sep 17 00:00:00 2001 From: Matthias Holoch Date: Wed, 24 Jan 2024 15:27:35 +0100 Subject: [PATCH 19/19] change parameter names to be inside the plugin instance's namespace --- dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp b/dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp index c86978f0..b66edc4a 100644 --- a/dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp +++ b/dijkstra_mesh_planner/src/dijkstra_mesh_planner.cpp @@ -132,9 +132,9 @@ bool DijkstraMeshPlanner::initialize(const std::string& plugin_name, const std:: map_frame_ = mesh_map_->mapFrame(); node_ = node; - config_.publish_vector_field = node_->declare_parameter("publish_vector_field", config_.publish_vector_field); - config_.publish_face_vectors = node_->declare_parameter("publish_face_vectors", config_.publish_face_vectors); - config_.goal_dist_offset = node->declare_parameter("goal_dist_offset", config_.goal_dist_offset); + 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); { // cost limit param rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = "Defines the vertex cost limit with which it can be accessed."; @@ -142,7 +142,7 @@ bool DijkstraMeshPlanner::initialize(const std::string& plugin_name, const std:: range.from_value = 0.0; range.to_value = 10.0; descriptor.floating_point_range.push_back(range); - config_.cost_limit = node->declare_parameter("cost_limit", config_.cost_limit); + config_.cost_limit = node->declare_parameter(name_ + ".cost_limit", config_.cost_limit); } path_pub_ = node_->create_publisher("~/path", rclcpp::QoS(1).transient_local());