Skip to content

Commit

Permalink
Add ability to publish layers of the costmap (ros-navigation#3254)
Browse files Browse the repository at this point in the history
* publish layers of costmap

* lint fix

* lint round 2 :)

* code review

* remove isPublishable

* lint

* test running

* rough structure complete

* completed test

* lint

* code review

* CI

* CI

* linting

* completed pub test

* CostmapLayer::matchSize may be executed concurrently (ros-navigation#3250)

* CostmapLayer::matchSize() add a mutex

* Fix typo (ros-navigation#3262)

* Adding new Nav2 Smoother: Savitzky-Golay Smoother (ros-navigation#3264)

* initial prototype of the Savitzky Golay Filter Path Smoother

* fixed indexing issue - tested working

* updates for filter

* adding unit tests for SG-filter smoother

* adding lifecycle transitions

* refactoring RPP a bit for cleanliness on way to ROSCon (ros-navigation#3265)

* refactor for RPP on way to ROSCon

* fixing header

* fixing header

* fixing header

* fix edge cases test samplings

* linting

* exceptions for compute path through poses (ros-navigation#3248)

* exceptions for compute path through poses

* lint fix

* code review

* code review

Co-authored-by: Joshua Wallace <josho.wallace.com>

* Reclaim Our CI Coverage from the Lords of Painful Subtle Regressions ⚔️⚔️⚔️ (ros-navigation#3266)

* test waypoint follower with composition off for logging

* adding no composition to all system tests

* Added Line Iterator (ros-navigation#3197)

* Added Line Iterator

* Updated Line Iterator to a new iteration method

* Added the resolution as a parameter/ fixed linting

* Added the resolution as a parameter/ fixed linting

* Added unittests for the line iterator

* Added unittests based on "unittest" package

* Fixed __init__.py and rephrased some docstrings

* Fixed linting errors

* Fixed Linting Errors

* Added some unittests and removed some methods

* Dummy commit for CircleCI Issue

Co-authored-by: Afif Swaidan <afif.swaidan@spexal.com>

* Use SetParameter Launch API to set the yaml filename for map_server (ros-navigation#3174)

* implement launch priority for the mapserver parameter yaml_filename

minor fix

fix commit

reincluded rewritten function

comment remaining lines for yaml_filename

removed default_value

issue with composable node

alternative soltion to the condition param not working in composable node

remove unused import

remove comments and reorder composablenode execution

fixing commit

fixing format

fixing lint

Update nav2_bringup/params/nav2_params.yaml

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* state new ros-rolling release changes and deprecation

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* adding reconfigure test to thetastar (ros-navigation#3275)

* Check for range_max of laserscan in updateFilter to avoid a implicit overflow crash. (ros-navigation#3276)

* Update amcl_node.cpp

* fit the code style

* fit code style

* BT Service Node to throw if service was not available in time (ros-navigation#3256)

* throw if service server wasn't available in time

mimic the behavior of the bt action node constructor

* throw if action unavailable in bt cancel action

* use chrono literals namespace

* fix linting errors

* fix code style divergence

* remove exec_depend on behaviortree_cpp_v3 (ros-navigation#3279)

* add parameterized refinement recursion numbers in Smac Planner Smoother and Simple Smoother (ros-navigation#3284)

* add parameterized refinement recursion numbers

* fix tests

* Add allow_unknown parameter to theta star planner (ros-navigation#3286)

* Add allow unknown parameter to theta star planner

* Add allow unknown parameter to tests

* missing comma

* Change cost of unknown tiles

* Uncrustify

* Include test cases waypoint follwer (ros-navigation#3288)

* WIP

* included missed_waypoing check

* finished inclding test

* fix format

* return default sleep value

* Dynamically changing polygons support (ros-navigation#3245)

* Add Collision Monitor polygon topics subscription

* Add the support of polygons published in different frame

* Internal review

* Fix working with polygons visualization

* Update nav2_collision_monitor/README.md

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Move getTransform to nav2_util

* Fix misprint

* Meet remaining review items:
* Update polygon params handling logic
* Warn if polygon shape was not set
* Publish with ownership movement

* Correct polygons_test.cpp parameters handling logic

* Adjust README for dynamic polygons logic update

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* adding getCostScalingFactor (ros-navigation#3290)

* Implemented smoother selector bt node (ros-navigation#3283)

* Implemented smoother selector bt node

Signed-off-by: Owen Hooper <17ofh@queensu.ca>

* updated copyright in modified file

Signed-off-by: Owen Hooper <17ofh@queensu.ca>

Signed-off-by: Owen Hooper <17ofh@queensu.ca>

* Pipe error codes (ros-navigation#3251)

* issue with finding key

* passed up codes to bt_navigator

* lint fix

* updates

* adding error_code_id back in

* error codes names in params

* bump error codes

* lint

* spelling

* test fix

* update behavior trees

* cleanup

* Update bt_action_server_impl.hpp

* code review

* lint

* code review

* log fix

* error code for waypoint follower

* clean up

* remove waypoint error test, too flaky on CI

* lint and code review

* rough imp for waypoint changes

* lint

* code review

* build fix

* clean up

* revert

* space

* remove

* try to make github happ

* stop gap

* loading in param file

* working tests :)

* lint

* fixed cmake

* lint

* lint

* trigger build

* added invalid plugin error

* added test for piping up error codes

* clean up

* test waypoint follower

* only launch what is needed

* waypoint test

* revert lines for robot navigator

* fix test

* waypoint test

* switched to uint16

* clean up

* code review

* todo to note

* lint

* remove comment

* Update nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* rename error_codes

* error code for navigate to pose

* error codes for navigate through poses.

* error codes for navigate through poses

* message update for waypoint follower

* rename to error code

* update node xml

Co-authored-by: Joshua Wallace <josho.wallace.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Solve bug when CostmapInfoServer is reactivated (ros-navigation#3292)

* Solve bug when CostmapInfoServer is reactivated

* Smoothness metrics update (ros-navigation#3294)

* Update metrics for path smoothness

* Support Savitzky-Golay smoother

* preempt/cancel test for time behavior, spin pluguin (ros-navigation#3301)

* include preempt/cancel test for time behavior, spin pluguin

* linting

* fix bug in code

* lint fix

* clean up test

* lint

* cleaned up test

* update

* revert Cmake

Signed-off-by: Owen Hooper <17ofh@queensu.ca>
Co-authored-by: Joshua Wallace <josho.wallace.com>
Co-authored-by: Hao-Xuan Song <44140526+Cryst4L9527@users.noreply.github.com>
Co-authored-by: jaeminSHIN <91681721+woawo1213@users.noreply.github.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
Co-authored-by: Afif Swaidan <53655365+afifswaidan@users.noreply.github.com>
Co-authored-by: Afif Swaidan <afif.swaidan@spexal.com>
Co-authored-by: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com>
Co-authored-by: Pradheep Krishna <padhupradheep@gmail.com>
Co-authored-by: Erwin Lejeune <erwin.lejeune15@gmail.com>
Co-authored-by: Adam Aposhian <aposhian.dev@gmail.com>
Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com>
Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com>
Co-authored-by: Owen Hooper <17ofh@queensu.ca>
Co-authored-by: MartiBolet <43337758+MartiBolet@users.noreply.github.com>
  • Loading branch information
14 people authored and Andrew Lycas committed Feb 23, 2023
1 parent 43d3206 commit 9c7ac84
Show file tree
Hide file tree
Showing 6 changed files with 263 additions and 7 deletions.
4 changes: 3 additions & 1 deletion nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -320,7 +320,9 @@ class Costmap2DROS : public nav2_util::LifecycleNode
// Publishers and subscribers
rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PolygonStamped>::SharedPtr
footprint_pub_;
std::unique_ptr<Costmap2DPublisher> costmap_publisher_{nullptr};
std::unique_ptr<Costmap2DPublisher> costmap_publisher_;

std::vector<std::unique_ptr<Costmap2DPublisher>> layer_publishers_;

rclcpp::Subscription<geometry_msgs::msg::Polygon>::SharedPtr footprint_sub_;
rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr parameter_sub_;
Expand Down
2 changes: 1 addition & 1 deletion nav2_costmap_2d/src/costmap_2d_publisher.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -187,8 +187,8 @@ void Costmap2DPublisher::publishCostmap()
prepareCostmap();
costmap_raw_pub_->publish(std::move(costmap_raw_));
}
float resolution = costmap_->getResolution();

float resolution = costmap_->getResolution();
if (always_send_full_costmap_ || grid_resolution != resolution ||
grid_width != costmap_->getSizeInCellsX() ||
grid_height != costmap_->getSizeInCellsY() ||
Expand Down
48 changes: 43 additions & 5 deletions nav2_costmap_2d/src/costmap_2d_ros.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -210,6 +210,20 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/)
layered_costmap_->getCostmap(), global_frame_,
"costmap", always_send_full_costmap_);

auto layers = layered_costmap_->getPlugins();

for (auto & layer : *layers) {
auto costmap_layer = std::dynamic_pointer_cast<CostmapLayer>(layer);
if (costmap_layer != nullptr) {
layer_publishers_.emplace_back(
std::make_unique<Costmap2DPublisher>(
shared_from_this(),
costmap_layer.get(), global_frame_,
layer->getName(), always_send_full_costmap_)
);
}
}

// Set the footprint
if (use_radius_) {
setRobotFootprint(makeFootprintFromRadius(robot_radius_));
Expand All @@ -233,8 +247,12 @@ Costmap2DROS::on_activate(const rclcpp_lifecycle::State & /*state*/)
{
RCLCPP_INFO(get_logger(), "Activating");

costmap_publisher_->on_activate();
footprint_pub_->on_activate();
costmap_publisher_->on_activate();

for (auto & layer_pub : layer_publishers_) {
layer_pub->on_activate();
}

// First, make sure that the transform between the robot base frame
// and the global frame is available
Expand Down Expand Up @@ -280,8 +298,13 @@ Costmap2DROS::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
RCLCPP_INFO(get_logger(), "Deactivating");

dyn_params_handler.reset();
costmap_publisher_->on_deactivate();

footprint_pub_->on_deactivate();
costmap_publisher_->on_deactivate();

for (auto & layer_pub : layer_publishers_) {
layer_pub->on_deactivate();
}

stop();

Expand All @@ -297,6 +320,13 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
{
RCLCPP_INFO(get_logger(), "Cleaning up");

costmap_publisher_.reset();
clear_costmap_service_.reset();

for (auto & layer_pub : layer_publishers_) {
layer_pub.reset();
}

layered_costmap_.reset();

tf_listener_.reset();
Expand All @@ -305,8 +335,6 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
footprint_sub_.reset();
footprint_pub_.reset();

costmap_publisher_.reset();
clear_costmap_service_.reset();

executor_thread_.reset();
return nav2_util::CallbackReturn::SUCCESS;
Expand Down Expand Up @@ -448,12 +476,22 @@ Costmap2DROS::mapUpdateLoop(double frequency)
layered_costmap_->getBounds(&x0, &xn, &y0, &yn);
costmap_publisher_->updateBounds(x0, xn, y0, yn);

for (auto &layer_pub: layer_publishers_) {
layer_pub->updateBounds(x0, xn, y0, yn);
}

auto current_time = now();
if ((last_publish_ + publish_cycle_ < current_time) || // publish_cycle_ is due
(current_time < last_publish_)) // time has moved backwards, probably due to a switch to sim_time // NOLINT
(current_time <
last_publish_)) // time has moved backwards, probably due to a switch to sim_time // NOLINT
{
RCLCPP_DEBUG(get_logger(), "Publish costmap at %s", name_.c_str());
costmap_publisher_->publishCostmap();

for (auto &layer_pub: layer_publishers_) {
layer_pub->publishCostmap();
}

last_publish_ = current_time;
}
}
Expand Down
22 changes: 22 additions & 0 deletions nav2_costmap_2d/test/integration/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -64,6 +64,18 @@ target_link_libraries(dyn_params_tests
nav2_costmap_2d_core
)

ament_add_gtest_executable(test_costmap_publisher_exec
test_costmap_2d_publisher.cpp
)
ament_target_dependencies(test_costmap_publisher_exec
${dependencies}
)
target_link_libraries(test_costmap_publisher_exec
nav2_costmap_2d_core
nav2_costmap_2d_client
layers
)

ament_add_test(test_collision_checker
GENERATE_RESULT_FOR_RETURN_CODE_ZERO
COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py"
Expand Down Expand Up @@ -114,6 +126,16 @@ ament_add_test(range_tests
TEST_EXECUTABLE=$<TARGET_FILE:range_tests_exec>
)

ament_add_test(test_costmap_publisher_exec
GENERATE_RESULT_FOR_RETURN_CODE_ZERO
COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py"
WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
ENV
TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml
TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR}
TEST_EXECUTABLE=$<TARGET_FILE:test_costmap_publisher_exec>
)

## TODO(bpwilcox): this test (I believe) is intended to be launched with the simple_driving_test.xml,
## which has a dependency on rosbag playback
# ament_add_gtest_executable(costmap_tester
Expand Down
16 changes: 16 additions & 0 deletions nav2_costmap_2d/test/integration/costmap_tests_launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,20 @@ def main(argv=sys.argv[1:]):
launchFile = os.path.join(os.getenv('TEST_LAUNCH_DIR'), 'costmap_map_server.launch.py')
testExecutable = os.getenv('TEST_EXECUTABLE')

map_to_odom = launch_ros.actions.Node(
package='tf2_ros',
executable='static_transform_publisher',
output='screen',
arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom']
)

odom_to_base_link = launch_ros.actions.Node(
package='tf2_ros',
executable='static_transform_publisher',
output='screen',
arguments=['0', '0', '0', '0', '0', '0', 'odom', 'base_link']
)

lifecycle_manager = launch_ros.actions.Node(
package='nav2_lifecycle_manager',
executable='lifecycle_manager',
Expand All @@ -39,6 +53,8 @@ def main(argv=sys.argv[1:]):

ld = LaunchDescription([
IncludeLaunchDescription(PythonLaunchDescriptionSource([launchFile])),
map_to_odom,
odom_to_base_link,
lifecycle_manager
])

Expand Down
178 changes: 178 additions & 0 deletions nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,178 @@
// Copyright (c) 2022 Joshua Wallace
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.

#include <gtest/gtest.h>

#include <future>

#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_costmap_2d/costmap_subscriber.hpp"
#include "nav2_costmap_2d/cost_values.hpp"
#include "tf2_ros/transform_listener.h"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"

class RclCppFixture
{
public:
RclCppFixture() {rclcpp::init(0, nullptr);}
~RclCppFixture() {rclcpp::shutdown();}
};
RclCppFixture g_rclcppfixture;

class CostmapRosLifecycleNode : public nav2_util::LifecycleNode
{
public:
explicit CostmapRosLifecycleNode(const std::string & name)
: LifecycleNode(name),
name_(name) {}

~CostmapRosLifecycleNode() override = default;

nav2_util::CallbackReturn
on_configure(const rclcpp_lifecycle::State &) override
{
costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
name_,
std::string{get_namespace()},
name_,
get_parameter("use_sim_time").as_bool());
costmap_thread_ = std::make_unique<nav2_util::NodeThread>(costmap_ros_);

costmap_ros_->configure();

return nav2_util::CallbackReturn::SUCCESS;
}

nav2_util::CallbackReturn
on_activate(const rclcpp_lifecycle::State &) override
{
costmap_ros_->activate();
return nav2_util::CallbackReturn::SUCCESS;
}

nav2_util::CallbackReturn
on_deactivate(const rclcpp_lifecycle::State &) override
{
costmap_ros_->deactivate();
return nav2_util::CallbackReturn::SUCCESS;
}

nav2_util::CallbackReturn
on_cleanup(const rclcpp_lifecycle::State &) override
{
costmap_thread_.reset();
costmap_ros_->deactivate();
return nav2_util::CallbackReturn::SUCCESS;
}

protected:
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
std::unique_ptr<nav2_util::NodeThread> costmap_thread_;
const std::string name_;
};

class LayerSubscriber
{
public:
explicit LayerSubscriber(const nav2_util::LifecycleNode::WeakPtr & parent)
{
auto node = parent.lock();

callback_group_ = node->create_callback_group(
rclcpp::CallbackGroupType::MutuallyExclusive, false);

rclcpp::SubscriptionOptions sub_option;
sub_option.callback_group = callback_group_;

std::string topic_name = "/fake_costmap/static_layer_raw";
layer_sub_ = node->create_subscription<nav2_msgs::msg::Costmap>(
topic_name,
rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
std::bind(&LayerSubscriber::layerCallback, this, std::placeholders::_1),
sub_option);

executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
executor_->add_callback_group(callback_group_, node->get_node_base_interface());
executor_thread_ = std::make_unique<nav2_util::NodeThread>(executor_);
}

~LayerSubscriber()
{
executor_thread_.reset();
}

std::promise<nav2_msgs::msg::Costmap::SharedPtr> layer_promise_;

protected:
void layerCallback(const nav2_msgs::msg::Costmap::SharedPtr layer)
{
if (!callback_hit_) {
layer_promise_.set_value(layer);
callback_hit_ = true;
}
}

rclcpp::Subscription<nav2_msgs::msg::Costmap>::SharedPtr layer_sub_;
rclcpp::CallbackGroup::SharedPtr callback_group_;
rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
std::unique_ptr<nav2_util::NodeThread> executor_thread_;
bool callback_hit_{false};
};

class CostmapRosTestFixture : public ::testing::Test
{
public:
CostmapRosTestFixture()
{
costmap_lifecycle_node_ = std::make_shared<CostmapRosLifecycleNode>("fake_costmap");
layer_subscriber_ = std::make_shared<LayerSubscriber>(
costmap_lifecycle_node_->shared_from_this());
costmap_lifecycle_node_->on_configure(costmap_lifecycle_node_->get_current_state());
costmap_lifecycle_node_->on_activate(costmap_lifecycle_node_->get_current_state());
}

~CostmapRosTestFixture() override
{
costmap_lifecycle_node_->on_deactivate(costmap_lifecycle_node_->get_current_state());
costmap_lifecycle_node_->on_cleanup(costmap_lifecycle_node_->get_current_state());
}

protected:
std::shared_ptr<CostmapRosLifecycleNode> costmap_lifecycle_node_;
std::shared_ptr<LayerSubscriber> layer_subscriber_;
};

TEST_F(CostmapRosTestFixture, costmap_pub_test)
{
auto future = layer_subscriber_->layer_promise_.get_future();
auto status = future.wait_for(std::chrono::seconds(5));
EXPECT_TRUE(status == std::future_status::ready);

auto costmap_raw = future.get();

// Check first 20 cells of the 10by10 map
unsigned int i = 0;
for (; i < 7; ++i) {
EXPECT_EQ(costmap_raw->data[i], nav2_costmap_2d::FREE_SPACE);
}
for (; i < 10; ++i) {
EXPECT_EQ(costmap_raw->data[i++], nav2_costmap_2d::LETHAL_OBSTACLE);
}
for (; i < 17; ++i) {
EXPECT_EQ(costmap_raw->data[i], nav2_costmap_2d::FREE_SPACE);
}
for (; i < 20; ++i) {
EXPECT_EQ(costmap_raw->data[i++], nav2_costmap_2d::LETHAL_OBSTACLE);
}
}

0 comments on commit 9c7ac84

Please sign in to comment.