Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 5 additions & 0 deletions .github/deps.repos
Original file line number Diff line number Diff line change
Expand Up @@ -3,3 +3,8 @@ repositories:
type: git
url: https://github.com/Fields2Cover/Fields2Cover.git
version: v1.2.1-devel

navigation2:
type: git
url: https://github.com/ros-navigation/navigation2.git
version: main
4 changes: 2 additions & 2 deletions .github/workflows/lint.yml
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@ jobs:
name: ament_${{ matrix.linter }}
runs-on: ubuntu-latest
container:
image: rostooling/setup-ros-docker:ubuntu-noble-ros-jazzy-ros-base-latest
image: ghcr.io/ros-tooling/setup-ros-docker/setup-ros-docker-ubuntu-noble-ros-rolling-ros-base:master
strategy:
fail-fast: false
matrix:
Expand All @@ -17,7 +17,7 @@ jobs:
- uses: ros-tooling/[email protected]
with:
linter: ${{ matrix.linter }}
distribution: jazzy
distribution: rolling
package-name: |
opennav_coverage
opennav_coverage_bt
Expand Down
7 changes: 4 additions & 3 deletions .github/workflows/test.yml
Original file line number Diff line number Diff line change
Expand Up @@ -12,19 +12,20 @@ jobs:
ROS_DISTRO: ${{ matrix.ros_distro }}
RMW_IMPLEMENTATION: rmw_cyclonedds_cpp
container:
image: rostooling/setup-ros-docker:ubuntu-noble-latest
image: ghcr.io/ros-tooling/setup-ros-docker/setup-ros-docker-ubuntu-noble-ros-rolling-ros-base:master
strategy:
fail-fast: false
matrix:
ros_distro: [jazzy]
ros_distro: [rolling]
steps:
- uses: actions/checkout@v2
- name: Install Cyclone DDS
run: sudo apt install ros-${{ matrix.ros_distro }}-rmw-cyclonedds-cpp -y
- name: Build and run tests
id: action-ros-ci
uses: ros-tooling/action-ros-ci@v0.3
uses: ros-tooling/action-ros-ci@v0.4
with:
package-name: nav2_bt_navigator nav2_behavior_tree opennav_coverage opennav_coverage_bt opennav_coverage_demo opennav_coverage_msgs opennav_coverage_navigator opennav_row_coverage
import-token: ${{ secrets.GITHUB_TOKEN }}
target-ros2-distro: ${{ matrix.ros_distro }}
vcs-repo-file-url: "${{ github.workspace }}/.github/deps.repos"
Expand Down
9 changes: 4 additions & 5 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@ This server exposes all of the features of Fields2Cover as a Lifecycle-Component

- `opennav_coverage_navigator`: Contains the BT Navigator plugin exposing `NavigateCompleteCoverage` action server analog to `NavigateToPose` and `NavigateThroughPoses`.

- `opennav_coverage_demo`: Contains an example launch and python node to trigger the coverage planner for a demonstration of the Navigator Plugin, Server, BT nodes, XML, and such.
- `opennav_coverage_demo`: Contains an example launch and python node to trigger the coverage planner for a demonstration of the Navigator Plugin, Server, BT nodes, XML, and such.

Fields2Cover is a living library with new features planned to be added (for example those discussed in a [Nav2 integration ticket](https://github.com/Fields2Cover/Fields2Cover/issues/73)). As such, as new F2C capabilities are created, they will be welcome for integration here. If you see anything missing, please let us know or submit a PR to expose it!

Expand Down Expand Up @@ -61,7 +61,7 @@ The result returns a `result.nav_path` -- which is a `nav_msgs/Path` containing

It also returns an error code, if any error occurred and the total planning time for metrics analysis.

Note that `SwathMode` settings are to be paired with the `opennav_coverage` server with polygons, while the `RowSwathMode` settings are to be paired with the `opennav_row_coverage` server based on annotated rows.
Note that `SwathMode` settings are to be paired with the `opennav_coverage` server with polygons, while the `RowSwathMode` settings are to be paired with the `opennav_row_coverage` server based on annotated rows.

### NavigateCompleteCoverage

Expand All @@ -82,7 +82,6 @@ The complete set of options are exposed as both dynamic parameters and through t

| Parameter | Description | Type |
|------------------------------|------------------------------------------------|--------|
| action_server_result_timeout | Action server result holding timeout (s) | double |
| coordinates_in_cartesian_frame | Whether incoming requests are in cartesian or GPS coordinates | bool |
| robot_width | Robots width (m) | double |
| operation_width | Width of implement or task for coverage | double |
Expand Down Expand Up @@ -146,7 +145,7 @@ N/A

#### A Quick Note On Skipping

Rows(r) and swaths(s) are numbered as such. r1, s1, r2, s2, .... rN-1, sN-1, rN.
Rows(r) and swaths(s) are numbered as such. r1, s1, r2, s2, .... rN-1, sN-1, rN.
In order to skip particular rows the 'opennav_coverage_msgs/RowSwathMode' provides 'skip_ids'
to be populated. For example, if the skip ids were set to {1, 3} for five rows the output would be the following: s2, s4.

Expand All @@ -156,7 +155,7 @@ As noted about `opennav_row_coverage` provides three ways to compute swaths whic

The `CENTER` generator iterates through each row, calculating a center between consecutive rows.

The `OFFSET` generator iterates through each row, calculating a relative offset between consecutive rows.
The `OFFSET` generator iterates through each row, calculating a relative offset between consecutive rows.

The `SWATHSAREROWS` generator iterates through each row and uses that row as the swath.

Expand Down
22 changes: 11 additions & 11 deletions opennav_coverage/include/opennav_coverage/coverage_server.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -21,9 +21,9 @@
#include "fields2cover.h" // NOLINT

#include "rclcpp/rclcpp.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav2_util/simple_action_server.hpp"
#include "nav2_ros_common/lifecycle_node.hpp"
#include "nav2_ros_common/node_utils.hpp"
#include "nav2_ros_common/simple_action_server.hpp"
#include "opennav_coverage/headland_generator.hpp"
#include "opennav_coverage/swath_generator.hpp"
#include "opennav_coverage/route_generator.hpp"
Expand All @@ -37,10 +37,10 @@ namespace opennav_coverage
* @brief An action server which implements highly reconfigurable complete
* coverage planning using the Fields2Cover library
*/
class CoverageServer : public nav2_util::LifecycleNode
class CoverageServer : public nav2::LifecycleNode
{
public:
using ActionServer = nav2_util::SimpleActionServer<ComputeCoveragePath>;
using ActionServer = nav2::SimpleActionServer<ComputeCoveragePath>;

/**
* @brief A constructor for opennav_coverage::CoverageServer
Expand Down Expand Up @@ -79,35 +79,35 @@ class CoverageServer : public nav2_util::LifecycleNode
* @param state Reference to LifeCycle node state
* @return SUCCESS or FAILURE
*/
nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
nav2::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;

/**
* @brief Activate member variables
* @param state Reference to LifeCycle node state
* @return SUCCESS or FAILURE
*/
nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
nav2::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;

/**
* @brief Deactivate member variables
* @param state Reference to LifeCycle node state
* @return SUCCESS or FAILURE
*/
nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
nav2::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;

/**
* @brief Reset member variables
* @param state Reference to LifeCycle node state
* @return SUCCESS or FAILURE
*/
nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
nav2::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;

/**
* @brief Called when in shutdown state
* @param state Reference to LifeCycle node state
* @return SUCCESS or FAILURE
*/
nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
nav2::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;

/**
* @brief Callback executed when a parameter change is detected
Expand All @@ -120,7 +120,7 @@ class CoverageServer : public nav2_util::LifecycleNode
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
std::mutex dynamic_params_lock_;

std::unique_ptr<ActionServer> action_server_;
typename ActionServer::SharedPtr action_server_;

std::unique_ptr<RobotParams> robot_params_;
std::unique_ptr<HeadlandGenerator> headland_gen_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -21,8 +21,8 @@
#include "fields2cover.h" // NOLINT

#include "rclcpp/rclcpp.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav2_ros_common/lifecycle_node.hpp"
#include "nav2_ros_common/node_utils.hpp"
#include "opennav_coverage_msgs/msg/headland_mode.hpp"
#include "opennav_coverage/utils.hpp"
#include "opennav_coverage/types.hpp"
Expand All @@ -45,13 +45,13 @@ class HeadlandGenerator
{
logger_ = node->get_logger();

nav2_util::declare_parameter_if_not_declared(
nav2::declare_parameter_if_not_declared(
node, "default_headland_type", rclcpp::ParameterValue("CONSTANT"));
std::string type_str = node->get_parameter("default_headland_type").as_string();
default_type_ = toType(type_str);
default_generator_ = createGenerator(default_type_);

nav2_util::declare_parameter_if_not_declared(
nav2::declare_parameter_if_not_declared(
node, "default_headland_width", rclcpp::ParameterValue(2.0));
default_headland_width_ = node->get_parameter("default_headland_width").as_double();
}
Expand Down
10 changes: 5 additions & 5 deletions opennav_coverage/include/opennav_coverage/path_generator.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -22,8 +22,8 @@
#include "fields2cover.h" // NOLINT

#include "rclcpp/rclcpp.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav2_ros_common/lifecycle_node.hpp"
#include "nav2_ros_common/node_utils.hpp"
#include "opennav_coverage_msgs/msg/path_mode.hpp"
#include "opennav_coverage/utils.hpp"
#include "opennav_coverage/types.hpp"
Expand All @@ -48,17 +48,17 @@ class PathGenerator
logger_ = node->get_logger();
robot_params_ = robot_params;

nav2_util::declare_parameter_if_not_declared(
nav2::declare_parameter_if_not_declared(
node, "default_path_type", rclcpp::ParameterValue("DUBIN"));
std::string type_str = node->get_parameter("default_path_type").as_string();
default_type_ = toType(type_str);

nav2_util::declare_parameter_if_not_declared(
nav2::declare_parameter_if_not_declared(
node, "default_path_continuity_type", rclcpp::ParameterValue("CONTINUOUS"));
std::string type_cont_str = node->get_parameter("default_path_continuity_type").as_string();
default_continuity_type_ = toContinuityType(type_cont_str);

nav2_util::declare_parameter_if_not_declared(
nav2::declare_parameter_if_not_declared(
node, "default_turn_point_distance", rclcpp::ParameterValue(0.1));
default_turn_point_distance_ = node->get_parameter("default_turn_point_distance").as_double();

Expand Down
12 changes: 6 additions & 6 deletions opennav_coverage/include/opennav_coverage/robot_params.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -21,8 +21,8 @@
#include "fields2cover.h" // NOLINT

#include "rclcpp/rclcpp.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav2_ros_common/lifecycle_node.hpp"
#include "nav2_ros_common/node_utils.hpp"
#include "opennav_coverage/utils.hpp"
#include "opennav_coverage/types.hpp"

Expand All @@ -42,19 +42,19 @@ class RobotParams
template<typename NodeT>
explicit RobotParams(const NodeT & node)
{
nav2_util::declare_parameter_if_not_declared(
nav2::declare_parameter_if_not_declared(
node, "robot_width", rclcpp::ParameterValue(2.1));
robot_.robot_width = node->get_parameter("robot_width").as_double();

nav2_util::declare_parameter_if_not_declared(
nav2::declare_parameter_if_not_declared(
node, "operation_width", rclcpp::ParameterValue(2.5));
robot_.op_width = node->get_parameter("operation_width").as_double();

nav2_util::declare_parameter_if_not_declared(
nav2::declare_parameter_if_not_declared(
node, "min_turning_radius", rclcpp::ParameterValue(0.4));
robot_.setMinRadius(node->get_parameter("min_turning_radius").as_double());

nav2_util::declare_parameter_if_not_declared(
nav2::declare_parameter_if_not_declared(
node, "linear_curv_change", rclcpp::ParameterValue(2.0));
robot_.linear_curv_change = node->get_parameter("linear_curv_change").as_double();
}
Expand Down
10 changes: 5 additions & 5 deletions opennav_coverage/include/opennav_coverage/route_generator.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -21,8 +21,8 @@
#include "fields2cover.h" // NOLINT

#include "rclcpp/rclcpp.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav2_ros_common/lifecycle_node.hpp"
#include "nav2_ros_common/node_utils.hpp"
#include "opennav_coverage_msgs/msg/route_mode.hpp"
#include "opennav_coverage/utils.hpp"
#include "opennav_coverage/types.hpp"
Expand All @@ -43,17 +43,17 @@ class RouteGenerator
template<typename NodeT>
explicit RouteGenerator(const NodeT & node)
{
nav2_util::declare_parameter_if_not_declared(
nav2::declare_parameter_if_not_declared(
node, "default_route_type", rclcpp::ParameterValue("BOUSTROPHEDON"));
std::string type_str = node->get_parameter("default_route_type").as_string();
default_type_ = toType(type_str);
default_generator_ = createGenerator(default_type_);

nav2_util::declare_parameter_if_not_declared(
nav2::declare_parameter_if_not_declared(
node, "default_spiral_n", rclcpp::ParameterValue(4));
default_spiral_n_ = node->get_parameter("default_spiral_n").as_int();

nav2_util::declare_parameter_if_not_declared(
nav2::declare_parameter_if_not_declared(
node, "default_custom_order", rclcpp::PARAMETER_INTEGER_ARRAY);
try {
// Get the custom order and cast to size_t
Expand Down
14 changes: 7 additions & 7 deletions opennav_coverage/include/opennav_coverage/swath_generator.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -22,8 +22,8 @@
#include "fields2cover.h" // NOLINT

#include "rclcpp/rclcpp.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav2_ros_common/lifecycle_node.hpp"
#include "nav2_ros_common/node_utils.hpp"
#include "opennav_coverage_msgs/msg/swath_mode.hpp"
#include "opennav_coverage/utils.hpp"
#include "opennav_coverage/types.hpp"
Expand All @@ -48,25 +48,25 @@ class SwathGenerator
logger_ = node->get_logger();
robot_params_ = robot_params;

nav2_util::declare_parameter_if_not_declared(
nav2::declare_parameter_if_not_declared(
node, "default_swath_type", rclcpp::ParameterValue("LENGTH"));
std::string type_str = node->get_parameter("default_swath_type").as_string();
default_type_ = toType(type_str);

nav2_util::declare_parameter_if_not_declared(
nav2::declare_parameter_if_not_declared(
node, "default_swath_angle_type", rclcpp::ParameterValue("BRUTE_FORCE"));
std::string angle_str = node->get_parameter("default_swath_angle_type").as_string();
default_angle_type_ = toAngleType(angle_str);

nav2_util::declare_parameter_if_not_declared(
nav2::declare_parameter_if_not_declared(
node, "default_swath_angle", rclcpp::ParameterValue(0.0));
default_swath_angle_ = node->get_parameter("default_swath_angle").as_double();

nav2_util::declare_parameter_if_not_declared(
nav2::declare_parameter_if_not_declared(
node, "default_step_angle", rclcpp::ParameterValue(1.7453e-2));
default_step_angle_ = node->get_parameter("default_step_angle").as_double();

nav2_util::declare_parameter_if_not_declared(
nav2::declare_parameter_if_not_declared(
node, "default_allow_overlap", rclcpp::ParameterValue(false));
default_allow_overlap_ = node->get_parameter("default_allow_overlap").as_bool();

Expand Down
2 changes: 1 addition & 1 deletion opennav_coverage/include/opennav_coverage/utils.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@
#include "fields2cover.h" // NOLINT
#include "rclcpp/rclcpp.hpp"
#include "opennav_coverage/types.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav2_ros_common/node_utils.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "geometry_msgs/msg/point32.hpp"

Expand Down
1 change: 1 addition & 0 deletions opennav_coverage/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
<depend>rclcpp_lifecycle</depend>
<depend>nav2_util</depend>
<depend>nav2_msgs</depend>
<depend>nav2_ros_common</depend>
<depend>nav_msgs</depend>
<depend>geometry_msgs</depend>
<depend>builtin_interfaces</depend>
Expand Down
Loading
Loading