-
Notifications
You must be signed in to change notification settings - Fork 1.3k
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge branch 'main' into logivations/fix_flickering_visualization
Signed-off-by: Vladyslav Hrynchak <vladyslav.hrynchak@logivations.com>
- Loading branch information
Showing
21 changed files
with
543 additions
and
27 deletions.
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
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
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
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 |
---|---|---|
@@ -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"; | ||
} | ||
} |
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 |
---|---|---|
@@ -0,0 +1,8 @@ | ||
# Get costmap cost at given point | ||
|
||
bool use_footprint | ||
float32 x | ||
float32 y | ||
float32 theta | ||
--- | ||
float32 cost |
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
62 changes: 62 additions & 0 deletions
62
nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp
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 |
---|---|---|
@@ -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_ |
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
Oops, something went wrong.