From 04c1f8b20781f61efe4055ed133852b09051fe74 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sebastian=20P=C3=BCtz?= Date: Tue, 2 Feb 2021 11:26:57 +0100 Subject: [PATCH 01/11] Update README.md reformulate the README introduction --- README.md | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index 6e6a02e..135a27f 100644 --- a/README.md +++ b/README.md @@ -1,11 +1,8 @@ # Mesh Navigation -The *Mesh Navigation* bundle provides software to perform efficient robot navigation on 2D-manifolds in 3D represented -as triangular meshes. It allows to safely navigate in various complex outdoor environments by using a modular extendable -layerd mesh map. Layers can be loaded as plugins and represent certain geometric or semantic metrics of the terrain. -The layered mesh map is integrated with Move Base Flex (MBF) which provides a universal ROS action interface for path -planning and motion control, as well as for recovery behaviours. Thus, additional planner and controller plugins running -on the layered mesh map are provided. +The *Mesh Navigation* bundle provides software for efficient robot navigation on 2D manifolds, which are represented in 3D as triangle meshes. It enables safe navigation in various complex outdoor environments by using a modularly extensible +layered mesh map. Layers can be loaded as plugins representing specific geometric or semantic metrics of the terrain. This allows the incorporation of obstacles in these complex outdoor environments into path and motion motion planning. +The layered *Mesh Map* is integrated with *Move Base Flex (MBF)*, which provides a universal ROS action interface for path planning, motion control, and for recovery behaviors. We also provide additional planner and controller plugins that run on the layered mesh map. Maintainer: [Sebastian Pütz](mailto:spuetz@uos.de) Author: [Sebastian Pütz](mailto:spuetz@uos.de) From fc51723b1d15ca31043efa2745491c2ca3f394f8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sebastian=20P=C3=BCtz?= Date: Tue, 2 Feb 2021 11:36:18 +0100 Subject: [PATCH 02/11] Update README.md add usage section, wip. --- README.md | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 135a27f..28c8d31 100644 --- a/README.md +++ b/README.md @@ -10,6 +10,7 @@ Author: [Sebastian Pütz](mailto:spuetz@uos.de) * [Publications](#publications) * [Installation](#installation) * [Software Stack](#software-stack) +* [Usage](#usage) * [Mesh Map](#mesh-map) * [Planners](#planners) * [Controllers](#controllers) @@ -112,9 +113,21 @@ The package structure is as follows: - `mesh_client` Is an experimental package to additionally load navigation meshes from a server. +## Usage + +See the **[pluto_robot](https://github.com/uos/pluto_robot)** bundle for example configurations of the mesh navigatoin stack and usage. + +### Mesh map configuratoin + +TODO + +### Planner and Controller configurations + +TODO + ### Path Planning and Motion Control -Use the `MeshGoal` tool to select a goal pose on the shown mesh in RViz. +Use the *MeshGoal* tool from the MeshTools bundle to select a goal pose on the shown mesh in *RViz*. This can be added to the top panel *RViz*. ## Mesh Map From c336d04f0d0d843e6dbc7a72565163fb54d706b9 Mon Sep 17 00:00:00 2001 From: Matthias Holoch Date: Wed, 24 Jan 2024 14:29:49 +0100 Subject: [PATCH 03/11] port pkg information to ROS2 --- cvp_mesh_planner/CMakeLists.txt | 74 +++++++++------------------ cvp_mesh_planner/COLCON_IGNORE | 0 cvp_mesh_planner/cvp_mesh_planner.xml | 2 +- cvp_mesh_planner/package.xml | 9 ++-- 4 files changed, 28 insertions(+), 57 deletions(-) delete mode 100644 cvp_mesh_planner/COLCON_IGNORE diff --git a/cvp_mesh_planner/CMakeLists.txt b/cvp_mesh_planner/CMakeLists.txt index b371164..03eb337 100644 --- a/cvp_mesh_planner/CMakeLists.txt +++ b/cvp_mesh_planner/CMakeLists.txt @@ -1,58 +1,32 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.8) project(cvp_mesh_planner) -set(CMAKE_CXX_STANDARD 14) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) +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/CVPMeshPlanner.cfg -) +pluginlib_export_plugin_description_file(mbf_mesh_core cvp_mesh_planner.xml) -catkin_package( - LIBRARIES cvp_mesh_planner - CATKIN_DEPENDS roscpp mbf_mesh_core mbf_utility mbf_msgs mesh_map dynamic_reconfigure -) - -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - -add_library(${PROJECT_NAME} - src/cvp_mesh_planner.cpp -) - -add_dependencies(${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} - ${PROJECT_NAME}_gencfg -) - -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${JSONCPP_LIBRARIES} -) +add_library(${PROJECT_NAME} src/cvp_mesh_planner.cpp) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $) +target_compile_definitions(${PROJECT_NAME} PRIVATE "CVP_MESH_PLANNER_BUILDING_LIBRARY") +ament_target_dependencies(${PROJECT_NAME} mbf_mesh_core mbf_msgs mbf_utility mesh_map rclcpp) +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 cvp_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/cvp_mesh_planner/COLCON_IGNORE b/cvp_mesh_planner/COLCON_IGNORE deleted file mode 100644 index e69de29..0000000 diff --git a/cvp_mesh_planner/cvp_mesh_planner.xml b/cvp_mesh_planner/cvp_mesh_planner.xml index 098bd76..0b2382e 100644 --- a/cvp_mesh_planner/cvp_mesh_planner.xml +++ b/cvp_mesh_planner/cvp_mesh_planner.xml @@ -1,4 +1,4 @@ - + diff --git a/cvp_mesh_planner/package.xml b/cvp_mesh_planner/package.xml index c67179f..ab2c962 100644 --- a/cvp_mesh_planner/package.xml +++ b/cvp_mesh_planner/package.xml @@ -1,5 +1,5 @@ - + cvp_mesh_planner 1.0.1 The Continuous Vector Field Planner (CVP) mesh planner package @@ -7,16 +7,13 @@ BSD 3-Clause Sebastian Pütz - catkin - - roscpp + rclcpp mbf_mesh_core mbf_utility mbf_msgs mesh_map - dynamic_reconfigure - + ament_cmake From 592c470cafeef2286f3c7edd3121344a05de8582 Mon Sep 17 00:00:00 2001 From: Matthias Holoch Date: Wed, 24 Jan 2024 14:40:27 +0100 Subject: [PATCH 04/11] port includes and function signature to ROS2 --- .../cvp_mesh_planner/cvp_mesh_planner.h | 19 ++++++++++--------- cvp_mesh_planner/src/cvp_mesh_planner.cpp | 14 +++++++------- 2 files changed, 17 insertions(+), 16 deletions(-) diff --git a/cvp_mesh_planner/include/cvp_mesh_planner/cvp_mesh_planner.h b/cvp_mesh_planner/include/cvp_mesh_planner/cvp_mesh_planner.h index 2f62020..ae54a79 100644 --- a/cvp_mesh_planner/include/cvp_mesh_planner/cvp_mesh_planner.h +++ b/cvp_mesh_planner/include/cvp_mesh_planner/cvp_mesh_planner.h @@ -39,17 +39,16 @@ #define MESH_NAVIGATION__MESH_PLANNER_H #include -#include +#include #include -#include -#include +#include namespace cvp_mesh_planner { class CVPMeshPlanner : public mbf_mesh_core::MeshPlanner { public: - typedef boost::shared_ptr Ptr; + typedef std::shared_ptr Ptr; /** * @brief Constructor @@ -71,15 +70,17 @@ class CVPMeshPlanner : public mbf_mesh_core::MeshPlanner * @param message a detailed outcome message * @return result outcome code, see the GetPath action definition */ - 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. * @return true if cancel has been successfully requested, false otherwise */ - virtual bool cancel(); + virtual bool cancel() override; /** * @brief Initializes the planner plugin with a user configured name and a shared pointer to the mesh map @@ -87,7 +88,7 @@ class CVPMeshPlanner : public mbf_mesh_core::MeshPlanner * @param mesh_map_ptr A shared pointer to the mesh map instance to access attributes and helper functions, etc. * @return true if the plugin has been initialized successfully */ - virtual bool initialize(const std::string& name, const boost::shared_ptr& mesh_map_ptr); + virtual bool initialize(const std::string& plugin_name, const std::shared_ptr& mesh_map_ptr, const rclcpp::Node::SharedPtr& node) override; protected: diff --git a/cvp_mesh_planner/src/cvp_mesh_planner.cpp b/cvp_mesh_planner/src/cvp_mesh_planner.cpp index 38f2bf7..e82cb65 100644 --- a/cvp_mesh_planner/src/cvp_mesh_planner.cpp +++ b/cvp_mesh_planner/src/cvp_mesh_planner.cpp @@ -38,9 +38,8 @@ #include #include -#include #include -#include +#include #include "cvp_mesh_planner/cvp_mesh_planner.h" //#define DEBUG @@ -59,9 +58,11 @@ CVPMeshPlanner::~CVPMeshPlanner() { } -uint32_t CVPMeshPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, - double tolerance, std::vector& plan, double& cost, - std::string& message) +uint32_t CVPMeshPlanner::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; @@ -130,8 +131,7 @@ bool CVPMeshPlanner::cancel() return true; } -bool CVPMeshPlanner::initialize(const std::string& plugin_name, - const boost::shared_ptr& mesh_map_ptr) +bool CVPMeshPlanner::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; From 54279d9b4cf2d93465db98b484fbf1f3397d80ff Mon Sep 17 00:00:00 2001 From: Matthias Holoch Date: Wed, 24 Jan 2024 14:46:01 +0100 Subject: [PATCH 05/11] refactor: add trailing underscore to class members --- .../cvp_mesh_planner/cvp_mesh_planner.h | 52 +++--- cvp_mesh_planner/src/cvp_mesh_planner.cpp | 172 +++++++++--------- 2 files changed, 107 insertions(+), 117 deletions(-) diff --git a/cvp_mesh_planner/include/cvp_mesh_planner/cvp_mesh_planner.h b/cvp_mesh_planner/include/cvp_mesh_planner/cvp_mesh_planner.h index ae54a79..645e514 100644 --- a/cvp_mesh_planner/include/cvp_mesh_planner/cvp_mesh_planner.h +++ b/cvp_mesh_planner/include/cvp_mesh_planner/cvp_mesh_planner.h @@ -170,58 +170,48 @@ class CVPMeshPlanner : public mbf_mesh_core::MeshPlanner private: //! shared pointer to the mesh map - mesh_map::MeshMap::Ptr mesh_map; + mesh_map::MeshMap::Ptr mesh_map_; //! the user defined plugin name - std::string name; + std::string name_; //! the private node handle with the user defined namespace (name) ros::NodeHandle private_nh; //! flag if cancel has been requested - std::atomic_bool cancel_planning; + std::atomic_bool cancel_planning_; //! publisher for the backtracked path - ros::Publisher path_pub; - - //! whether to publish the vector field or not - bool publish_vector_field; - - //! whether to also publish direction vectors at the triangle centers - bool publish_face_vectors; + ros::Publisher path_pub_; //! the map coordinate frame / system id - std::string map_frame; - - //! an offset that determines how far beyond the goal (robot's position) is propagated. - float goal_dist_offset; - - //! shared pointer to dynamic reconfigure server - boost::shared_ptr> reconfigure_server_ptr; - - //! dynamic reconfigure callback function binding - dynamic_reconfigure::Server::CallbackType config_callback; - - //! indicates if dynamic reconfigure has been called the first time - bool first_config; - - //! the current dynamic reconfigure planner configuration - CVPMeshPlannerConfig config; + std::string map_frame_; + + // handle of callback for changing parameters dynamically + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr reconfiguration_callback_handle_; + struct { + //! whether to publish the vector field or not + bool publish_vector_field = false; + //! whether to also publish direction vectors at the triangle centers + bool publish_face_vectors; + //! an offset that determines how far beyond the goal (robot's position) is propagated. + float goal_dist_offset; + } config_; //! theta angles to the source of the wave front propagation - lvr2::DenseVertexMap direction; + lvr2::DenseVertexMap direction_; //! predecessors while wave propagation - lvr2::DenseVertexMap predecessors; + lvr2::DenseVertexMap predecessors_; //! the face which is cut by the computed line to the source - lvr2::DenseVertexMap cutting_faces; + lvr2::DenseVertexMap cutting_faces_; //! stores the current vector map containing vectors pointing to the seed - lvr2::DenseVertexMap vector_map; + lvr2::DenseVertexMap vector_map_; //! potential field / scalar distance field to the seed - lvr2::DenseVertexMap potential; + lvr2::DenseVertexMap potential_; }; } // namespace cvp_mesh_planner diff --git a/cvp_mesh_planner/src/cvp_mesh_planner.cpp b/cvp_mesh_planner/src/cvp_mesh_planner.cpp index e82cb65..587b183 100644 --- a/cvp_mesh_planner/src/cvp_mesh_planner.cpp +++ b/cvp_mesh_planner/src/cvp_mesh_planner.cpp @@ -64,7 +64,7 @@ uint32_t CVPMeshPlanner::makePlan(const geometry_msgs::msg::PoseStamped& start, std::vector& plan, double& cost, std::string& message) { - const auto& mesh = mesh_map->mesh(); + const auto& mesh = mesh_map_->mesh(); std::list> path; // mesh_map->combineVertexCosts(); // TODO should be outside the planner @@ -80,7 +80,7 @@ uint32_t CVPMeshPlanner::makePlan(const geometry_msgs::msg::PoseStamped& start, std_msgs::Header header; header.stamp = ros::Time::now(); - header.frame_id = mesh_map->mapFrame(); + header.frame_id = mesh_map_->mapFrame(); cost = 0; float dir_length; @@ -90,7 +90,7 @@ uint32_t CVPMeshPlanner::makePlan(const geometry_msgs::msg::PoseStamped& start, lvr2::FaceHandle fH = path.front().second; path.pop_front(); - const auto& face_normals = mesh_map->faceNormals(); + const auto& face_normals = mesh_map_->faceNormals(); for (auto& next : path) { geometry_msgs::PoseStamped pose; @@ -113,13 +113,13 @@ uint32_t CVPMeshPlanner::makePlan(const geometry_msgs::msg::PoseStamped& start, 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) { - mesh_map->publishVectorField("vector_field", vector_map, publish_face_vectors); + mesh_map_->publishVectorField("vector_field", vector_map_, publish_face_vectors); } return outcome; @@ -127,24 +127,24 @@ uint32_t CVPMeshPlanner::makePlan(const geometry_msgs::msg::PoseStamped& start, bool CVPMeshPlanner::cancel() { - cancel_planning = true; + cancel_planning_ = true; return true; } bool CVPMeshPlanner::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(); + private_nh = ros::NodeHandle("~/" + name_); 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(); - direction = lvr2::DenseVertexMap(mesh.nextVertexIndex(), 0); + path_pub_ = private_nh.advertise("path", 1, true); + const auto& mesh = mesh_map_->mesh(); + direction_ = lvr2::DenseVertexMap(mesh.nextVertexIndex(), 0); // TODO check all map dependencies! (loaded layers etc...) reconfigure_server_ptr = boost::shared_ptr>( @@ -170,23 +170,23 @@ void CVPMeshPlanner::reconfigureCallback(cvp_mesh_planner::CVPMeshPlannerConfig& void CVPMeshPlanner::computeVectorMap() { - const auto& mesh = mesh_map->mesh(); - const auto& face_normals = mesh_map->faceNormals(); - const auto& vertex_normals = mesh_map->vertexNormals(); + const auto& mesh = mesh_map_->mesh(); + const auto& face_normals = mesh_map_->faceNormals(); + const auto& vertex_normals = mesh_map_->vertexNormals(); for (auto v3 : mesh.vertices()) { // if(vertex_costs[v3] > config.cost_limit || !predecessors.containsKey(v3)) // continue; - 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; // get the cut face - const auto& optFh = cutting_faces.get(v3); + const auto& optFh = cutting_faces_.get(v3); // if no cut face, continue with the next vertex if (!optFh) continue; @@ -198,18 +198,18 @@ void CVPMeshPlanner::computeVectorMap() // compute the direction vector and rotate it by theta, which is stored in // the direction vertex map - const auto dirVec = (vec1 - vec3).rotated(vertex_normals[v3], direction[v3]); + const auto dirVec = (vec1 - vec3).rotated(vertex_normals[v3], direction_[v3]); // 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 CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& start, const mesh_map::Vector& goal, std::list>& path) { - return waveFrontPropagation(start, goal, mesh_map->edgeDistances(), mesh_map->vertexCosts(), path, potential, - predecessors); + return waveFrontPropagation(start, goal, mesh_map_->edgeDistances(), mesh_map_->vertexCosts(), path, potential_, + predecessors_); } inline bool CVPMeshPlanner::waveFrontUpdateWithS(lvr2::DenseVertexMap& distances, @@ -217,7 +217,7 @@ inline bool CVPMeshPlanner::waveFrontUpdateWithS(lvr2::DenseVertexMap& di const lvr2::VertexHandle& v1, const lvr2::VertexHandle& v2, const lvr2::VertexHandle& v3) { - const auto& mesh = mesh_map->mesh(); + const auto& mesh = mesh_map_->mesh(); const double u1 = distances[v1]; const double u2 = distances[v2]; @@ -261,11 +261,11 @@ inline bool CVPMeshPlanner::waveFrontUpdateWithS(lvr2::DenseVertexMap& di if (S <= 0 && std::fabs(t1cos) <= 1) { const double theta = acos(t1cos); - predecessors[v3] = v1; - direction[v3] = static_cast(theta); + predecessors_[v3] = v1; + direction_[v3] = static_cast(theta); distances[v3] = static_cast(u3tmp); const lvr2::FaceHandle fh = mesh.getFaceBetween(v1, v2, v3).unwrap(); - cutting_faces.insert(v3, fh); + cutting_faces_.insert(v3, fh); #ifdef DEBUG mesh_map->publishDebugVector(v3, v1, fh, theta, mesh_map::color(0.9, 0.9, 0.2), "dir_vec" + std::to_string(v3.idx())); @@ -277,11 +277,11 @@ inline bool CVPMeshPlanner::waveFrontUpdateWithS(lvr2::DenseVertexMap& di u3tmp = u1 + b; if (u3tmp < u3) { - predecessors[v3] = v1; - direction[v3] = 0; + predecessors_[v3] = v1; + direction_[v3] = 0; distances[v3] = u3tmp; const lvr2::FaceHandle fh = mesh.getFaceBetween(v1, v2, v3).unwrap(); - cutting_faces.insert(v3, fh); + cutting_faces_.insert(v3, fh); #ifdef DEBUG mesh_map->publishDebugVector(v3, v1, fh, 0, mesh_map::color(0.9, 0.9, 0.2), "dir_vec" + std::to_string(v3.idx())); @@ -299,10 +299,10 @@ inline bool CVPMeshPlanner::waveFrontUpdateWithS(lvr2::DenseVertexMap& di { const lvr2::FaceHandle fh = mesh.getFaceBetween(v1, v2, v3).unwrap(); const double theta = -acos(t2cos); - direction[v3] = static_cast(theta); + direction_[v3] = static_cast(theta); distances[v3] = static_cast(u3tmp); - predecessors[v3] = v2; - cutting_faces.insert(v3, fh); + predecessors_[v3] = v2; + cutting_faces_.insert(v3, fh); #ifdef DEBUG mesh_map->publishDebugVector(v3, v2, fh, theta, mesh_map::color(0.9, 0.9, 0.2), "dir_vec" + std::to_string(v3.idx())); @@ -314,11 +314,11 @@ inline bool CVPMeshPlanner::waveFrontUpdateWithS(lvr2::DenseVertexMap& di u3tmp = u2 + a; if (u3tmp < u3) { - direction[v3] = 0; + direction_[v3] = 0; distances[v3] = u3tmp; - predecessors[v3] = v2; + predecessors_[v3] = v2; const lvr2::FaceHandle fh = mesh.getFaceBetween(v1, v2, v3).unwrap(); - cutting_faces.insert(v3, fh); + cutting_faces_.insert(v3, fh); #ifdef DEBUG mesh_map->publishDebugVector(v3, v2, fh, 0, mesh_map::color(0.9, 0.9, 0.2), "dir_vec" + std::to_string(v3.idx())); @@ -337,7 +337,7 @@ inline bool CVPMeshPlanner::waveFrontUpdate(lvr2::DenseVertexMap& distanc const lvr2::VertexHandle& v1, const lvr2::VertexHandle& v2, const lvr2::VertexHandle& v3) { - const auto& mesh = mesh_map->mesh(); + const auto& mesh = mesh_map_->mesh(); const double u1 = distances[v1]; const double u2 = distances[v2]; @@ -387,14 +387,14 @@ inline bool CVPMeshPlanner::waveFrontUpdate(lvr2::DenseVertexMap& distanc if (u3tmp < u3) { auto fH = mesh.getFaceBetween(v1, v2, v3).unwrap(); - cutting_faces.insert(v3, fH); - predecessors[v3] = v1; + cutting_faces_.insert(v3, fH); + predecessors_[v3] = v1; #ifdef DEBUG mesh_map->publishDebugVector(v3, v1, fH, 0, mesh_map::color(0.9, 0.9, 0.2), "dir_vec" + std::to_string(v3.idx())); #endif distances[v3] = static_cast(u3tmp); - direction[v3] = 0; + direction_[v3] = 0; return true; } return false; @@ -406,14 +406,14 @@ inline bool CVPMeshPlanner::waveFrontUpdate(lvr2::DenseVertexMap& distanc if (u3tmp < u3) { auto fH = mesh.getFaceBetween(v1, v2, v3).unwrap(); - cutting_faces.insert(v3, fH); - predecessors[v3] = v2; + cutting_faces_.insert(v3, fH); + predecessors_[v3] = v2; #ifdef DEBUG mesh_map->publishDebugVector(v3, v2, fH, 0, mesh_map::color(0.9, 0.9, 0.2), "dir_vec" + std::to_string(v3.idx())); #endif distances[v3] = static_cast(u3tmp); - direction[v3] = 0; + direction_[v3] = 0; return true; } return false; @@ -459,12 +459,12 @@ inline bool CVPMeshPlanner::waveFrontUpdate(lvr2::DenseVertexMap& distanc if (theta1 < theta0 && theta2 < theta0) { auto fH = mesh.getFaceBetween(v1, v2, v3).unwrap(); - cutting_faces.insert(v3, fH); + cutting_faces_.insert(v3, fH); distances[v3] = static_cast(u3tmp); if (theta1 < theta2) { - predecessors[v3] = v1; - direction[v3] = theta1; + predecessors_[v3] = v1; + direction_[v3] = theta1; #ifdef DEBUG mesh_map->publishDebugVector(v3, v1, fH, theta1, mesh_map::color(0.9, 0.9, 0.2), "dir_vec" + std::to_string(v3.idx())); @@ -472,8 +472,8 @@ inline bool CVPMeshPlanner::waveFrontUpdate(lvr2::DenseVertexMap& distanc } else { - predecessors[v3] = v2; - direction[v3] = -theta2; + predecessors_[v3] = v2; + direction_[v3] = -theta2; #ifdef DEBUG mesh_map->publishDebugVector(v3, v2, fH, -theta2, mesh_map::color(0.9, 0.9, 0.2), "dir_vec" + std::to_string(v3.idx())); @@ -487,14 +487,14 @@ inline bool CVPMeshPlanner::waveFrontUpdate(lvr2::DenseVertexMap& distanc if (u3tmp < u3) { auto fH = mesh.getFaceBetween(v1, v2, v3).unwrap(); - cutting_faces.insert(v3, fH); - predecessors[v3] = v1; + cutting_faces_.insert(v3, fH); + predecessors_[v3] = v1; distances[v3] = static_cast(u3tmp); #ifdef DEBUG mesh_map->publishDebugVector(v3, v1, fH, 0, mesh_map::color(0.9, 0.9, 0.2), "dir_vec" + std::to_string(v3.idx())); #endif - direction[v3] = 0; + direction_[v3] = 0; return true; } return false; @@ -505,14 +505,14 @@ inline bool CVPMeshPlanner::waveFrontUpdate(lvr2::DenseVertexMap& distanc if (u3tmp < u3) { auto fH = mesh.getFaceBetween(v1, v2, v3).unwrap(); - cutting_faces.insert(v3, fH); - predecessors[v3] = v2; + cutting_faces_.insert(v3, fH); + predecessors_[v3] = v2; distances[v3] = static_cast(u3tmp); #ifdef DEBUG mesh_map->publishDebugVector(v3, v2, fH, 0, mesh_map::color(0.9, 0.9, 0.2), "dir_vec" + std::to_string(v3.idx())); #endif - direction[v3] = 0; + direction_[v3] = 0; return true; } return false; @@ -529,7 +529,7 @@ inline bool CVPMeshPlanner::waveFrontUpdateFMM( const lvr2::VertexHandle &v2tmp, const lvr2::VertexHandle &v3) { - const auto& mesh = mesh_map->mesh(); + const auto& mesh = mesh_map_->mesh(); bool v1_smaller = distances[v1tmp] < distances[v2tmp]; const lvr2::VertexHandle v1 = v1_smaller ? v1tmp : v2tmp; @@ -577,12 +577,12 @@ inline bool CVPMeshPlanner::waveFrontUpdateFMM( if(u3_tmp < u3) { auto fH = mesh.getFaceBetween(v1, v2, v3).unwrap(); - cutting_faces.insert(v3, fH); - predecessors[v3] = v1; + cutting_faces_.insert(v3, fH); + predecessors_[v3] = v1; distances[v3] = static_cast(u3_tmp); const double theta = acos(cos_theta); const double phi = asin((e * sin(theta)) / sqrt(a_sq * e * e - 2 * a * cos_theta)); - direction[v3] = theta + phi - M_PI_2; + direction_[v3] = theta + phi - M_PI_2; return true; } } @@ -593,19 +593,19 @@ inline bool CVPMeshPlanner::waveFrontUpdateFMM( if (u1t < u2t) { if (u1t < u3) { auto fH = mesh.getFaceBetween(v1, v2, v3).unwrap(); - cutting_faces.insert(v3, fH); - predecessors[v3] = v1; + cutting_faces_.insert(v3, fH); + predecessors_[v3] = v1; distances[v3] = static_cast(u1t); - direction[v3] = 0; + direction_[v3] = 0; return true; } } else { if (u2t < u3) { auto fH = mesh.getFaceBetween(v1, v2, v3).unwrap(); - cutting_faces.insert(v3, fH); - predecessors[v3] = v2; + cutting_faces_.insert(v3, fH); + predecessors_[v3] = v2; distances[v3] = static_cast(u2t); - direction[v3] = 0; + direction_[v3] = 0; return true; } } @@ -624,24 +624,24 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s { ROS_DEBUG_STREAM("Init wave front propagation."); - const auto& mesh = mesh_map->mesh(); - const auto& vertex_costs = mesh_map->vertexCosts(); - auto& invalid = mesh_map->invalid; + const auto& mesh = mesh_map_->mesh(); + const auto& vertex_costs = mesh_map_->vertexCosts(); + 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"); mesh_map::Vector start = original_start; mesh_map::Vector goal = original_goal; // Find the containing faces of start and goal - const auto& start_opt = mesh_map->getContainingFace(start, 0.4); - const auto& goal_opt = mesh_map->getContainingFace(goal, 0.4); + const auto& start_opt = mesh_map_->getContainingFace(start, 0.4); + const auto& goal_opt = mesh_map_->getContainingFace(goal, 0.4); ros::WallTime t_initialization_start = ros::WallTime::now(); // reset cancel planning - cancel_planning = false; + cancel_planning_ = false; if (!start_opt) return mbf_msgs::GetPathResult::INVALID_START; @@ -651,8 +651,8 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s const auto& start_face = start_opt.unwrap(); const auto& goal_face = goal_opt.unwrap(); - mesh_map->publishDebugFace(start_face, mesh_map::color(0, 0, 1), "start_face"); - mesh_map->publishDebugFace(goal_face, mesh_map::color(0, 1, 0), "goal_face"); + mesh_map_->publishDebugFace(start_face, mesh_map::color(0, 0, 1), "start_face"); + mesh_map_->publishDebugFace(goal_face, mesh_map::color(0, 1, 0), "goal_face"); path.clear(); distances.clear(); @@ -666,7 +666,7 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s lvr2::DenseVertexMap fixed(mesh.nextVertexIndex(), false); // clear vector field map - vector_map.clear(); + vector_map_.clear(); // initialize distances with infinity // initialize predecessor of each vertex with itself @@ -684,8 +684,8 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s const mesh_map::Vector diff = start - mesh.getVertexPosition(vH); const float dist = diff.length(); distances[vH] = dist; - vector_map.insert(vH, diff); - cutting_faces.insert(vH, start_face); + vector_map_.insert(vH, diff); + cutting_faces_.insert(vH, start_face); fixed[vH] = true; pq.insert(vH, dist); } @@ -694,9 +694,9 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s ROS_DEBUG_STREAM("The goal is at (" << goal.x << ", " << goal.y << ", " << goal.z << ") at the face (" << goal_vertices[0] << ", " << goal_vertices[1] << ", " << goal_vertices[2] << ")"); - mesh_map->publishDebugPoint(mesh.getVertexPosition(goal_vertices[0]), mesh_map::color(0, 0, 1), "goal_face_v1"); - mesh_map->publishDebugPoint(mesh.getVertexPosition(goal_vertices[1]), mesh_map::color(0, 0, 1), "goal_face_v2"); - mesh_map->publishDebugPoint(mesh.getVertexPosition(goal_vertices[2]), mesh_map::color(0, 0, 1), "goal_face_v3"); + mesh_map_->publishDebugPoint(mesh.getVertexPosition(goal_vertices[0]), mesh_map::color(0, 0, 1), "goal_face_v1"); + mesh_map_->publishDebugPoint(mesh.getVertexPosition(goal_vertices[1]), mesh_map::color(0, 0, 1), "goal_face_v2"); + mesh_map_->publishDebugPoint(mesh.getVertexPosition(goal_vertices[2]), mesh_map::color(0, 0, 1), "goal_face_v3"); float goal_dist = std::numeric_limits::infinity(); @@ -707,7 +707,7 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s ros::WallTime t_wavefront_start = ros::WallTime::now(); double initialization_duration = (t_wavefront_start - t_initialization_start).toNSec() * 1e-6; - while (!pq.isEmpty() && !cancel_planning) + while (!pq.isEmpty() && !cancel_planning_) { lvr2::VertexHandle current_vh = pq.popMin().key(); @@ -833,7 +833,7 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s } } - if (cancel_planning) + if (cancel_planning_) { ROS_WARN_STREAM("Wave front propagation has been canceled!"); return mbf_msgs::GetPathResult::CANCELED; @@ -870,13 +870,13 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s path.push_front(std::pair(current_pos, current_face)); // move from the goal position towards the start position - while (current_pos.distance2(start) > config.step_width && !cancel_planning) + while (current_pos.distance2(start) > config.step_width && !cancel_planning_) { // move current pos ahead on the surface following the vector field, // updates the current face if necessary try { - if (mesh_map->meshAhead(current_pos, current_face, config.step_width)) + if (mesh_map_->meshAhead(current_pos, current_face, config.step_width)) { path.push_front(std::pair(current_pos, current_face)); } @@ -903,7 +903,7 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s ROS_INFO_STREAM("Vector field post computation (ms): " << vector_field_duration); ROS_INFO_STREAM("Path backtracking duration (ms): " << path_backtracking_duration); - if (cancel_planning) + if (cancel_planning_) { ROS_WARN_STREAM("Wave front propagation has been canceled!"); return mbf_msgs::GetPathResult::CANCELED; From bff8ef2fe65175ad7fece792d925234e0b83357a Mon Sep 17 00:00:00 2001 From: Matthias Holoch Date: Wed, 24 Jan 2024 15:10:21 +0100 Subject: [PATCH 06/11] change param declaration to fit ROS2 --- cvp_mesh_planner/cfg/CVPMeshPlanner.cfg | 10 ------- .../cvp_mesh_planner/cvp_mesh_planner.h | 26 +++++++++++----- cvp_mesh_planner/src/cvp_mesh_planner.cpp | 30 +++++++++++++++---- 3 files changed, 43 insertions(+), 23 deletions(-) delete mode 100755 cvp_mesh_planner/cfg/CVPMeshPlanner.cfg diff --git a/cvp_mesh_planner/cfg/CVPMeshPlanner.cfg b/cvp_mesh_planner/cfg/CVPMeshPlanner.cfg deleted file mode 100755 index a446e22..0000000 --- a/cvp_mesh_planner/cfg/CVPMeshPlanner.cfg +++ /dev/null @@ -1,10 +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) -gen.add("step_width", double_t, 0, "The vector field back tracking step width.", 0.4, 0.01, 1.0) - -exit(gen.generate("cvp_mesh_planner", "cvp_mesh_planner", "CVPMeshPlanner")) diff --git a/cvp_mesh_planner/include/cvp_mesh_planner/cvp_mesh_planner.h b/cvp_mesh_planner/include/cvp_mesh_planner/cvp_mesh_planner.h index 645e514..69a613d 100644 --- a/cvp_mesh_planner/include/cvp_mesh_planner/cvp_mesh_planner.h +++ b/cvp_mesh_planner/include/cvp_mesh_planner/cvp_mesh_planner.h @@ -42,6 +42,7 @@ #include #include #include +#include namespace cvp_mesh_planner { @@ -163,9 +164,16 @@ class CVPMeshPlanner : public mbf_mesh_core::MeshPlanner void computeVectorMap(); /** - * @brief Dynamic reconfigure callback + * @brief gets called on new incoming reconfigure parameters + * + * @param cfg new configuration + * @param level level + * @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 cvp mesh planner */ - void reconfigureCallback(cvp_mesh_planner::CVPMeshPlannerConfig& cfg, uint32_t level); + rcl_interfaces::msg::SetParametersResult reconfigureCallback(std::vector parameters); private: @@ -175,14 +183,14 @@ class CVPMeshPlanner : public mbf_mesh_core::MeshPlanner //! the user defined plugin name std::string name_; - //! the private node handle with the user defined namespace (name) - ros::NodeHandle private_nh; + //! pointer to the node in which this plugin is running + rclcpp::Node::SharedPtr node_; //! flag if cancel has been requested std::atomic_bool cancel_planning_; //! publisher for the backtracked path - ros::Publisher path_pub_; + rclcpp::Publisher::SharedPtr path_pub_; //! the map coordinate frame / system id std::string map_frame_; @@ -193,9 +201,13 @@ class CVPMeshPlanner : public mbf_mesh_core::MeshPlanner //! whether to publish the vector field or not bool publish_vector_field = false; //! whether to also publish direction vectors at the triangle centers - bool publish_face_vectors; + bool publish_face_vectors = false; //! an offset that determines how far beyond the goal (robot's position) is propagated. - float goal_dist_offset; + double goal_dist_offset = 0.3; + //! Defines the vertex cost limit with which it can be accessed. + double cost_limit = 1.0; + //! The vector field back tracking step width. + double step_width = 0.4; } config_; //! theta angles to the source of the wave front propagation diff --git a/cvp_mesh_planner/src/cvp_mesh_planner.cpp b/cvp_mesh_planner/src/cvp_mesh_planner.cpp index 587b183..196acda 100644 --- a/cvp_mesh_planner/src/cvp_mesh_planner.cpp +++ b/cvp_mesh_planner/src/cvp_mesh_planner.cpp @@ -136,13 +136,31 @@ bool CVPMeshPlanner::initialize(const std::string& plugin_name, const std::share mesh_map_ = mesh_map_ptr; name_ = plugin_name; map_frame_ = mesh_map_->mapFrame(); - private_nh = ros::NodeHandle("~/" + name_); - - 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); + 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); + { // 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); + } + { // step width param + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = "The vector field back tracking step width."; + rcl_interfaces::msg::FloatingPointRange range; + range.from_value = 0.01; + range.to_value = 1.0; + descriptor.floating_point_range.push_back(range); + config_.step_width = node->declare_parameter("step_width", config_.step_width); + } - 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(); direction_ = lvr2::DenseVertexMap(mesh.nextVertexIndex(), 0); // TODO check all map dependencies! (loaded layers etc...) From 60417cd181411f2dfd15315383448adf30eebcd2 Mon Sep 17 00:00:00 2001 From: Matthias Holoch Date: Wed, 24 Jan 2024 15:10:51 +0100 Subject: [PATCH 07/11] fix typo in docs --- 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 ddfc30e..dcff269 100644 --- a/mesh_map/include/mesh_map/mesh_map.h +++ b/mesh_map/include/mesh_map/mesh_map.h @@ -72,7 +72,7 @@ class MeshMap bool readMap(); /** - * @brief Loads all configures layer plugins + * @brief Loads all configured layer plugins * @return true if the layer plugins have been load successfully. */ bool loadLayerPlugins(); From fff9871693de7d0670c13b5575028f1f87321990 Mon Sep 17 00:00:00 2001 From: Matthias Holoch Date: Wed, 24 Jan 2024 15:16:10 +0100 Subject: [PATCH 08/11] change logging and reconfigure callback to work with ROS2 --- cvp_mesh_planner/src/cvp_mesh_planner.cpp | 121 +++++++++++----------- 1 file changed, 61 insertions(+), 60 deletions(-) diff --git a/cvp_mesh_planner/src/cvp_mesh_planner.cpp b/cvp_mesh_planner/src/cvp_mesh_planner.cpp index 196acda..2f19e55 100644 --- a/cvp_mesh_planner/src/cvp_mesh_planner.cpp +++ b/cvp_mesh_planner/src/cvp_mesh_planner.cpp @@ -69,7 +69,7 @@ uint32_t CVPMeshPlanner::makePlan(const geometry_msgs::msg::PoseStamped& start, // mesh_map->combineVertexCosts(); // TODO should be outside the planner - ROS_INFO("start wave front propagation."); + RCLCPP_INFO(node_->get_logger(), "start wave front propagation."); mesh_map::Vector goal_vec = mesh_map::toVector(goal.pose.position); mesh_map::Vector start_vec = mesh_map::toVector(start.pose.position); @@ -115,7 +115,7 @@ uint32_t CVPMeshPlanner::makePlan(const geometry_msgs::msg::PoseStamped& start, 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) { @@ -165,25 +165,26 @@ bool CVPMeshPlanner::initialize(const std::string& plugin_name, const std::share direction_ = lvr2::DenseVertexMap(mesh.nextVertexIndex(), 0); // TODO check all map dependencies! (loaded layers etc...) - reconfigure_server_ptr = boost::shared_ptr>( - new dynamic_reconfigure::Server(private_nh)); - - config_callback = boost::bind(&CVPMeshPlanner::reconfigureCallback, this, _1, _2); - reconfigure_server_ptr->setCallback(config_callback); + reconfiguration_callback_handle_ = node_->add_on_set_parameters_callback(std::bind( + &CVPMeshPlanner::reconfigureCallback, this, std::placeholders::_1)); return true; } -void CVPMeshPlanner::reconfigureCallback(cvp_mesh_planner::CVPMeshPlannerConfig& cfg, uint32_t level) +rcl_interfaces::msg::SetParametersResult CVPMeshPlanner::reconfigureCallback(std::vector parameters) { - ROS_INFO_STREAM("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(); + } else if (parameter.get_name() == name_ + ".step_width") { + config_.step_width = parameter.as_double(); + } } - config = cfg; + + result.successful = true; + return result; } void CVPMeshPlanner::computeVectorMap() @@ -390,7 +391,7 @@ inline bool CVPMeshPlanner::waveFrontUpdate(lvr2::DenseVertexMap& distanc if (!std::isfinite(u3tmp)) { - ROS_ERROR_STREAM("u3 tmp is not finite!"); + RCLCPP_ERROR_STREAM(node_->get_logger(), "u3 tmp is not finite!"); } if (u3tmp < u3) { @@ -444,32 +445,32 @@ inline bool CVPMeshPlanner::waveFrontUpdate(lvr2::DenseVertexMap& distanc #ifdef DEBUG if (!std::isfinite(theta0 + theta1 + theta2)) { - ROS_ERROR_STREAM("------------------"); + RCLCPP_ERROR_STREAM(node_->get_logger(), "------------------"); if (std::isnan(theta0)) - ROS_ERROR_STREAM("Theta0 is NaN!"); + RCLCPP_ERROR_STREAM(node_->get_logger(), "Theta0 is NaN!"); if (std::isnan(theta1)) - ROS_ERROR_STREAM("Theta1 is NaN!"); + RCLCPP_ERROR_STREAM(node_->get_logger(), "Theta1 is NaN!"); if (std::isnan(theta2)) - ROS_ERROR_STREAM("Theta2 is NaN!"); + RCLCPP_ERROR_STREAM(node_->get_logger(), "Theta2 is NaN!"); if (std::isinf(theta2)) - ROS_ERROR_STREAM("Theta2 is inf!"); + RCLCPP_ERROR_STREAM(node_->get_logger(), "Theta2 is inf!"); if (std::isinf(theta2)) - ROS_ERROR_STREAM("Theta2 is inf!"); + RCLCPP_ERROR_STREAM(node_->get_logger(), "Theta2 is inf!"); if (std::isinf(theta2)) - ROS_ERROR_STREAM("Theta2 is inf!"); + RCLCPP_ERROR_STREAM(node_->get_logger(), "Theta2 is inf!"); if (std::isnan(t1a)) - ROS_ERROR_STREAM("t1a is NaN!"); + RCLCPP_ERROR_STREAM(node_->get_logger(), "t1a is NaN!"); if (std::isnan(t2a)) - ROS_ERROR_STREAM("t2a is NaN!"); + RCLCPP_ERROR_STREAM(node_->get_logger(), "t2a is NaN!"); if (std::fabs(t2a) > 1) { - ROS_ERROR_STREAM("|t2a| is > 1: " << t2a); - ROS_INFO_STREAM("a: " << a << ", u3: " << u3tmp << ", u2: " << u2 << ", a+u2: " << a + u2); + RCLCPP_ERROR_STREAM(node_->get_logger(), "|t2a| is > 1: " << t2a); + RCLCPP_INFO_STREAM(node_->get_logger(), "a: " << a << ", u3: " << u3tmp << ", u2: " << u2 << ", a+u2: " << a + u2); } if (std::fabs(t1a) > 1) { - ROS_ERROR_STREAM("|t1a| is > 1: " << t1a); - ROS_INFO_STREAM("b: " << b << ", u3: " << u3tmp << ", u1: " << u1 << ", b+u1: " << b + u1); + RCLCPP_ERROR_STREAM(node_->get_logger(), "|t1a| is > 1: " << t1a); + RCLCPP_INFO_STREAM(node_->get_logger(), "b: " << b << ", u3: " << u3tmp << ", u1: " << u1 << ", b+u1: " << b + u1); } } #endif @@ -640,7 +641,7 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s lvr2::DenseVertexMap& distances, lvr2::DenseVertexMap& predecessors) { - ROS_DEBUG_STREAM("Init wave front propagation."); + RCLCPP_DEBUG_STREAM(node_->get_logger(), "Init wave front propagation."); const auto& mesh = mesh_map_->mesh(); const auto& vertex_costs = mesh_map_->vertexCosts(); @@ -662,9 +663,9 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s 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_face = start_opt.unwrap(); const auto& goal_face = goal_opt.unwrap(); @@ -678,7 +679,7 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s if (goal_face == start_face) { - return mbf_msgs::GetPathResult::SUCCESS; + return mbf_msgs::action::GetPath::Result::SUCCESS; } lvr2::DenseVertexMap fixed(mesh.nextVertexIndex(), false); @@ -709,7 +710,7 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s } std::array goal_vertices = mesh.getVerticesOfFace(goal_face); - ROS_DEBUG_STREAM("The goal is at (" << goal.x << ", " << goal.y << ", " << goal.z << ") at the face (" + RCLCPP_DEBUG_STREAM(node_->get_logger(), "The goal is at (" << goal.x << ", " << goal.y << ", " << goal.z << ") at the face (" << goal_vertices[0] << ", " << goal_vertices[1] << ", " << goal_vertices[2] << ")"); mesh_map_->publishDebugPoint(mesh.getVertexPosition(goal_vertices[0]), mesh_map::color(0, 0, 1), "goal_face_v1"); @@ -718,7 +719,7 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s float goal_dist = std::numeric_limits::infinity(); - ROS_DEBUG_STREAM("Start wavefront propagation..."); + RCLCPP_DEBUG_STREAM(node_->get_logger(), "Start wavefront propagation..."); size_t fixed_cnt = 0; size_t fixed_set_cnt = 0; @@ -746,7 +747,7 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s if (goal_dist == std::numeric_limits::infinity() && fixed[goal_vertices[0]] && fixed[goal_vertices[1]] && fixed[goal_vertices[2]]) { - ROS_DEBUG_STREAM("Wave front reached the goal!"); + RCLCPP_DEBUG_STREAM(node_->get_logger(), "Wave front reached the goal!"); goal_dist = distances[current_vh] + goal_dist_offset; } } @@ -840,26 +841,26 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s catch (lvr2::PanicException exception) { invalid.insert(current_vh, true); - ROS_ERROR_STREAM("Found invalid vertex!"); + RCLCPP_ERROR_STREAM(node_->get_logger(), "Found invalid vertex!"); continue; } catch (lvr2::VertexLoopException exception) { invalid.insert(current_vh, true); - ROS_ERROR_STREAM("Found invalid vertex!"); + RCLCPP_ERROR_STREAM(node_->get_logger(), "Found invalid vertex!"); continue; } } if (cancel_planning_) { - ROS_WARN_STREAM("Wave front propagation has been canceled!"); - return mbf_msgs::GetPathResult::CANCELED; + RCLCPP_WARN_STREAM(node_->get_logger(), "Wave front propagation has been canceled!"); + return mbf_msgs::action::GetPath::Result::CANCELED; } ros::WallTime t_wavefront_end = ros::WallTime::now(); double wavefront_propagation_duration = (t_wavefront_end - t_wavefront_start).toNSec() * 1e-6; - ROS_DEBUG_STREAM("Finished wave front propagation."); - ROS_DEBUG_STREAM("Computing the vector map..."); + RCLCPP_DEBUG_STREAM(node_->get_logger(), "Finished wave front propagation."); + RCLCPP_DEBUG_STREAM(node_->get_logger(), "Computing the vector map..."); computeVectorMap(); ros::WallTime t_vector_field_end = ros::WallTime::now(); @@ -877,37 +878,37 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s if (!path_exists) { - ROS_WARN("Predecessor of the goal is not set! No path found!"); - return mbf_msgs::GetPathResult::NO_PATH_FOUND; + RCLCPP_WARN(node_->get_logger(), "Predecessor of the goal is not set! No path found!"); + return mbf_msgs::action::GetPath::Result::NO_PATH_FOUND; } - ROS_DEBUG_STREAM("Start vector field back tracking!"); + RCLCPP_DEBUG_STREAM(node_->get_logger(), "Start vector field back tracking!"); lvr2::FaceHandle current_face = goal_face; mesh_map::Vector current_pos = goal; path.push_front(std::pair(current_pos, current_face)); // move from the goal position towards the start position - while (current_pos.distance2(start) > config.step_width && !cancel_planning_) + while (current_pos.distance2(start) > config_.step_width && !cancel_planning_) { // move current pos ahead on the surface following the vector field, // updates the current face if necessary try { - if (mesh_map_->meshAhead(current_pos, current_face, config.step_width)) + if (mesh_map_->meshAhead(current_pos, current_face, config_.step_width)) { path.push_front(std::pair(current_pos, current_face)); } else { - ROS_WARN_STREAM("Could not find a valid path, while back-tracking from the goal"); - return mbf_msgs::GetPathResult::NO_PATH_FOUND; + RCLCPP_WARN_STREAM(node_->get_logger(), "Could not find a valid path, while back-tracking from the goal"); + return mbf_msgs::action::GetPath::Result::NO_PATH_FOUND; } } catch (lvr2::PanicException exception) { - ROS_ERROR_STREAM("Could not find a valid path, while back-tracking from the goal: HalfEdgeMesh panicked!"); - return mbf_msgs::GetPathResult::NO_PATH_FOUND; + RCLCPP_ERROR_STREAM(node_->get_logger(), "Could not find a valid path, while back-tracking from the goal: HalfEdgeMesh panicked!"); + return mbf_msgs::action::GetPath::Result::NO_PATH_FOUND; } } path.push_front(std::pair(start, start_face)); @@ -915,20 +916,20 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s ros::WallTime t_path_backtracking = ros::WallTime::now(); double path_backtracking_duration = (t_path_backtracking - t_vector_field_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): "<< wavefront_propagation_duration); - ROS_INFO_STREAM("Vector field post computation (ms): " << vector_field_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): "<< wavefront_propagation_duration); + RCLCPP_INFO_STREAM(node_->get_logger(), "Vector field post computation (ms): " << vector_field_duration); + RCLCPP_INFO_STREAM(node_->get_logger(), "Path backtracking duration (ms): " << path_backtracking_duration); if (cancel_planning_) { - ROS_WARN_STREAM("Wave front propagation has been canceled!"); - return mbf_msgs::GetPathResult::CANCELED; + RCLCPP_WARN_STREAM(node_->get_logger(), "Wave front propagation has been canceled!"); + return mbf_msgs::action::GetPath::Result::CANCELED; } - ROS_INFO_STREAM("Successfully finished vector field back tracking!"); - return mbf_msgs::GetPathResult::SUCCESS; + RCLCPP_INFO_STREAM(node_->get_logger(), "Successfully finished vector field back tracking!"); + return mbf_msgs::action::GetPath::Result::SUCCESS; } } /* namespace cvp_mesh_planner */ From 89dadfad813c2e7d9258aecb228d55eb16f7a3d8 Mon Sep 17 00:00:00 2001 From: Matthias Holoch Date: Wed, 24 Jan 2024 15:23:13 +0100 Subject: [PATCH 09/11] change timing to use chrono; change msg types and publishing to fit ROS2 --- cvp_mesh_planner/src/cvp_mesh_planner.cpp | 47 ++++++++++++----------- 1 file changed, 24 insertions(+), 23 deletions(-) diff --git a/cvp_mesh_planner/src/cvp_mesh_planner.cpp b/cvp_mesh_planner/src/cvp_mesh_planner.cpp index 2f19e55..405f09f 100644 --- a/cvp_mesh_planner/src/cvp_mesh_planner.cpp +++ b/cvp_mesh_planner/src/cvp_mesh_planner.cpp @@ -38,6 +38,7 @@ #include #include +#include #include #include @@ -78,8 +79,8 @@ uint32_t CVPMeshPlanner::makePlan(const geometry_msgs::msg::PoseStamped& start, 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; @@ -93,7 +94,7 @@ uint32_t CVPMeshPlanner::makePlan(const geometry_msgs::msg::PoseStamped& start, const auto& face_normals = mesh_map_->faceNormals(); for (auto& next : path) { - geometry_msgs::PoseStamped pose; + geometry_msgs::msg::PoseStamped pose; pose.header = header; pose.pose = mesh_map::calculatePoseFromPosition(vec, next.first, face_normals[fH], dir_length); cost += dir_length; @@ -102,24 +103,24 @@ uint32_t CVPMeshPlanner::makePlan(const geometry_msgs::msg::PoseStamped& start, plan.push_back(pose); } - geometry_msgs::PoseStamped pose; + geometry_msgs::msg::PoseStamped pose; pose.header = header; pose.pose = mesh_map::calculatePoseFromPosition(vec, goal_vec, face_normals[fH], dir_length); cost += dir_length; plan.push_back(pose); } - nav_msgs::Path path_msg; + nav_msgs::msg::Path path_msg; 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; @@ -657,7 +658,7 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s const auto& start_opt = mesh_map_->getContainingFace(start, 0.4); const auto& goal_opt = mesh_map_->getContainingFace(goal, 0.4); - ros::WallTime t_initialization_start = ros::WallTime::now(); + const auto t_initialization_start = std::chrono::steady_clock::now(); // reset cancel planning cancel_planning_ = false; @@ -723,8 +724,8 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s size_t fixed_cnt = 0; size_t fixed_set_cnt = 0; - ros::WallTime t_wavefront_start = ros::WallTime::now(); - double initialization_duration = (t_wavefront_start - t_initialization_start).toNSec() * 1e-6; + const auto t_wavefront_start = std::chrono::steady_clock::now(); + const auto initialization_duration_ms = std::chrono::duration_cast(t_wavefront_start - t_initialization_start); while (!pq.isEmpty() && !cancel_planning_) { @@ -736,7 +737,7 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s if (distances[current_vh] > goal_dist) continue; - if (vertex_costs[current_vh] > config.cost_limit) + if (vertex_costs[current_vh] > config_.cost_limit) continue; if (invalid[current_vh]) @@ -748,7 +749,7 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s fixed[goal_vertices[2]]) { RCLCPP_DEBUG_STREAM(node_->get_logger(), "Wave front reached the goal!"); - goal_dist = distances[current_vh] + goal_dist_offset; + goal_dist = distances[current_vh] + config_.goal_dist_offset; } } @@ -857,14 +858,14 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s RCLCPP_WARN_STREAM(node_->get_logger(), "Wave front propagation has been canceled!"); return mbf_msgs::action::GetPath::Result::CANCELED; } - ros::WallTime t_wavefront_end = ros::WallTime::now(); - double wavefront_propagation_duration = (t_wavefront_end - t_wavefront_start).toNSec() * 1e-6; + const auto t_wavefront_end = std::chrono::steady_clock::now(); + const auto wavefront_propagation_duration_ms = std::chrono::duration_cast(t_wavefront_end - t_wavefront_start); RCLCPP_DEBUG_STREAM(node_->get_logger(), "Finished wave front propagation."); RCLCPP_DEBUG_STREAM(node_->get_logger(), "Computing the vector map..."); computeVectorMap(); - ros::WallTime t_vector_field_end = ros::WallTime::now(); - double vector_field_duration = (t_vector_field_end - t_wavefront_end).toNSec() * 1e-6; + const auto t_vector_field_end = std::chrono::steady_clock::now(); + const auto vector_field_duration_ms = std::chrono::duration_cast(t_vector_field_end - t_wavefront_end); bool path_exists = false; for (auto goal_vertex : goal_vertices) @@ -913,14 +914,14 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s } path.push_front(std::pair(start, start_face)); - ros::WallTime t_path_backtracking = ros::WallTime::now(); - double path_backtracking_duration = (t_path_backtracking - t_vector_field_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_vector_field_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): "<< wavefront_propagation_duration); - RCLCPP_INFO_STREAM(node_->get_logger(), "Vector field post computation (ms): " << vector_field_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): "<< wavefront_propagation_duration_ms.count()); + RCLCPP_INFO_STREAM(node_->get_logger(), "Vector field post computation (ms): " << vector_field_duration_ms.count()); + RCLCPP_INFO_STREAM(node_->get_logger(), "Path backtracking duration (ms): " << path_backtracking_duration_ms.count()); if (cancel_planning_) { From 0d676a97c5ef844fedeaa822e80e2f2b4c32f780 Mon Sep 17 00:00:00 2001 From: Matthias Holoch Date: Wed, 24 Jan 2024 15:26:14 +0100 Subject: [PATCH 10/11] change paramter names to be inside the plugin instance's namespace --- cvp_mesh_planner/src/cvp_mesh_planner.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/cvp_mesh_planner/src/cvp_mesh_planner.cpp b/cvp_mesh_planner/src/cvp_mesh_planner.cpp index 405f09f..4d8e18c 100644 --- a/cvp_mesh_planner/src/cvp_mesh_planner.cpp +++ b/cvp_mesh_planner/src/cvp_mesh_planner.cpp @@ -139,9 +139,9 @@ bool CVPMeshPlanner::initialize(const std::string& plugin_name, const std::share 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."; @@ -149,7 +149,7 @@ bool CVPMeshPlanner::initialize(const std::string& plugin_name, const std::share 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); } { // step width param rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -158,7 +158,7 @@ bool CVPMeshPlanner::initialize(const std::string& plugin_name, const std::share range.from_value = 0.01; range.to_value = 1.0; descriptor.floating_point_range.push_back(range); - config_.step_width = node->declare_parameter("step_width", config_.step_width); + config_.step_width = node->declare_parameter(name_ + ".step_width", config_.step_width); } path_pub_ = node->create_publisher("~/path", rclcpp::QoS(1).transient_local()); From bbcda49aaf7adafcbbe8994a5b8127d014721d9b Mon Sep 17 00:00:00 2001 From: Matthias Holoch Date: Tue, 30 Jan 2024 11:03:48 +0100 Subject: [PATCH 11/11] refactor: revert accidental formatting --- .../include/cvp_mesh_planner/cvp_mesh_planner.h | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/cvp_mesh_planner/include/cvp_mesh_planner/cvp_mesh_planner.h b/cvp_mesh_planner/include/cvp_mesh_planner/cvp_mesh_planner.h index 69a613d..6b6847d 100644 --- a/cvp_mesh_planner/include/cvp_mesh_planner/cvp_mesh_planner.h +++ b/cvp_mesh_planner/include/cvp_mesh_planner/cvp_mesh_planner.h @@ -71,10 +71,8 @@ class CVPMeshPlanner : public mbf_mesh_core::MeshPlanner * @param message a detailed outcome message * @return result outcome code, see the GetPath action definition */ - virtual uint32_t makePlan(const geometry_msgs::msg::PoseStamped& start, - const geometry_msgs::msg::PoseStamped& goal, - double tolerance, - std::vector& plan, double& cost, + 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; /**