-
Notifications
You must be signed in to change notification settings - Fork 71
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
41 port cvp mesh planner pkg to ros2 #46
Merged
Merged
Changes from 10 commits
Commits
Show all changes
11 commits
Select commit
Hold shift + click to select a range
04c1f8b
Update README.md
spuetz fc51723
Update README.md
spuetz c336d04
port pkg information to ROS2
Cakem1x 592c470
port includes and function signature to ROS2
Cakem1x 54279d9
refactor: add trailing underscore to class members
Cakem1x bff8ef2
change param declaration to fit ROS2
Cakem1x 60417cd
fix typo in docs
Cakem1x fff9871
change logging and reconfigure callback to work with ROS2
Cakem1x 89dadfa
change timing to use chrono; change msg types and publishing to fit ROS2
Cakem1x 0d676a9
change paramter names to be inside the plugin instance's namespace
Cakem1x bbcda49
refactor: revert accidental formatting
Cakem1x File filter
Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -1,18 +1,16 @@ | ||
# 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:[email protected]) | ||
Author: [Sebastian Pütz](mailto:[email protected]) | ||
|
||
* [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 | ||
|
||
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 | ||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include> | ||
$<INSTALL_INTERFACE:include>) | ||
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() |
Empty file.
This file was deleted.
Oops, something went wrong.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -39,17 +39,17 @@ | |
#define MESH_NAVIGATION__MESH_PLANNER_H | ||
|
||
#include <mbf_mesh_core/mesh_planner.h> | ||
#include <mbf_msgs/GetPathResult.h> | ||
#include <mbf_msgs/action/get_path.hpp> | ||
#include <mesh_map/mesh_map.h> | ||
#include <cvp_mesh_planner/CVPMeshPlannerConfig.h> | ||
#include <nav_msgs/Path.h> | ||
#include <nav_msgs/msg/path.hpp> | ||
#include <rclcpp/rclcpp.hpp> | ||
|
||
namespace cvp_mesh_planner | ||
{ | ||
class CVPMeshPlanner : public mbf_mesh_core::MeshPlanner | ||
{ | ||
public: | ||
typedef boost::shared_ptr<cvp_mesh_planner::CVPMeshPlanner> Ptr; | ||
typedef std::shared_ptr<cvp_mesh_planner::CVPMeshPlanner> Ptr; | ||
|
||
/** | ||
* @brief Constructor | ||
|
@@ -71,23 +71,25 @@ 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<geometry_msgs::PoseStamped>& 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<geometry_msgs::msg::PoseStamped>& 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 | ||
* @param name The user configured name, which is used as namespace for parameters, etc. | ||
* @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::MeshMap>& mesh_map_ptr); | ||
virtual bool initialize(const std::string& plugin_name, const std::shared_ptr<mesh_map::MeshMap>& mesh_map_ptr, const rclcpp::Node::SharedPtr& node) override; | ||
|
||
protected: | ||
|
||
|
@@ -162,65 +164,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<rclcpp::Parameter> 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<nav_msgs::msg::Path>::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<dynamic_reconfigure::Server<cvp_mesh_planner::CVPMeshPlannerConfig>> reconfigure_server_ptr; | ||
|
||
//! dynamic reconfigure callback function binding | ||
dynamic_reconfigure::Server<cvp_mesh_planner::CVPMeshPlannerConfig>::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_; | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. I did this with member variables in the ftc_controller. |
||
|
||
//! theta angles to the source of the wave front propagation | ||
lvr2::DenseVertexMap<float> direction; | ||
lvr2::DenseVertexMap<float> direction_; | ||
|
||
//! predecessors while wave propagation | ||
lvr2::DenseVertexMap<lvr2::VertexHandle> predecessors; | ||
lvr2::DenseVertexMap<lvr2::VertexHandle> predecessors_; | ||
|
||
//! the face which is cut by the computed line to the source | ||
lvr2::DenseVertexMap<lvr2::FaceHandle> cutting_faces; | ||
lvr2::DenseVertexMap<lvr2::FaceHandle> cutting_faces_; | ||
|
||
//! stores the current vector map containing vectors pointing to the seed | ||
lvr2::DenseVertexMap<mesh_map::Vector> vector_map; | ||
lvr2::DenseVertexMap<mesh_map::Vector> vector_map_; | ||
|
||
//! potential field / scalar distance field to the seed | ||
lvr2::DenseVertexMap<float> potential; | ||
lvr2::DenseVertexMap<float> potential_; | ||
}; | ||
|
||
} // namespace cvp_mesh_planner | ||
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -1,22 +1,19 @@ | ||
<?xml version="1.0"?> | ||
<package format="2"> | ||
<package format="3"> | ||
<name>cvp_mesh_planner</name> | ||
<version>1.0.1</version> | ||
<description>The Continuous Vector Field Planner (CVP) mesh planner package</description> | ||
<maintainer email="[email protected]">Sebastian Pütz</maintainer> | ||
<license>BSD 3-Clause</license> | ||
<author email="[email protected]">Sebastian Pütz</author> | ||
|
||
<buildtool_depend>catkin</buildtool_depend> | ||
|
||
<depend>roscpp</depend> | ||
<depend>rclcpp</depend> | ||
<depend>mbf_mesh_core</depend> | ||
<depend>mbf_utility</depend> | ||
<depend>mbf_msgs</depend> | ||
<depend>mesh_map</depend> | ||
<depend>dynamic_reconfigure</depend> | ||
|
||
<export> | ||
<mbf_mesh_core plugin="${prefix}/cvp_mesh_planner.xml"/> | ||
<build_type>ament_cmake</build_type> | ||
</export> | ||
</package> |
Oops, something went wrong.
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
you have formatted each variable in a line, but not cost
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
whoops, will fix it!