diff --git a/README.md b/README.md index 6e6a02e3..28c8d313 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) @@ -13,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) @@ -115,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 diff --git a/cvp_mesh_planner/CMakeLists.txt b/cvp_mesh_planner/CMakeLists.txt index b3711649..03eb3371 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 e69de29b..00000000 diff --git a/cvp_mesh_planner/cfg/CVPMeshPlanner.cfg b/cvp_mesh_planner/cfg/CVPMeshPlanner.cfg deleted file mode 100755 index a446e227..00000000 --- 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/cvp_mesh_planner.xml b/cvp_mesh_planner/cvp_mesh_planner.xml index 098bd761..0b2382e4 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/include/cvp_mesh_planner/cvp_mesh_planner.h b/cvp_mesh_planner/include/cvp_mesh_planner/cvp_mesh_planner.h index 2f620200..6b6847df 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,17 @@ #define MESH_NAVIGATION__MESH_PLANNER_H #include -#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 +71,15 @@ 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 +87,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: @@ -162,65 +162,66 @@ 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: //! 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; + //! 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; + 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; + rclcpp::Publisher::SharedPtr 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 = false; + //! an offset that determines how far beyond the goal (robot's position) is propagated. + 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 - 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/package.xml b/cvp_mesh_planner/package.xml index c67179f4..ab2c9620 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 diff --git a/cvp_mesh_planner/src/cvp_mesh_planner.cpp b/cvp_mesh_planner/src/cvp_mesh_planner.cpp index 38f2bf76..4d8e18ca 100644 --- a/cvp_mesh_planner/src/cvp_mesh_planner.cpp +++ b/cvp_mesh_planner/src/cvp_mesh_planner.cpp @@ -38,9 +38,9 @@ #include #include -#include +#include #include -#include +#include #include "cvp_mesh_planner/cvp_mesh_planner.h" //#define DEBUG @@ -59,16 +59,18 @@ 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(); + const auto& mesh = mesh_map_->mesh(); std::list> path; // 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); @@ -77,9 +79,9 @@ uint32_t CVPMeshPlanner::makePlan(const geometry_msgs::PoseStamped& start, const path.reverse(); - std_msgs::Header header; - header.stamp = ros::Time::now(); - header.frame_id = mesh_map->mapFrame(); + std_msgs::msg::Header header; + header.stamp = node_->now(); + header.frame_id = mesh_map_->mapFrame(); cost = 0; float dir_length; @@ -89,10 +91,10 @@ uint32_t CVPMeshPlanner::makePlan(const geometry_msgs::PoseStamped& start, const 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; + geometry_msgs::msg::PoseStamped pose; pose.header = header; pose.pose = mesh_map::calculatePoseFromPosition(vec, next.first, face_normals[fH], dir_length); cost += dir_length; @@ -101,24 +103,24 @@ uint32_t CVPMeshPlanner::makePlan(const geometry_msgs::PoseStamped& start, const 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); - mesh_map->publishVertexCosts(potential, "Potential"); - ROS_INFO_STREAM("Path length: " << cost << "m"); + 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; @@ -126,67 +128,85 @@ uint32_t CVPMeshPlanner::makePlan(const geometry_msgs::PoseStamped& start, const bool CVPMeshPlanner::cancel() { - cancel_planning = true; + cancel_planning_ = true; 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; - 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); - // TODO check all map dependencies! (loaded layers etc...) + mesh_map_ = mesh_map_ptr; + name_ = plugin_name; + map_frame_ = mesh_map_->mapFrame(); + node_ = node; + + 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."; + rcl_interfaces::msg::FloatingPointRange range; + range.from_value = 0.0; + range.to_value = 10.0; + descriptor.floating_point_range.push_back(range); + config_.cost_limit = node->declare_parameter(name_ + ".cost_limit", config_.cost_limit); + } + { // 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(name_ + ".step_width", config_.step_width); + } - reconfigure_server_ptr = boost::shared_ptr>( - new dynamic_reconfigure::Server(private_nh)); + 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...) - 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() { - 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 +218,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 +237,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 +281,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 +297,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 +319,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 +334,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 +357,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]; @@ -372,7 +392,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) { @@ -387,14 +407,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 +426,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; @@ -426,32 +446,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 @@ -459,12 +479,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 +492,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 +507,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 +525,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 +549,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 +597,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 +613,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; } } @@ -622,37 +642,37 @@ 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(); - 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(); + const auto t_initialization_start = std::chrono::steady_clock::now(); // reset cancel planning - cancel_planning = false; + 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(); - 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(); @@ -660,13 +680,13 @@ 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); // clear vector field map - vector_map.clear(); + vector_map_.clear(); // initialize distances with infinity // initialize predecessor of each vertex with itself @@ -684,30 +704,30 @@ 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); } 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"); - 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(); - 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; - 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) + while (!pq.isEmpty() && !cancel_planning_) { lvr2::VertexHandle current_vh = pq.popMin().key(); @@ -717,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]) @@ -728,8 +748,8 @@ 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!"); - goal_dist = distances[current_vh] + goal_dist_offset; + RCLCPP_DEBUG_STREAM(node_->get_logger(), "Wave front reached the goal!"); + goal_dist = distances[current_vh] + config_.goal_dist_offset; } } @@ -822,30 +842,30 @@ 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) + 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..."); + 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) @@ -859,58 +879,58 @@ 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)); - 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); - 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_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) + 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 */ diff --git a/mesh_map/include/mesh_map/mesh_map.h b/mesh_map/include/mesh_map/mesh_map.h index ddfc30e6..dcff269a 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();