m5p3nc3r/agv_pro_ros2
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

add humble-navigation2

Browse Source
This commit is contained in:
X-lanni
2025-05-27 19:03:40 +08:00
parent 974abb5e1e
commit e74ec539c2
1280 changed files with 204114 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
navigation2/nav2_rviz_plugins/src/goal_tool.cpp
+54
View File
@@ -0,0 +1,54 @@
// Copyright (c) 2019 Intel Corporation
//
// 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 "nav2_rviz_plugins/goal_tool.hpp"
#include <memory>
#include <string>
#include "nav2_rviz_plugins/goal_common.hpp"
#include "rviz_common/display_context.hpp"
#include "rviz_common/load_resource.hpp"
namespace nav2_rviz_plugins
{
GoalTool::GoalTool()
: rviz_default_plugins::tools::PoseTool()
{
shortcut_key_ = 'g';
}
GoalTool::~GoalTool()
{
}
void GoalTool::onInitialize()
{
PoseTool::onInitialize();
setName("Nav2 Goal");
setIcon(rviz_common::loadPixmap("package://rviz_default_plugins/icons/classes/SetGoal.png"));
}
void
GoalTool::onPoseSet(double x, double y, double theta)
{
// Set goal pose on global object GoalUpdater to update nav2 Panel
GoalUpdater.setGoal(x, y, theta, context_->getFixedFrame());
}
} // namespace nav2_rviz_plugins
#include <pluginlib/class_list_macros.hpp> // NOLINT
PLUGINLIB_EXPORT_CLASS(nav2_rviz_plugins::GoalTool, rviz_common::Tool)
navigation2/nav2_rviz_plugins/src/nav2_panel.cpp
+990
View File
@@ -0,0 +1,990 @@
// Copyright (c) 2019 Intel Corporation
//
// 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 "nav2_rviz_plugins/nav2_panel.hpp"
#include <QtConcurrent/QtConcurrent>
#include <QVBoxLayout>
#include <memory>
#include <vector>
#include <utility>
#include <chrono>
#include <string>
#include "nav2_rviz_plugins/goal_common.hpp"
#include "rviz_common/display_context.hpp"
#include "ament_index_cpp/get_package_share_directory.hpp"
using namespace std::chrono_literals;
namespace nav2_rviz_plugins
{
using nav2_util::geometry_utils::orientationAroundZAxis;
// Define global GoalPoseUpdater so that the nav2 GoalTool plugin can access to update goal pose
GoalPoseUpdater GoalUpdater;
Nav2Panel::Nav2Panel(QWidget * parent)
: Panel(parent),
server_timeout_(100)
{
// Create the control button and its tooltip
start_reset_button_ = new QPushButton;
pause_resume_button_ = new QPushButton;
navigation_mode_button_ = new QPushButton;
navigation_status_indicator_ = new QLabel;
localization_status_indicator_ = new QLabel;
navigation_goal_status_indicator_ = new QLabel;
navigation_feedback_indicator_ = new QLabel;
// Create the state machine used to present the proper control button states in the UI
const char * startup_msg = "Configure and activate all nav2 lifecycle nodes";
const char * shutdown_msg = "Deactivate and cleanup all nav2 lifecycle nodes";
const char * cancel_msg = "Cancel navigation";
const char * pause_msg = "Deactivate all nav2 lifecycle nodes";
const char * resume_msg = "Activate all nav2 lifecycle nodes";
const char * single_goal_msg = "Change to waypoint / nav through poses style navigation";
const char * waypoint_goal_msg = "Start following waypoints";
const char * nft_goal_msg = "Start navigating through poses";
const char * cancel_waypoint_msg = "Cancel waypoint / viapoint accumulation mode";
const QString navigation_active("<table><tr><td width=100><b>Navigation:</b></td>"
"<td><font color=green>active</color></td></tr></table>");
const QString navigation_inactive("<table><tr><td width=100><b>Navigation:</b></td>"
"<td>inactive</td></tr></table>");
const QString navigation_unknown("<table><tr><td width=100><b>Navigation:</b></td>"
"<td>unknown</td></tr></table>");
const QString localization_active("<table><tr><td width=100><b>Localization:</b></td>"
"<td><font color=green>active</color></td></tr></table>");
const QString localization_inactive("<table><tr><td width=100><b>Localization:</b></td>"
"<td>inactive</td></tr></table>");
const QString localization_unknown("<table><tr><td width=100><b>Localization:</b></td>"
"<td>unknown</td></tr></table>");
navigation_status_indicator_->setText(navigation_unknown);
localization_status_indicator_->setText(localization_unknown);
navigation_goal_status_indicator_->setText(getGoalStatusLabel());
navigation_feedback_indicator_->setText(getNavThroughPosesFeedbackLabel());
navigation_status_indicator_->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
localization_status_indicator_->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
navigation_goal_status_indicator_->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
navigation_feedback_indicator_->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
pre_initial_ = new QState();
pre_initial_->setObjectName("pre_initial");
pre_initial_->assignProperty(start_reset_button_, "text", "Startup");
pre_initial_->assignProperty(start_reset_button_, "enabled", false);
pre_initial_->assignProperty(pause_resume_button_, "text", "Pause");
pre_initial_->assignProperty(pause_resume_button_, "enabled", false);
pre_initial_->assignProperty(
navigation_mode_button_, "text",
"Waypoint / Nav Through Poses Mode");
pre_initial_->assignProperty(navigation_mode_button_, "enabled", false);
initial_ = new QState();
initial_->setObjectName("initial");
initial_->assignProperty(start_reset_button_, "text", "Startup");
initial_->assignProperty(start_reset_button_, "toolTip", startup_msg);
initial_->assignProperty(start_reset_button_, "enabled", true);
initial_->assignProperty(pause_resume_button_, "text", "Pause");
initial_->assignProperty(pause_resume_button_, "enabled", false);
initial_->assignProperty(navigation_mode_button_, "text", "Waypoint / Nav Through Poses Mode");
initial_->assignProperty(navigation_mode_button_, "enabled", false);
// State entered when navigate_to_pose action is not active
idle_ = new QState();
idle_->setObjectName("idle");
idle_->assignProperty(start_reset_button_, "text", "Reset");
idle_->assignProperty(start_reset_button_, "toolTip", shutdown_msg);
idle_->assignProperty(start_reset_button_, "enabled", true);
idle_->assignProperty(pause_resume_button_, "text", "Pause");
idle_->assignProperty(pause_resume_button_, "enabled", true);
idle_->assignProperty(pause_resume_button_, "toolTip", pause_msg);
idle_->assignProperty(navigation_mode_button_, "text", "Waypoint / Nav Through Poses Mode");
idle_->assignProperty(navigation_mode_button_, "enabled", true);
idle_->assignProperty(navigation_mode_button_, "toolTip", single_goal_msg);
// State entered when navigate_to_pose action is not active
accumulating_ = new QState();
accumulating_->setObjectName("accumulating");
accumulating_->assignProperty(start_reset_button_, "text", "Cancel Accumulation");
accumulating_->assignProperty(start_reset_button_, "toolTip", cancel_waypoint_msg);
accumulating_->assignProperty(start_reset_button_, "enabled", true);
accumulating_->assignProperty(pause_resume_button_, "text", "Start Nav Through Poses");
accumulating_->assignProperty(pause_resume_button_, "enabled", true);
accumulating_->assignProperty(pause_resume_button_, "toolTip", nft_goal_msg);
accumulating_->assignProperty(navigation_mode_button_, "text", "Start Waypoint Following");
accumulating_->assignProperty(navigation_mode_button_, "enabled", true);
accumulating_->assignProperty(navigation_mode_button_, "toolTip", waypoint_goal_msg);
accumulated_wp_ = new QState();
accumulated_wp_->setObjectName("accumulated_wp");
accumulated_wp_->assignProperty(start_reset_button_, "text", "Cancel");
accumulated_wp_->assignProperty(start_reset_button_, "toolTip", cancel_msg);
accumulated_wp_->assignProperty(start_reset_button_, "enabled", true);
accumulated_wp_->assignProperty(pause_resume_button_, "text", "Start Nav Through Poses");
accumulated_wp_->assignProperty(pause_resume_button_, "enabled", false);
accumulated_wp_->assignProperty(pause_resume_button_, "toolTip", nft_goal_msg);
accumulated_wp_->assignProperty(navigation_mode_button_, "text", "Start Waypoint Following");
accumulated_wp_->assignProperty(navigation_mode_button_, "enabled", false);
accumulated_wp_->assignProperty(navigation_mode_button_, "toolTip", waypoint_goal_msg);
accumulated_nav_through_poses_ = new QState();
accumulated_nav_through_poses_->setObjectName("accumulated_nav_through_poses");
accumulated_nav_through_poses_->assignProperty(start_reset_button_, "text", "Cancel");
accumulated_nav_through_poses_->assignProperty(start_reset_button_, "toolTip", cancel_msg);
accumulated_nav_through_poses_->assignProperty(start_reset_button_, "enabled", true);
accumulated_nav_through_poses_->assignProperty(
pause_resume_button_, "text",
"Start Nav Through Poses");
accumulated_nav_through_poses_->assignProperty(pause_resume_button_, "enabled", false);
accumulated_nav_through_poses_->assignProperty(pause_resume_button_, "toolTip", nft_goal_msg);
accumulated_nav_through_poses_->assignProperty(
navigation_mode_button_, "text",
"Start Waypoint Following");
accumulated_nav_through_poses_->assignProperty(navigation_mode_button_, "enabled", false);
accumulated_nav_through_poses_->assignProperty(
navigation_mode_button_, "toolTip",
waypoint_goal_msg);
// State entered to cancel the navigate_to_pose action
canceled_ = new QState();
canceled_->setObjectName("canceled");
// State entered to reset the nav2 lifecycle nodes
reset_ = new QState();
reset_->setObjectName("reset");
// State entered while the navigate_to_pose action is active
running_ = new QState();
running_->setObjectName("running");
running_->assignProperty(start_reset_button_, "text", "Cancel");
running_->assignProperty(start_reset_button_, "toolTip", cancel_msg);
running_->assignProperty(pause_resume_button_, "text", "Pause");
running_->assignProperty(pause_resume_button_, "enabled", false);
running_->assignProperty(navigation_mode_button_, "text", "Waypoint mode");
running_->assignProperty(navigation_mode_button_, "enabled", false);
// State entered when pause is requested
paused_ = new QState();
paused_->setObjectName("pausing");
paused_->assignProperty(start_reset_button_, "text", "Reset");
paused_->assignProperty(start_reset_button_, "toolTip", shutdown_msg);
paused_->assignProperty(pause_resume_button_, "text", "Resume");
paused_->assignProperty(pause_resume_button_, "toolTip", resume_msg);
paused_->assignProperty(pause_resume_button_, "enabled", true);
paused_->assignProperty(navigation_mode_button_, "text", "Start navigation");
paused_->assignProperty(navigation_mode_button_, "toolTip", resume_msg);
paused_->assignProperty(navigation_mode_button_, "enabled", true);
// State entered to resume the nav2 lifecycle nodes
resumed_ = new QState();
resumed_->setObjectName("resuming");
QObject::connect(initial_, SIGNAL(exited()), this, SLOT(onStartup()));
QObject::connect(canceled_, SIGNAL(exited()), this, SLOT(onCancel()));
QObject::connect(reset_, SIGNAL(exited()), this, SLOT(onShutdown()));
QObject::connect(paused_, SIGNAL(entered()), this, SLOT(onPause()));
QObject::connect(resumed_, SIGNAL(exited()), this, SLOT(onResume()));
QObject::connect(accumulating_, SIGNAL(entered()), this, SLOT(onAccumulating()));
QObject::connect(accumulated_wp_, SIGNAL(entered()), this, SLOT(onAccumulatedWp()));
QObject::connect(
accumulated_nav_through_poses_, SIGNAL(entered()), this,
SLOT(onAccumulatedNTP()));
// Start/Reset button click transitions
initial_->addTransition(start_reset_button_, SIGNAL(clicked()), idle_);
idle_->addTransition(start_reset_button_, SIGNAL(clicked()), reset_);
running_->addTransition(start_reset_button_, SIGNAL(clicked()), canceled_);
paused_->addTransition(start_reset_button_, SIGNAL(clicked()), reset_);
idle_->addTransition(navigation_mode_button_, SIGNAL(clicked()), accumulating_);
accumulating_->addTransition(navigation_mode_button_, SIGNAL(clicked()), accumulated_wp_);
accumulating_->addTransition(
pause_resume_button_, SIGNAL(
clicked()), accumulated_nav_through_poses_);
accumulating_->addTransition(start_reset_button_, SIGNAL(clicked()), idle_);
accumulated_wp_->addTransition(start_reset_button_, SIGNAL(clicked()), canceled_);
accumulated_nav_through_poses_->addTransition(start_reset_button_, SIGNAL(clicked()), canceled_);
// Internal state transitions
canceled_->addTransition(canceled_, SIGNAL(entered()), idle_);
reset_->addTransition(reset_, SIGNAL(entered()), initial_);
resumed_->addTransition(resumed_, SIGNAL(entered()), idle_);
// Pause/Resume button click transitions
idle_->addTransition(pause_resume_button_, SIGNAL(clicked()), paused_);
paused_->addTransition(pause_resume_button_, SIGNAL(clicked()), resumed_);
// ROSAction Transitions: So when actions are updated remotely (failing, succeeding, etc)
// the state of the application will also update. This means that if in the processing
// states and then goes inactive, move back to the idle state. Vise versa as well.
ROSActionQTransition * idleTransition = new ROSActionQTransition(QActionState::INACTIVE);
idleTransition->setTargetState(running_);
idle_->addTransition(idleTransition);
ROSActionQTransition * runningTransition = new ROSActionQTransition(QActionState::ACTIVE);
runningTransition->setTargetState(idle_);
running_->addTransition(runningTransition);
ROSActionQTransition * idleAccumulatedWpTransition =
new ROSActionQTransition(QActionState::INACTIVE);
idleAccumulatedWpTransition->setTargetState(accumulated_wp_);
idle_->addTransition(idleAccumulatedWpTransition);
ROSActionQTransition * accumulatedWpTransition = new ROSActionQTransition(QActionState::ACTIVE);
accumulatedWpTransition->setTargetState(idle_);
accumulated_wp_->addTransition(accumulatedWpTransition);
ROSActionQTransition * idleAccumulatedNTPTransition =
new ROSActionQTransition(QActionState::INACTIVE);
idleAccumulatedNTPTransition->setTargetState(accumulated_nav_through_poses_);
idle_->addTransition(idleAccumulatedNTPTransition);
ROSActionQTransition * accumulatedNTPTransition = new ROSActionQTransition(QActionState::ACTIVE);
accumulatedNTPTransition->setTargetState(idle_);
accumulated_nav_through_poses_->addTransition(accumulatedNTPTransition);
auto options = rclcpp::NodeOptions().arguments(
{"--ros-args", "--remap", "__node:=rviz_navigation_dialog_action_client", "--"});
client_node_ = std::make_shared<rclcpp::Node>("_", options);
client_nav_ = std::make_shared<nav2_lifecycle_manager::LifecycleManagerClient>(
"lifecycle_manager_navigation", client_node_);
client_loc_ = std::make_shared<nav2_lifecycle_manager::LifecycleManagerClient>(
"lifecycle_manager_localization", client_node_);
initial_thread_ = new InitialThread(client_nav_, client_loc_);
connect(initial_thread_, &InitialThread::finished, initial_thread_, &QObject::deleteLater);
QSignalTransition * activeSignal = new QSignalTransition(
initial_thread_,
&InitialThread::navigationActive);
activeSignal->setTargetState(idle_);
pre_initial_->addTransition(activeSignal);
QSignalTransition * inactiveSignal = new QSignalTransition(
initial_thread_,
&InitialThread::navigationInactive);
inactiveSignal->setTargetState(initial_);
pre_initial_->addTransition(inactiveSignal);
QObject::connect(
initial_thread_, &InitialThread::navigationActive,
[this, navigation_active] {
navigation_status_indicator_->setText(navigation_active);
});
QObject::connect(
initial_thread_, &InitialThread::navigationInactive,
[this, navigation_inactive] {
navigation_status_indicator_->setText(navigation_inactive);
navigation_goal_status_indicator_->setText(getGoalStatusLabel());
navigation_feedback_indicator_->setText(getNavThroughPosesFeedbackLabel());
});
QObject::connect(
initial_thread_, &InitialThread::localizationActive,
[this, localization_active] {
localization_status_indicator_->setText(localization_active);
});
QObject::connect(
initial_thread_, &InitialThread::localizationInactive,
[this, localization_inactive] {
localization_status_indicator_->setText(localization_inactive);
});
state_machine_.addState(pre_initial_);
state_machine_.addState(initial_);
state_machine_.addState(idle_);
state_machine_.addState(running_);
state_machine_.addState(canceled_);
state_machine_.addState(reset_);
state_machine_.addState(paused_);
state_machine_.addState(resumed_);
state_machine_.addState(accumulating_);
state_machine_.addState(accumulated_wp_);
state_machine_.addState(accumulated_nav_through_poses_);
state_machine_.setInitialState(pre_initial_);
// delay starting initial thread until state machine has started or a race occurs
QObject::connect(&state_machine_, SIGNAL(started()), this, SLOT(startThread()));
state_machine_.start();
// Lay out the items in the panel
QVBoxLayout * main_layout = new QVBoxLayout;
main_layout->addWidget(navigation_status_indicator_);
main_layout->addWidget(localization_status_indicator_);
main_layout->addWidget(navigation_goal_status_indicator_);
main_layout->addWidget(navigation_feedback_indicator_);
main_layout->addWidget(pause_resume_button_);
main_layout->addWidget(start_reset_button_);
main_layout->addWidget(navigation_mode_button_);
main_layout->setContentsMargins(10, 10, 10, 10);
setLayout(main_layout);
navigation_action_client_ =
rclcpp_action::create_client<nav2_msgs::action::NavigateToPose>(
client_node_,
"navigate_to_pose");
waypoint_follower_action_client_ =
rclcpp_action::create_client<nav2_msgs::action::FollowWaypoints>(
client_node_,
"follow_waypoints");
nav_through_poses_action_client_ =
rclcpp_action::create_client<nav2_msgs::action::NavigateThroughPoses>(
client_node_,
"navigate_through_poses");
navigation_goal_ = nav2_msgs::action::NavigateToPose::Goal();
waypoint_follower_goal_ = nav2_msgs::action::FollowWaypoints::Goal();
nav_through_poses_goal_ = nav2_msgs::action::NavigateThroughPoses::Goal();
wp_navigation_markers_pub_ =
client_node_->create_publisher<visualization_msgs::msg::MarkerArray>(
"waypoints",
rclcpp::QoS(1).transient_local());
QObject::connect(
&GoalUpdater, SIGNAL(updateGoal(double,double,double,QString)), // NOLINT
this, SLOT(onNewGoal(double,double,double,QString))); // NOLINT
}
Nav2Panel::~Nav2Panel()
{
}
void
Nav2Panel::onInitialize()
{
auto node = getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node();
// create action feedback subscribers
navigation_feedback_sub_ =
node->create_subscription<nav2_msgs::action::NavigateToPose::Impl::FeedbackMessage>(
"navigate_to_pose/_action/feedback",
rclcpp::SystemDefaultsQoS(),
[this](const nav2_msgs::action::NavigateToPose::Impl::FeedbackMessage::SharedPtr msg) {
navigation_feedback_indicator_->setText(getNavToPoseFeedbackLabel(msg->feedback));
});
nav_through_poses_feedback_sub_ =
node->create_subscription<nav2_msgs::action::NavigateThroughPoses::Impl::FeedbackMessage>(
"navigate_through_poses/_action/feedback",
rclcpp::SystemDefaultsQoS(),
[this](const nav2_msgs::action::NavigateThroughPoses::Impl::FeedbackMessage::SharedPtr msg) {
navigation_feedback_indicator_->setText(getNavThroughPosesFeedbackLabel(msg->feedback));
});
// create action goal status subscribers
navigation_goal_status_sub_ = node->create_subscription<action_msgs::msg::GoalStatusArray>(
"navigate_to_pose/_action/status",
rclcpp::SystemDefaultsQoS(),
[this](const action_msgs::msg::GoalStatusArray::SharedPtr msg) {
navigation_goal_status_indicator_->setText(
getGoalStatusLabel(msg->status_list.back().status));
if (msg->status_list.back().status != action_msgs::msg::GoalStatus::STATUS_EXECUTING) {
navigation_feedback_indicator_->setText(getNavToPoseFeedbackLabel());
}
});
nav_through_poses_goal_status_sub_ = node->create_subscription<action_msgs::msg::GoalStatusArray>(
"navigate_through_poses/_action/status",
rclcpp::SystemDefaultsQoS(),
[this](const action_msgs::msg::GoalStatusArray::SharedPtr msg) {
navigation_goal_status_indicator_->setText(
getGoalStatusLabel(msg->status_list.back().status));
if (msg->status_list.back().status != action_msgs::msg::GoalStatus::STATUS_EXECUTING) {
navigation_feedback_indicator_->setText(getNavThroughPosesFeedbackLabel());
}
});
}
void
Nav2Panel::startThread()
{
// start initial thread now that state machine is started
initial_thread_->start();
}
void
Nav2Panel::onPause()
{
QFuture<void> futureNav =
QtConcurrent::run(
std::bind(
&nav2_lifecycle_manager::LifecycleManagerClient::pause,
client_nav_.get(), std::placeholders::_1), server_timeout_);
QFuture<void> futureLoc =
QtConcurrent::run(
std::bind(
&nav2_lifecycle_manager::LifecycleManagerClient::pause,
client_loc_.get(), std::placeholders::_1), server_timeout_);
}
void
Nav2Panel::onResume()
{
QFuture<void> futureNav =
QtConcurrent::run(
std::bind(
&nav2_lifecycle_manager::LifecycleManagerClient::resume,
client_nav_.get(), std::placeholders::_1), server_timeout_);
QFuture<void> futureLoc =
QtConcurrent::run(
std::bind(
&nav2_lifecycle_manager::LifecycleManagerClient::resume,
client_loc_.get(), std::placeholders::_1), server_timeout_);
}
void
Nav2Panel::onStartup()
{
QFuture<void> futureNav =
QtConcurrent::run(
std::bind(
&nav2_lifecycle_manager::LifecycleManagerClient::startup,
client_nav_.get(), std::placeholders::_1), server_timeout_);
QFuture<void> futureLoc =
QtConcurrent::run(
std::bind(
&nav2_lifecycle_manager::LifecycleManagerClient::startup,
client_loc_.get(), std::placeholders::_1), server_timeout_);
}
void
Nav2Panel::onShutdown()
{
QFuture<void> futureNav =
QtConcurrent::run(
std::bind(
&nav2_lifecycle_manager::LifecycleManagerClient::reset,
client_nav_.get(), std::placeholders::_1), server_timeout_);
QFuture<void> futureLoc =
QtConcurrent::run(
std::bind(
&nav2_lifecycle_manager::LifecycleManagerClient::reset,
client_loc_.get(), std::placeholders::_1), server_timeout_);
timer_.stop();
}
void
Nav2Panel::onCancel()
{
QFuture<void> future =
QtConcurrent::run(
std::bind(
&Nav2Panel::onCancelButtonPressed,
this));
}
void
Nav2Panel::onNewGoal(double x, double y, double theta, QString frame)
{
auto pose = geometry_msgs::msg::PoseStamped();
pose.header.stamp = rclcpp::Clock().now();
pose.header.frame_id = frame.toStdString();
pose.pose.position.x = x;
pose.pose.position.y = y;
pose.pose.position.z = 0.0;
pose.pose.orientation = orientationAroundZAxis(theta);
if (state_machine_.configuration().contains(accumulating_)) {
acummulated_poses_.push_back(pose);
} else {
std::cout << "Start navigation" << std::endl;
startNavigation(pose);
}
updateWpNavigationMarkers();
}
void
Nav2Panel::onCancelButtonPressed()
{
if (navigation_goal_handle_) {
auto future_cancel = navigation_action_client_->async_cancel_goal(navigation_goal_handle_);
if (rclcpp::spin_until_future_complete(client_node_, future_cancel, server_timeout_) !=
rclcpp::FutureReturnCode::SUCCESS)
{
RCLCPP_ERROR(client_node_->get_logger(), "Failed to cancel goal");
} else {
navigation_goal_handle_.reset();
}
}
if (waypoint_follower_goal_handle_) {
auto future_cancel =
waypoint_follower_action_client_->async_cancel_goal(waypoint_follower_goal_handle_);
if (rclcpp::spin_until_future_complete(client_node_, future_cancel, server_timeout_) !=
rclcpp::FutureReturnCode::SUCCESS)
{
RCLCPP_ERROR(client_node_->get_logger(), "Failed to cancel waypoint follower");
} else {
waypoint_follower_goal_handle_.reset();
}
}
if (nav_through_poses_goal_handle_) {
auto future_cancel =
nav_through_poses_action_client_->async_cancel_goal(nav_through_poses_goal_handle_);
if (rclcpp::spin_until_future_complete(client_node_, future_cancel, server_timeout_) !=
rclcpp::FutureReturnCode::SUCCESS)
{
RCLCPP_ERROR(client_node_->get_logger(), "Failed to cancel nav through pose action");
} else {
nav_through_poses_goal_handle_.reset();
}
}
timer_.stop();
}
void
Nav2Panel::onAccumulatedWp()
{
std::cout << "Start waypoint" << std::endl;
startWaypointFollowing(acummulated_poses_);
acummulated_poses_.clear();
}
void
Nav2Panel::onAccumulatedNTP()
{
std::cout << "Start navigate through poses" << std::endl;
startNavThroughPoses(acummulated_poses_);
acummulated_poses_.clear();
}
void
Nav2Panel::onAccumulating()
{
acummulated_poses_.clear();
}
void
Nav2Panel::timerEvent(QTimerEvent * event)
{
if (state_machine_.configuration().contains(accumulated_wp_)) {
if (event->timerId() == timer_.timerId()) {
if (!waypoint_follower_goal_handle_) {
RCLCPP_DEBUG(client_node_->get_logger(), "Waiting for Goal");
state_machine_.postEvent(new ROSActionQEvent(QActionState::INACTIVE));
return;
}
rclcpp::spin_some(client_node_);
auto status = waypoint_follower_goal_handle_->get_status();
// Check if the goal is still executing
if (status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED ||
status == action_msgs::msg::GoalStatus::STATUS_EXECUTING)
{
state_machine_.postEvent(new ROSActionQEvent(QActionState::ACTIVE));
} else {
state_machine_.postEvent(new ROSActionQEvent(QActionState::INACTIVE));
timer_.stop();
}
}
} else if (state_machine_.configuration().contains(accumulated_nav_through_poses_)) {
if (event->timerId() == timer_.timerId()) {
if (!nav_through_poses_goal_handle_) {
RCLCPP_DEBUG(client_node_->get_logger(), "Waiting for Goal");
state_machine_.postEvent(new ROSActionQEvent(QActionState::INACTIVE));
return;
}
rclcpp::spin_some(client_node_);
auto status = nav_through_poses_goal_handle_->get_status();
// Check if the goal is still executing
if (status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED ||
status == action_msgs::msg::GoalStatus::STATUS_EXECUTING)
{
state_machine_.postEvent(new ROSActionQEvent(QActionState::ACTIVE));
} else {
state_machine_.postEvent(new ROSActionQEvent(QActionState::INACTIVE));
timer_.stop();
}
}
} else {
if (event->timerId() == timer_.timerId()) {
if (!navigation_goal_handle_) {
RCLCPP_DEBUG(client_node_->get_logger(), "Waiting for Goal");
state_machine_.postEvent(new ROSActionQEvent(QActionState::INACTIVE));
return;
}
rclcpp::spin_some(client_node_);
auto status = navigation_goal_handle_->get_status();
// Check if the goal is still executing
if (status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED ||
status == action_msgs::msg::GoalStatus::STATUS_EXECUTING)
{
state_machine_.postEvent(new ROSActionQEvent(QActionState::ACTIVE));
} else {
state_machine_.postEvent(new ROSActionQEvent(QActionState::INACTIVE));
timer_.stop();
}
}
}
}
void
Nav2Panel::startWaypointFollowing(std::vector<geometry_msgs::msg::PoseStamped> poses)
{
auto is_action_server_ready =
waypoint_follower_action_client_->wait_for_action_server(std::chrono::seconds(5));
if (!is_action_server_ready) {
RCLCPP_ERROR(
client_node_->get_logger(), "follow_waypoints action server is not available."
" Is the initial pose set?");
return;
}
// Send the goal poses
waypoint_follower_goal_.poses = poses;
RCLCPP_DEBUG(
client_node_->get_logger(), "Sending a path of %zu waypoints:",
waypoint_follower_goal_.poses.size());
for (auto waypoint : waypoint_follower_goal_.poses) {
RCLCPP_DEBUG(
client_node_->get_logger(),
"\t(%lf, %lf)", waypoint.pose.position.x, waypoint.pose.position.y);
}
// Enable result awareness by providing an empty lambda function
auto send_goal_options =
rclcpp_action::Client<nav2_msgs::action::FollowWaypoints>::SendGoalOptions();
send_goal_options.result_callback = [this](auto) {
waypoint_follower_goal_handle_.reset();
};
auto future_goal_handle =
waypoint_follower_action_client_->async_send_goal(waypoint_follower_goal_, send_goal_options);
if (rclcpp::spin_until_future_complete(client_node_, future_goal_handle, server_timeout_) !=
rclcpp::FutureReturnCode::SUCCESS)
{
RCLCPP_ERROR(client_node_->get_logger(), "Send goal call failed");
return;
}
// Get the goal handle and save so that we can check on completion in the timer callback
waypoint_follower_goal_handle_ = future_goal_handle.get();
if (!waypoint_follower_goal_handle_) {
RCLCPP_ERROR(client_node_->get_logger(), "Goal was rejected by server");
return;
}
timer_.start(200, this);
}
void
Nav2Panel::startNavThroughPoses(std::vector<geometry_msgs::msg::PoseStamped> poses)
{
auto is_action_server_ready =
nav_through_poses_action_client_->wait_for_action_server(std::chrono::seconds(5));
if (!is_action_server_ready) {
RCLCPP_ERROR(
client_node_->get_logger(), "navigate_through_poses action server is not available."
" Is the initial pose set?");
return;
}
nav_through_poses_goal_.poses = poses;
RCLCPP_INFO(
client_node_->get_logger(),
"NavigateThroughPoses will be called using the BT Navigator's default behavior tree.");
RCLCPP_DEBUG(
client_node_->get_logger(), "Sending a path of %zu waypoints:",
nav_through_poses_goal_.poses.size());
for (auto waypoint : nav_through_poses_goal_.poses) {
RCLCPP_DEBUG(
client_node_->get_logger(),
"\t(%lf, %lf)", waypoint.pose.position.x, waypoint.pose.position.y);
}
// Enable result awareness by providing an empty lambda function
auto send_goal_options =
rclcpp_action::Client<nav2_msgs::action::NavigateThroughPoses>::SendGoalOptions();
send_goal_options.result_callback = [this](auto) {
nav_through_poses_goal_handle_.reset();
};
auto future_goal_handle =
nav_through_poses_action_client_->async_send_goal(nav_through_poses_goal_, send_goal_options);
if (rclcpp::spin_until_future_complete(client_node_, future_goal_handle, server_timeout_) !=
rclcpp::FutureReturnCode::SUCCESS)
{
RCLCPP_ERROR(client_node_->get_logger(), "Send goal call failed");
return;
}
// Get the goal handle and save so that we can check on completion in the timer callback
nav_through_poses_goal_handle_ = future_goal_handle.get();
if (!nav_through_poses_goal_handle_) {
RCLCPP_ERROR(client_node_->get_logger(), "Goal was rejected by server");
return;
}
timer_.start(200, this);
}
void
Nav2Panel::startNavigation(geometry_msgs::msg::PoseStamped pose)
{
auto is_action_server_ready =
navigation_action_client_->wait_for_action_server(std::chrono::seconds(5));
if (!is_action_server_ready) {
RCLCPP_ERROR(
client_node_->get_logger(),
"navigate_to_pose action server is not available."
" Is the initial pose set?");
return;
}
// Send the goal pose
navigation_goal_.pose = pose;
RCLCPP_INFO(
client_node_->get_logger(),
"NavigateToPose will be called using the BT Navigator's default behavior tree.");
// Enable result awareness by providing an empty lambda function
auto send_goal_options =
rclcpp_action::Client<nav2_msgs::action::NavigateToPose>::SendGoalOptions();
send_goal_options.result_callback = [this](auto) {
navigation_goal_handle_.reset();
};
auto future_goal_handle =
navigation_action_client_->async_send_goal(navigation_goal_, send_goal_options);
if (rclcpp::spin_until_future_complete(client_node_, future_goal_handle, server_timeout_) !=
rclcpp::FutureReturnCode::SUCCESS)
{
RCLCPP_ERROR(client_node_->get_logger(), "Send goal call failed");
return;
}
// Get the goal handle and save so that we can check on completion in the timer callback
navigation_goal_handle_ = future_goal_handle.get();
if (!navigation_goal_handle_) {
RCLCPP_ERROR(client_node_->get_logger(), "Goal was rejected by server");
return;
}
timer_.start(200, this);
}
void
Nav2Panel::save(rviz_common::Config config) const
{
Panel::save(config);
}
void
Nav2Panel::load(const rviz_common::Config & config)
{
Panel::load(config);
}
void
Nav2Panel::resetUniqueId()
{
unique_id = 0;
}
int
Nav2Panel::getUniqueId()
{
int temp_id = unique_id;
unique_id += 1;
return temp_id;
}
void
Nav2Panel::updateWpNavigationMarkers()
{
resetUniqueId();
auto marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>();
for (size_t i = 0; i < acummulated_poses_.size(); i++) {
// Draw a green arrow at the waypoint pose
visualization_msgs::msg::Marker arrow_marker;
arrow_marker.header = acummulated_poses_[i].header;
arrow_marker.id = getUniqueId();
arrow_marker.type = visualization_msgs::msg::Marker::ARROW;
arrow_marker.action = visualization_msgs::msg::Marker::ADD;
arrow_marker.pose = acummulated_poses_[i].pose;
arrow_marker.scale.x = 0.3;
arrow_marker.scale.y = 0.05;
arrow_marker.scale.z = 0.02;
arrow_marker.color.r = 0;
arrow_marker.color.g = 255;
arrow_marker.color.b = 0;
arrow_marker.color.a = 1.0f;
arrow_marker.lifetime = rclcpp::Duration(0s);
arrow_marker.frame_locked = false;
marker_array->markers.push_back(arrow_marker);
// Draw a red circle at the waypoint pose
visualization_msgs::msg::Marker circle_marker;
circle_marker.header = acummulated_poses_[i].header;
circle_marker.id = getUniqueId();
circle_marker.type = visualization_msgs::msg::Marker::SPHERE;
circle_marker.action = visualization_msgs::msg::Marker::ADD;
circle_marker.pose = acummulated_poses_[i].pose;
circle_marker.scale.x = 0.05;
circle_marker.scale.y = 0.05;
circle_marker.scale.z = 0.05;
circle_marker.color.r = 255;
circle_marker.color.g = 0;
circle_marker.color.b = 0;
circle_marker.color.a = 1.0f;
circle_marker.lifetime = rclcpp::Duration(0s);
circle_marker.frame_locked = false;
marker_array->markers.push_back(circle_marker);
// Draw the waypoint number
visualization_msgs::msg::Marker marker_text;
marker_text.header = acummulated_poses_[i].header;
marker_text.id = getUniqueId();
marker_text.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
marker_text.action = visualization_msgs::msg::Marker::ADD;
marker_text.pose = acummulated_poses_[i].pose;
marker_text.pose.position.z += 0.2; // draw it on top of the waypoint
marker_text.scale.x = 0.07;
marker_text.scale.y = 0.07;
marker_text.scale.z = 0.07;
marker_text.color.r = 0;
marker_text.color.g = 255;
marker_text.color.b = 0;
marker_text.color.a = 1.0f;
marker_text.lifetime = rclcpp::Duration(0s);
marker_text.frame_locked = false;
marker_text.text = "wp_" + std::to_string(i + 1);
marker_array->markers.push_back(marker_text);
}
if (marker_array->markers.empty()) {
visualization_msgs::msg::Marker clear_all_marker;
clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL;
marker_array->markers.push_back(clear_all_marker);
}
wp_navigation_markers_pub_->publish(std::move(marker_array));
}
inline QString
Nav2Panel::getGoalStatusLabel(int8_t status)
{
std::string status_str;
switch (status) {
case action_msgs::msg::GoalStatus::STATUS_EXECUTING:
status_str = "<font color=green>active</color>";
break;
case action_msgs::msg::GoalStatus::STATUS_SUCCEEDED:
status_str = "<font color=green>reached</color>";
break;
case action_msgs::msg::GoalStatus::STATUS_CANCELED:
status_str = "<font color=orange>canceled</color>";
break;
case action_msgs::msg::GoalStatus::STATUS_ABORTED:
status_str = "<font color=red>aborted</color>";
break;
case action_msgs::msg::GoalStatus::STATUS_UNKNOWN:
status_str = "unknown";
break;
default:
status_str = "inactive";
break;
}
return QString(
std::string(
"<table><tr><td width=100><b>Feedback:</b></td><td>" +
status_str + "</td></tr></table>").c_str());
}
inline QString
Nav2Panel::getNavToPoseFeedbackLabel(nav2_msgs::action::NavigateToPose::Feedback msg)
{
return QString(std::string("<table>" + toLabel(msg) + "</table>").c_str());
}
inline QString
Nav2Panel::getNavThroughPosesFeedbackLabel(nav2_msgs::action::NavigateThroughPoses::Feedback msg)
{
return QString(
std::string(
"<table><tr><td width=150>Poses remaining:</td><td>" +
std::to_string(msg.number_of_poses_remaining) +
"</td></tr>" + toLabel(msg) + "</table>").c_str());
}
template<typename T>
inline std::string Nav2Panel::toLabel(T & msg)
{
return std::string(
"<tr><td width=150>ETA:</td><td>" +
toString(rclcpp::Duration(msg.estimated_time_remaining).seconds(), 0) + " s"
"</td></tr><tr><td width=150>Distance remaining:</td><td>" +
toString(msg.distance_remaining, 2) + " m"
"</td></tr><tr><td width=150>Time taken:</td><td>" +
toString(rclcpp::Duration(msg.navigation_time).seconds(), 0) + " s"
"</td></tr><tr><td width=150>Recoveries:</td><td>" +
std::to_string(msg.number_of_recoveries) +
"</td></tr>");
}
inline std::string
Nav2Panel::toString(double val, int precision)
{
std::ostringstream out;
out.precision(precision);
out << std::fixed << val;
return out.str();
}
} // namespace nav2_rviz_plugins
#include <pluginlib/class_list_macros.hpp> // NOLINT
PLUGINLIB_EXPORT_CLASS(nav2_rviz_plugins::Nav2Panel, rviz_common::Panel)
navigation2/nav2_rviz_plugins/src/particle_cloud_display/flat_weighted_arrows_array.cpp
+141
View File
@@ -0,0 +1,141 @@
/*
* Copyright (c) 2012, Willow Garage, Inc.
* Copyright (c) 2018, Bosch Software Innovations GmbH.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
// Copyright (c) 2019 Intel Corporation
// Copyright (c) 2020 Sarthak Mittal
//
// 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 "nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp"
#include <vector>
#include <string>
#include <algorithm>
#include <OgreSceneManager.h>
#include <OgreTechnique.h>
#include "rviz_rendering/material_manager.hpp"
namespace nav2_rviz_plugins
{
FlatWeightedArrowsArray::FlatWeightedArrowsArray(Ogre::SceneManager * scene_manager)
: scene_manager_(scene_manager), manual_object_(nullptr) {}
FlatWeightedArrowsArray::~FlatWeightedArrowsArray()
{
if (manual_object_) {
scene_manager_->destroyManualObject(manual_object_);
}
}
void FlatWeightedArrowsArray::createAndAttachManualObject(Ogre::SceneNode * scene_node)
{
manual_object_ = scene_manager_->createManualObject();
manual_object_->setDynamic(true);
scene_node->attachObject(manual_object_);
}
void FlatWeightedArrowsArray::updateManualObject(
Ogre::ColourValue color,
float alpha,
float min_length,
float max_length,
const std::vector<nav2_rviz_plugins::OgrePoseWithWeight> & poses)
{
clear();
color.a = alpha;
setManualObjectMaterial();
rviz_rendering::MaterialManager::enableAlphaBlending(material_, alpha);
manual_object_->begin(
material_->getName(), Ogre::RenderOperation::OT_LINE_LIST, "rviz_rendering");
setManualObjectVertices(color, min_length, max_length, poses);
manual_object_->end();
}
void FlatWeightedArrowsArray::clear()
{
if (manual_object_) {
manual_object_->clear();
}
}
void FlatWeightedArrowsArray::setManualObjectMaterial()
{
static int material_count = 0;
std::string material_name = "FlatWeightedArrowsMaterial" + std::to_string(material_count++);
material_ = rviz_rendering::MaterialManager::createMaterialWithNoLighting(material_name);
}
void FlatWeightedArrowsArray::setManualObjectVertices(
const Ogre::ColourValue & color,
float min_length,
float max_length,
const std::vector<nav2_rviz_plugins::OgrePoseWithWeight> & poses)
{
manual_object_->estimateVertexCount(poses.size() * 6);
float scale = max_length - min_length;
float length;
for (const auto & pose : poses) {
length = std::min(std::max(pose.weight * scale + min_length, min_length), max_length);
Ogre::Vector3 vertices[6];
vertices[0] = pose.position; // back of arrow
vertices[1] =
pose.position + pose.orientation * Ogre::Vector3(length, 0, 0); // tip of arrow
vertices[2] = vertices[1];
vertices[3] = pose.position + pose.orientation * Ogre::Vector3(
0.75f * length, 0.2f * length, 0);
vertices[4] = vertices[1];
vertices[5] = pose.position + pose.orientation * Ogre::Vector3(
0.75f * length, -0.2f * length,
0);
for (const auto & vertex : vertices) {
manual_object_->position(vertex);
manual_object_->colour(color);
}
}
}
} // namespace nav2_rviz_plugins
navigation2/nav2_rviz_plugins/src/particle_cloud_display/particle_cloud_display.cpp
+423
View File
@@ -0,0 +1,423 @@
/*
* Copyright (c) 2012, Willow Garage, Inc.
* Copyright (c) 2018, Bosch Software Innovations GmbH.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
// Copyright (c) 2019 Intel Corporation
// Copyright (c) 2020 Sarthak Mittal
//
// 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 "nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp"
#include <memory>
#include <string>
#include <OgreManualObject.h>
#include <OgreMaterialManager.h>
#include <OgreTechnique.h>
#include "rviz_common/logging.hpp"
#include "rviz_common/msg_conversions.hpp"
#include "rviz_common/properties/enum_property.hpp"
#include "rviz_common/properties/color_property.hpp"
#include "rviz_common/properties/float_property.hpp"
#include "rviz_common/validate_floats.hpp"
#include "rviz_rendering/objects/arrow.hpp"
#include "rviz_rendering/objects/axes.hpp"
#include "nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp"
namespace nav2_rviz_plugins
{
namespace
{
struct ShapeType
{
enum
{
Arrow2d,
Arrow3d,
Axes,
};
};
} // namespace
ParticleCloudDisplay::ParticleCloudDisplay(
rviz_common::DisplayContext * display_context,
Ogre::SceneNode * scene_node)
: ParticleCloudDisplay()
{
context_ = display_context;
scene_node_ = scene_node;
scene_manager_ = context_->getSceneManager();
arrows2d_ = std::make_unique<FlatWeightedArrowsArray>(scene_manager_);
arrows2d_->createAndAttachManualObject(scene_node);
arrow_node_ = scene_node_->createChildSceneNode();
axes_node_ = scene_node_->createChildSceneNode();
updateShapeChoice();
}
ParticleCloudDisplay::ParticleCloudDisplay()
: min_length_(0.02f), max_length_(0.3f)
{
initializeProperties();
shape_property_->addOption("Arrow (Flat)", ShapeType::Arrow2d);
shape_property_->addOption("Arrow (3D)", ShapeType::Arrow3d);
shape_property_->addOption("Axes", ShapeType::Axes);
arrow_alpha_property_->setMin(0);
arrow_alpha_property_->setMax(1);
arrow_min_length_property_->setMax(max_length_);
arrow_max_length_property_->setMin(min_length_);
}
void ParticleCloudDisplay::initializeProperties()
{
shape_property_ = new rviz_common::properties::EnumProperty(
"Shape", "Arrow (Flat)", "Shape to display the pose as.", this, SLOT(updateShapeChoice()));
arrow_color_property_ = new rviz_common::properties::ColorProperty(
"Color", QColor(255, 25, 0), "Color to draw the arrows.", this, SLOT(updateArrowColor()));
arrow_alpha_property_ = new rviz_common::properties::FloatProperty(
"Alpha",
1.0f,
"Amount of transparency to apply to the displayed poses.",
this,
SLOT(updateArrowColor()));
arrow_min_length_property_ = new rviz_common::properties::FloatProperty(
"Min Arrow Length", min_length_, "Minimum length of the arrows.", this, SLOT(updateGeometry()));
arrow_max_length_property_ = new rviz_common::properties::FloatProperty(
"Max Arrow Length", max_length_, "Maximum length of the arrows.", this, SLOT(updateGeometry()));
// Scales are set based on initial values
length_scale_ = max_length_ - min_length_;
shaft_radius_scale_ = 0.0435;
head_length_scale_ = 0.3043;
head_radius_scale_ = 0.1304;
}
ParticleCloudDisplay::~ParticleCloudDisplay()
{
// because of forward declaration of arrow and axes, destructor cannot be declared in .hpp as
// default
}
void ParticleCloudDisplay::onInitialize()
{
MFDClass::onInitialize();
arrows2d_ = std::make_unique<FlatWeightedArrowsArray>(scene_manager_);
arrows2d_->createAndAttachManualObject(scene_node_);
arrow_node_ = scene_node_->createChildSceneNode();
axes_node_ = scene_node_->createChildSceneNode();
updateShapeChoice();
}
void ParticleCloudDisplay::processMessage(const nav2_msgs::msg::ParticleCloud::ConstSharedPtr msg)
{
if (!validateFloats(*msg)) {
setStatus(
rviz_common::properties::StatusProperty::Error,
"Topic",
"Message contained invalid floating point values (nans or infs)");
return;
}
if (!setTransform(msg->header)) {
return;
}
poses_.resize(msg->particles.size());
for (std::size_t i = 0; i < msg->particles.size(); ++i) {
poses_[i].position = rviz_common::pointMsgToOgre(msg->particles[i].pose.position);
poses_[i].orientation = rviz_common::quaternionMsgToOgre(msg->particles[i].pose.orientation);
poses_[i].weight = static_cast<float>(msg->particles[i].weight);
}
updateDisplay();
context_->queueRender();
}
bool ParticleCloudDisplay::validateFloats(const nav2_msgs::msg::ParticleCloud & msg)
{
for (auto & particle : msg.particles) {
if (!rviz_common::validateFloats(particle.pose) ||
!rviz_common::validateFloats(particle.weight))
{
return false;
}
}
return true;
}
bool ParticleCloudDisplay::setTransform(std_msgs::msg::Header const & header)
{
Ogre::Vector3 position;
Ogre::Quaternion orientation;
if (!context_->getFrameManager()->getTransform(header, position, orientation)) {
setMissingTransformToFixedFrame(header.frame_id);
return false;
}
setTransformOk();
scene_node_->setPosition(position);
scene_node_->setOrientation(orientation);
return true;
}
void ParticleCloudDisplay::updateDisplay()
{
int shape = shape_property_->getOptionInt();
switch (shape) {
case ShapeType::Arrow2d:
updateArrows2d();
arrows3d_.clear();
axes_.clear();
break;
case ShapeType::Arrow3d:
updateArrows3d();
arrows2d_->clear();
axes_.clear();
break;
case ShapeType::Axes:
updateAxes();
arrows2d_->clear();
arrows3d_.clear();
break;
}
}
void ParticleCloudDisplay::updateArrows2d()
{
arrows2d_->updateManualObject(
arrow_color_property_->getOgreColor(),
arrow_alpha_property_->getFloat(),
min_length_,
max_length_,
poses_);
}
void ParticleCloudDisplay::updateArrows3d()
{
while (arrows3d_.size() < poses_.size()) {
arrows3d_.push_back(makeArrow3d());
}
while (arrows3d_.size() > poses_.size()) {
arrows3d_.pop_back();
}
Ogre::Quaternion adjust_orientation(Ogre::Degree(-90), Ogre::Vector3::UNIT_Y);
float shaft_length;
for (std::size_t i = 0; i < poses_.size(); ++i) {
shaft_length = std::min(
std::max(
poses_[i].weight * length_scale_ + min_length_,
min_length_), max_length_);
arrows3d_[i]->set(
shaft_length,
shaft_length * shaft_radius_scale_,
shaft_length * head_length_scale_,
shaft_length * head_radius_scale_
);
arrows3d_[i]->setPosition(poses_[i].position);
arrows3d_[i]->setOrientation(poses_[i].orientation * adjust_orientation);
}
}
void ParticleCloudDisplay::updateAxes()
{
while (axes_.size() < poses_.size()) {
axes_.push_back(makeAxes());
}
while (axes_.size() > poses_.size()) {
axes_.pop_back();
}
float shaft_length;
for (std::size_t i = 0; i < poses_.size(); ++i) {
shaft_length = std::min(
std::max(
poses_[i].weight * length_scale_ + min_length_,
min_length_), max_length_);
axes_[i]->set(shaft_length, shaft_length * shaft_radius_scale_);
axes_[i]->setPosition(poses_[i].position);
axes_[i]->setOrientation(poses_[i].orientation);
}
}
std::unique_ptr<rviz_rendering::Arrow> ParticleCloudDisplay::makeArrow3d()
{
Ogre::ColourValue color = arrow_color_property_->getOgreColor();
color.a = arrow_alpha_property_->getFloat();
auto arrow = std::make_unique<rviz_rendering::Arrow>(
scene_manager_,
arrow_node_,
min_length_,
min_length_ * shaft_radius_scale_,
min_length_ * head_length_scale_,
min_length_ * head_radius_scale_
);
arrow->setColor(color);
return arrow;
}
std::unique_ptr<rviz_rendering::Axes> ParticleCloudDisplay::makeAxes()
{
return std::make_unique<rviz_rendering::Axes>(
scene_manager_,
axes_node_,
min_length_,
min_length_ * shaft_radius_scale_
);
}
void ParticleCloudDisplay::reset()
{
MFDClass::reset();
arrows2d_->clear();
arrows3d_.clear();
axes_.clear();
}
void ParticleCloudDisplay::updateShapeChoice()
{
int shape = shape_property_->getOptionInt();
bool use_axes = shape == ShapeType::Axes;
arrow_color_property_->setHidden(use_axes);
arrow_alpha_property_->setHidden(use_axes);
if (initialized()) {
updateDisplay();
}
}
void ParticleCloudDisplay::updateArrowColor()
{
int shape = shape_property_->getOptionInt();
Ogre::ColourValue color = arrow_color_property_->getOgreColor();
color.a = arrow_alpha_property_->getFloat();
if (shape == ShapeType::Arrow2d) {
updateArrows2d();
} else if (shape == ShapeType::Arrow3d) {
for (const auto & arrow : arrows3d_) {
arrow->setColor(color);
}
}
context_->queueRender();
}
void ParticleCloudDisplay::updateGeometry()
{
min_length_ = arrow_min_length_property_->getFloat();
max_length_ = arrow_max_length_property_->getFloat();
length_scale_ = max_length_ - min_length_;
arrow_min_length_property_->setMax(max_length_);
arrow_max_length_property_->setMin(min_length_);
int shape = shape_property_->getOptionInt();
switch (shape) {
case ShapeType::Arrow2d:
updateArrows2d();
arrows3d_.clear();
axes_.clear();
break;
case ShapeType::Arrow3d:
updateArrow3dGeometry();
arrows2d_->clear();
axes_.clear();
break;
case ShapeType::Axes:
updateAxesGeometry();
arrows2d_->clear();
arrows3d_.clear();
break;
}
context_->queueRender();
}
void ParticleCloudDisplay::updateArrow3dGeometry()
{
float shaft_length;
for (std::size_t i = 0; i < poses_.size() && i < arrows3d_.size(); ++i) {
shaft_length = std::min(
std::max(
poses_[i].weight * length_scale_ + min_length_,
min_length_), max_length_);
arrows3d_[i]->set(
shaft_length,
shaft_length * shaft_radius_scale_,
shaft_length * head_length_scale_,
shaft_length * head_radius_scale_
);
}
}
void ParticleCloudDisplay::updateAxesGeometry()
{
float shaft_length;
for (std::size_t i = 0; i < poses_.size() && i < axes_.size(); ++i) {
shaft_length = std::min(
std::max(
poses_[i].weight * length_scale_ + min_length_,
min_length_), max_length_);
axes_[i]->set(shaft_length, shaft_length * shaft_radius_scale_);
}
}
void ParticleCloudDisplay::setShape(QString shape)
{
shape_property_->setValue(shape);
}
} // namespace nav2_rviz_plugins
#include <pluginlib/class_list_macros.hpp> // NOLINT
PLUGINLIB_EXPORT_CLASS(nav2_rviz_plugins::ParticleCloudDisplay, rviz_common::Display)