Skip to content

Commit

Permalink
Merge branch 'main' into logivations/fix_flickering_visualization
Browse files Browse the repository at this point in the history
Signed-off-by: Vladyslav Hrynchak <vladyslav.hrynchak@logivations.com>
  • Loading branch information
VladyslavHrynchak200204 authored Jul 29, 2024
2 parents fd0f756 + 4516a6a commit 1053ff9
Show file tree
Hide file tree
Showing 21 changed files with 543 additions and 27 deletions.
2 changes: 1 addition & 1 deletion README.md
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@ For detailed instructions on how to:
- [Container Images for Building Nav2](https://github.com/orgs/ros-navigation/packages/container/package/navigation2)
- [Contribute](https://docs.nav2.org/development_guides/involvement_docs/index.html)

Please visit our [documentation site](https://docs.nav2.org/). [Please visit our community Slack here](https://join.slack.com/t/navigation2/shared_invite/zt-hu52lnnq-cKYjuhTY~sEMbZXL8p9tOw) (if this link does not work, please contact maintainers to reactivate).
Please visit our [documentation site](https://docs.nav2.org/). [Please visit our community Slack here](https://join.slack.com/t/navigation2/shared_invite/zt-uj428p0x-jKx8U7OzK1IOWp5TnDS2rA) (if this link does not work, please contact maintainers to reactivate).

If you need professional services related to Nav2, please contact Open Navigation at info@opennav.org.

Expand Down
1 change: 1 addition & 0 deletions nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,7 @@ inline BT::NodeStatus GoalUpdater::tick()

getInput("input_goal", goal);

// Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin
callback_group_executor_.spin_all(std::chrono::milliseconds(1));
callback_group_executor_.spin_all(std::chrono::milliseconds(49));

Expand Down
13 changes: 13 additions & 0 deletions nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -47,10 +47,12 @@
#include "geometry_msgs/msg/polygon_stamped.h"
#include "nav2_costmap_2d/costmap_2d_publisher.hpp"
#include "nav2_costmap_2d/footprint.hpp"
#include "nav2_costmap_2d/footprint_collision_checker.hpp"
#include "nav2_costmap_2d/clear_costmap_service.hpp"
#include "nav2_costmap_2d/layered_costmap.hpp"
#include "nav2_costmap_2d/layer.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_msgs/srv/get_cost.hpp"
#include "pluginlib/class_loader.hpp"
#include "tf2/convert.h"
#include "tf2/LinearMath/Transform.h"
Expand Down Expand Up @@ -339,6 +341,15 @@ class Costmap2DROS : public nav2_util::LifecycleNode
*/
double getRobotRadius() {return robot_radius_;}

/** @brief Get the cost at a point in costmap
* @param request x and y coordinates in map
* @param response cost of the point
*/
void getCostCallback(
const std::shared_ptr<rmw_request_id_t>,
const std::shared_ptr<nav2_msgs::srv::GetCost::Request> request,
const std::shared_ptr<nav2_msgs::srv::GetCost::Response> response);

protected:
// Publishers and subscribers
rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PolygonStamped>::SharedPtr
Expand Down Expand Up @@ -413,6 +424,8 @@ class Costmap2DROS : public nav2_util::LifecycleNode
std::vector<geometry_msgs::msg::Point> unpadded_footprint_;
std::vector<geometry_msgs::msg::Point> padded_footprint_;

// Services
rclcpp::Service<nav2_msgs::srv::GetCost>::SharedPtr get_cost_service_;
std::unique_ptr<ClearCostmapService> clear_costmap_service_;

// Dynamic parameters handler
Expand Down
37 changes: 37 additions & 0 deletions nav2_costmap_2d/src/costmap_2d_ros.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -251,6 +251,12 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/)
setRobotFootprint(new_footprint);
}

// Service to get the cost at a point
get_cost_service_ = create_service<nav2_msgs::srv::GetCost>(
"get_cost_" + getName(),
std::bind(&Costmap2DROS::getCostCallback, this, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3));

// Add cleaning service
clear_costmap_service_ = std::make_unique<ClearCostmapService>(shared_from_this(), *this);

Expand Down Expand Up @@ -819,4 +825,35 @@ Costmap2DROS::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameter
return result;
}

void Costmap2DROS::getCostCallback(
const std::shared_ptr<rmw_request_id_t>,
const std::shared_ptr<nav2_msgs::srv::GetCost::Request> request,
const std::shared_ptr<nav2_msgs::srv::GetCost::Response> response)
{
unsigned int mx, my;

Costmap2D * costmap = layered_costmap_->getCostmap();

if (request->use_footprint) {
Footprint footprint = layered_costmap_->getFootprint();
FootprintCollisionChecker<Costmap2D *> collision_checker(costmap);

RCLCPP_INFO(
get_logger(), "Received request to get cost at footprint pose (%.2f, %.2f, %.2f)",
request->x, request->y, request->theta);

response->cost = collision_checker.footprintCostAtPose(
request->x, request->y, request->theta, footprint);
} else if (costmap->worldToMap(request->x, request->y, mx, my)) {
RCLCPP_INFO(
get_logger(), "Received request to get cost at point (%f, %f)", request->x, request->y);

// Get the cost at the map coordinates
response->cost = static_cast<float>(costmap->getCost(mx, my));
} else {
RCLCPP_WARN(get_logger(), "Point (%f, %f) is out of bounds", request->x, request->y);
response->cost = -1.0;
}
}

} // namespace nav2_costmap_2d
22 changes: 20 additions & 2 deletions nav2_costmap_2d/test/integration/costmap_tests_launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -36,14 +36,32 @@ def main(argv=sys.argv[1:]):
package='tf2_ros',
executable='static_transform_publisher',
output='screen',
arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'],
arguments=[
'--x', '0',
'--y', '0',
'--z', '0',
'--roll', '0',
'--pitch', '0',
'--yaw', '0',
'--frame-id', 'map',
'--child-frame-id', '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'],
arguments=[
'--x', '0',
'--y', '0',
'--z', '0',
'--roll', '0',
'--pitch', '0',
'--yaw', '0',
'--frame-id', 'odom',
'--child-frame-id', 'base_link'
],
)

lifecycle_manager = launch_ros.actions.Node(
Expand Down
5 changes: 5 additions & 0 deletions nav2_costmap_2d/test/unit/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,11 @@ target_link_libraries(costmap_convesion_test
${PROJECT_NAME}::nav2_costmap_2d_core
)

ament_add_gtest(costmap_cost_service_test costmap_cost_service_test.cpp)
target_link_libraries(costmap_cost_service_test
${PROJECT_NAME}::nav2_costmap_2d_core
)

ament_add_gtest(declare_parameter_test declare_parameter_test.cpp)
target_link_libraries(declare_parameter_test
${PROJECT_NAME}::nav2_costmap_2d_core
Expand Down
89 changes: 89 additions & 0 deletions nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,89 @@
// Copyright (c) 2024 Jatin Patil
//
// 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.

#include <gtest/gtest.h>

#include <memory>
#include <chrono>

#include <rclcpp/rclcpp.hpp>
#include "nav2_msgs/srv/get_cost.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"

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

using namespace std::chrono_literals;

class GetCostServiceTest : public ::testing::Test
{
protected:
void SetUp() override
{
costmap_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>("costmap");
client_ = costmap_->create_client<nav2_msgs::srv::GetCost>(
"/costmap/get_cost_costmap");
costmap_->on_configure(rclcpp_lifecycle::State());
ASSERT_TRUE(client_->wait_for_service(10s));
}

std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_;
rclcpp::Client<nav2_msgs::srv::GetCost>::SharedPtr client_;
};

TEST_F(GetCostServiceTest, TestWithoutFootprint)
{
auto request = std::make_shared<nav2_msgs::srv::GetCost::Request>();
request->x = 0.5;
request->y = 1.0;
request->use_footprint = false;

auto result_future = client_->async_send_request(request);
if (rclcpp::spin_until_future_complete(
costmap_,
result_future) == rclcpp::FutureReturnCode::SUCCESS)
{
auto response = result_future.get();
EXPECT_GE(response->cost, 0.0) << "Cost is less than 0";
EXPECT_LE(response->cost, 255.0) << "Cost is greater than 255";
} else {
FAIL() << "Failed to call service";
}
}

TEST_F(GetCostServiceTest, TestWithFootprint)
{
auto request = std::make_shared<nav2_msgs::srv::GetCost::Request>();
request->x = 1.0;
request->y = 1.0;
request->theta = 0.5;
request->use_footprint = true;

auto result_future = client_->async_send_request(request);
if (rclcpp::spin_until_future_complete(
costmap_,
result_future) == rclcpp::FutureReturnCode::SUCCESS)
{
auto response = result_future.get();
EXPECT_GE(response->cost, 0.0) << "Cost is less than 0";
EXPECT_LE(response->cost, 255.0) << "Cost is greater than 255";
} else {
FAIL() << "Failed to call service";
}
}
1 change: 1 addition & 0 deletions nav2_msgs/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"msg/Particle.msg"
"msg/ParticleCloud.msg"
"msg/MissedWaypoint.msg"
"srv/GetCost.srv"
"srv/GetCostmap.srv"
"srv/IsPathValid.srv"
"srv/ClearCostmapExceptRegion.srv"
Expand Down
8 changes: 8 additions & 0 deletions nav2_msgs/srv/GetCost.srv
Original file line number Diff line number Diff line change
@@ -0,0 +1,8 @@
# Get costmap cost at given point

bool use_footprint
float32 x
float32 y
float32 theta
---
float32 cost
2 changes: 2 additions & 0 deletions nav2_rviz_plugins/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,7 @@ find_package(visualization_msgs REQUIRED)
find_package(yaml_cpp_vendor REQUIRED)

set(nav2_rviz_plugins_headers_to_moc
include/nav2_rviz_plugins/costmap_cost_tool.hpp
include/nav2_rviz_plugins/docking_panel.hpp
include/nav2_rviz_plugins/goal_pose_updater.hpp
include/nav2_rviz_plugins/goal_common.hpp
Expand All @@ -52,6 +53,7 @@ include_directories(
set(library_name ${PROJECT_NAME})

add_library(${library_name} SHARED
src/costmap_cost_tool.cpp
src/docking_panel.cpp
src/goal_tool.cpp
src/nav2_panel.cpp
Expand Down
62 changes: 62 additions & 0 deletions nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,62 @@
// Copyright (c) 2024 Jatin Patil
//
// 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.

#ifndef NAV2_RVIZ_PLUGINS__COSTMAP_COST_TOOL_HPP_
#define NAV2_RVIZ_PLUGINS__COSTMAP_COST_TOOL_HPP_

#include <nav2_msgs/srv/get_cost.hpp>
#include <rviz_common/tool.hpp>
#include <rviz_default_plugins/tools/point/point_tool.hpp>
#include <rclcpp/rclcpp.hpp>

namespace nav2_rviz_plugins
{
class CostmapCostTool : public rviz_common::Tool
{
Q_OBJECT

public:
CostmapCostTool();
virtual ~CostmapCostTool();

void onInitialize() override;
void activate() override;
void deactivate() override;

int processMouseEvent(rviz_common::ViewportMouseEvent & event) override;

void callCostService(float x, float y);

void handleLocalCostResponse(rclcpp::Client<nav2_msgs::srv::GetCost>::SharedFuture);
void handleGlobalCostResponse(rclcpp::Client<nav2_msgs::srv::GetCost>::SharedFuture);

private Q_SLOTS:
void updateAutoDeactivate();

private:
rclcpp::Client<nav2_msgs::srv::GetCost>::SharedPtr local_cost_client_;
rclcpp::Client<nav2_msgs::srv::GetCost>::SharedPtr global_cost_client_;
rclcpp::Node::SharedPtr node_;

QCursor std_cursor_;
QCursor hit_cursor_;
rviz_common::properties::BoolProperty * auto_deactivate_property_;
rviz_common::properties::QosProfileProperty * qos_profile_property_;

rclcpp::QoS qos_profile_;
};

} // namespace nav2_rviz_plugins

#endif // NAV2_RVIZ_PLUGINS__COSTMAP_COST_TOOL_HPP_
6 changes: 6 additions & 0 deletions nav2_rviz_plugins/plugins_description.xml
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,12 @@
<description>The Nav2 rviz panel for dock and undock actions.</description>
</class>

<class name="nav2_rviz_plugins/CostmapCostTool"
type="nav2_rviz_plugins::CostmapCostTool"
base_class_type="rviz_common::Tool">
<description>A Nav2 tool for getting the cost of point in costmap.</description>
</class>

<class name="nav2_rviz_plugins/ParticleCloud"
type="nav2_rviz_plugins::ParticleCloudDisplay"
base_class_type="rviz_common::Display">
Expand Down
Loading

0 comments on commit 1053ff9

Please sign in to comment.