Skip to content
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 11 commits into from
Jan 30, 2024
24 changes: 17 additions & 7 deletions README.md
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)
Expand Down Expand Up @@ -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

Expand Down
74 changes: 24 additions & 50 deletions cvp_mesh_planner/CMakeLists.txt
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 removed cvp_mesh_planner/COLCON_IGNORE
Empty file.
10 changes: 0 additions & 10 deletions cvp_mesh_planner/cfg/CVPMeshPlanner.cfg

This file was deleted.

2 changes: 1 addition & 1 deletion cvp_mesh_planner/cvp_mesh_planner.xml
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
<library path="lib/libcvp_mesh_planner">
<library path="cvp_mesh_planner">
<class name="cvp_mesh_planner/CVPMeshPlanner" type="cvp_mesh_planner::CVPMeshPlanner"
base_class_type="mbf_mesh_core::MeshPlanner">
<description>
Expand Down
91 changes: 47 additions & 44 deletions cvp_mesh_planner/include/cvp_mesh_planner/cvp_mesh_planner.h
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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,
Copy link
Member

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

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

whoops, will fix it!

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:

Expand Down Expand Up @@ -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_;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I did this with member variables in the ftc_controller.
But i like this option, to use a struct member variable with all config options.
Maybe we can change it inside the ftc_controller and use this option for all mbf_plugins


//! 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
Expand Down
9 changes: 3 additions & 6 deletions cvp_mesh_planner/package.xml
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>
Loading
Loading