add humble-navigation2
This commit is contained in:
X-lanni
@@ -0,0 +1,126 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(nav2_rviz_plugins)
|
||||
|
||||
# Default to C++14
|
||||
if(NOT CMAKE_CXX_STANDARD)
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
endif()
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic -Werror)
|
||||
endif()
|
||||
|
||||
# Qt5 boilerplate options from http://doc.qt.io/qt-5/cmake-manual.html
|
||||
set(CMAKE_INCLUDE_CURRENT_DIR ON)
|
||||
set(CMAKE_AUTOMOC ON)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(nav2_util REQUIRED)
|
||||
find_package(nav2_lifecycle_manager REQUIRED)
|
||||
find_package(nav2_msgs REQUIRED)
|
||||
find_package(nav_msgs REQUIRED)
|
||||
find_package(pluginlib REQUIRED)
|
||||
find_package(Qt5 REQUIRED COMPONENTS Core Gui Widgets Test Concurrent)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(rclcpp_lifecycle REQUIRED)
|
||||
find_package(rviz_common REQUIRED)
|
||||
find_package(rviz_default_plugins REQUIRED)
|
||||
find_package(rviz_ogre_vendor REQUIRED)
|
||||
find_package(rviz_rendering REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(tf2_geometry_msgs REQUIRED)
|
||||
find_package(visualization_msgs REQUIRED)
|
||||
|
||||
set(nav2_rviz_plugins_headers_to_moc
|
||||
include/nav2_rviz_plugins/goal_pose_updater.hpp
|
||||
include/nav2_rviz_plugins/goal_common.hpp
|
||||
include/nav2_rviz_plugins/goal_tool.hpp
|
||||
include/nav2_rviz_plugins/nav2_panel.hpp
|
||||
include/nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp
|
||||
include/nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp
|
||||
)
|
||||
|
||||
include_directories(
|
||||
include
|
||||
)
|
||||
|
||||
set(library_name ${PROJECT_NAME})
|
||||
|
||||
add_library(${library_name} SHARED
|
||||
src/goal_tool.cpp
|
||||
src/nav2_panel.cpp
|
||||
src/particle_cloud_display/flat_weighted_arrows_array.cpp
|
||||
src/particle_cloud_display/particle_cloud_display.cpp
|
||||
${nav2_rviz_plugins_headers_to_moc}
|
||||
)
|
||||
|
||||
set(dependencies
|
||||
geometry_msgs
|
||||
nav2_util
|
||||
nav2_lifecycle_manager
|
||||
nav2_msgs
|
||||
nav_msgs
|
||||
pluginlib
|
||||
Qt5
|
||||
rclcpp
|
||||
rclcpp_lifecycle
|
||||
rviz_common
|
||||
rviz_default_plugins
|
||||
rviz_ogre_vendor
|
||||
rviz_rendering
|
||||
std_msgs
|
||||
tf2_geometry_msgs
|
||||
)
|
||||
|
||||
ament_target_dependencies(${library_name}
|
||||
${dependencies}
|
||||
)
|
||||
|
||||
target_include_directories(${library_name} PUBLIC
|
||||
${Qt5Widgets_INCLUDE_DIRS}
|
||||
${OGRE_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
target_link_libraries(${library_name}
|
||||
rviz_common::rviz_common
|
||||
)
|
||||
|
||||
# Causes the visibility macros to use dllexport rather than dllimport,
|
||||
# which is appropriate when building the dll but not consuming it.
|
||||
# TODO: Make this specific to this project (not rviz default plugins)
|
||||
target_compile_definitions(${library_name} PRIVATE "RVIZ_DEFAULT_PLUGINS_BUILDING_LIBRARY")
|
||||
|
||||
pluginlib_export_plugin_description_file(rviz_common plugins_description.xml)
|
||||
|
||||
install(
|
||||
TARGETS ${library_name}
|
||||
EXPORT ${library_name}
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib
|
||||
RUNTIME DESTINATION bin
|
||||
INCLUDES DESTINATION include
|
||||
)
|
||||
|
||||
install(
|
||||
DIRECTORY include/
|
||||
DESTINATION include/
|
||||
)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_export_include_directories(include)
|
||||
ament_export_targets(${library_name} HAS_LIBRARY_TARGET)
|
||||
ament_export_dependencies(
|
||||
Qt5
|
||||
rviz_common
|
||||
geometry_msgs
|
||||
map_msgs
|
||||
nav_msgs
|
||||
rclcpp
|
||||
)
|
||||
|
||||
ament_package()
|
||||
@@ -0,0 +1,25 @@
|
||||
// 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.
|
||||
|
||||
#ifndef NAV2_RVIZ_PLUGINS__GOAL_COMMON_HPP_
|
||||
#define NAV2_RVIZ_PLUGINS__GOAL_COMMON_HPP_
|
||||
|
||||
#include "nav2_rviz_plugins/goal_pose_updater.hpp"
|
||||
|
||||
namespace nav2_rviz_plugins
|
||||
{
|
||||
extern GoalPoseUpdater GoalUpdater;
|
||||
} // nav2_rviz_plugins
|
||||
|
||||
#endif // NAV2_RVIZ_PLUGINS__GOAL_COMMON_HPP_
|
||||
@@ -0,0 +1,43 @@
|
||||
// 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.
|
||||
|
||||
#ifndef NAV2_RVIZ_PLUGINS__GOAL_POSE_UPDATER_HPP_
|
||||
#define NAV2_RVIZ_PLUGINS__GOAL_POSE_UPDATER_HPP_
|
||||
|
||||
#include <QObject>
|
||||
|
||||
namespace nav2_rviz_plugins
|
||||
{
|
||||
|
||||
/// Class to set and update goal pose by emitting signal
|
||||
class GoalPoseUpdater : public QObject
|
||||
{
|
||||
Q_OBJECT
|
||||
|
||||
public:
|
||||
GoalPoseUpdater() {}
|
||||
~GoalPoseUpdater() {}
|
||||
|
||||
void setGoal(double x, double y, double theta, QString frame)
|
||||
{
|
||||
emit updateGoal(x, y, theta, frame);
|
||||
}
|
||||
|
||||
signals:
|
||||
void updateGoal(double x, double y, double theta, QString frame);
|
||||
};
|
||||
|
||||
} // namespace nav2_rviz_plugins
|
||||
|
||||
#endif // NAV2_RVIZ_PLUGINS__GOAL_POSE_UPDATER_HPP_
|
||||
@@ -0,0 +1,55 @@
|
||||
// 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.
|
||||
|
||||
#ifndef NAV2_RVIZ_PLUGINS__GOAL_TOOL_HPP_
|
||||
#define NAV2_RVIZ_PLUGINS__GOAL_TOOL_HPP_
|
||||
|
||||
#include <QObject>
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include "rviz_default_plugins/tools/pose/pose_tool.hpp"
|
||||
#include "rviz_default_plugins/visibility_control.hpp"
|
||||
|
||||
namespace rviz_common
|
||||
{
|
||||
|
||||
class DisplayContext;
|
||||
|
||||
namespace properties
|
||||
{
|
||||
class StringProperty;
|
||||
} // namespace properties
|
||||
} // namespace rviz_common
|
||||
|
||||
namespace nav2_rviz_plugins
|
||||
{
|
||||
|
||||
class RVIZ_DEFAULT_PLUGINS_PUBLIC GoalTool : public rviz_default_plugins::tools::PoseTool
|
||||
{
|
||||
Q_OBJECT
|
||||
|
||||
public:
|
||||
GoalTool();
|
||||
~GoalTool() override;
|
||||
|
||||
void onInitialize() override;
|
||||
|
||||
protected:
|
||||
void onPoseSet(double x, double y, double theta) override;
|
||||
};
|
||||
|
||||
} // namespace nav2_rviz_plugins
|
||||
|
||||
#endif // NAV2_RVIZ_PLUGINS__GOAL_TOOL_HPP_
|
||||
@@ -0,0 +1,249 @@
|
||||
// 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.
|
||||
|
||||
#ifndef NAV2_RVIZ_PLUGINS__NAV2_PANEL_HPP_
|
||||
#define NAV2_RVIZ_PLUGINS__NAV2_PANEL_HPP_
|
||||
|
||||
#include <QtWidgets>
|
||||
#include <QBasicTimer>
|
||||
#undef NO_ERROR
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "nav2_lifecycle_manager/lifecycle_manager_client.hpp"
|
||||
#include "nav2_msgs/action/navigate_to_pose.hpp"
|
||||
#include "nav2_msgs/action/navigate_through_poses.hpp"
|
||||
#include "nav2_msgs/action/follow_waypoints.hpp"
|
||||
#include "nav2_rviz_plugins/ros_action_qevent.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "rclcpp_action/rclcpp_action.hpp"
|
||||
#include "rviz_common/panel.hpp"
|
||||
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
|
||||
#include "visualization_msgs/msg/marker_array.hpp"
|
||||
#include "nav2_util/geometry_utils.hpp"
|
||||
|
||||
class QPushButton;
|
||||
|
||||
namespace nav2_rviz_plugins
|
||||
{
|
||||
|
||||
class InitialThread;
|
||||
|
||||
/// Panel to interface to the nav2 stack
|
||||
class Nav2Panel : public rviz_common::Panel
|
||||
{
|
||||
Q_OBJECT
|
||||
|
||||
public:
|
||||
explicit Nav2Panel(QWidget * parent = 0);
|
||||
virtual ~Nav2Panel();
|
||||
|
||||
void onInitialize() override;
|
||||
|
||||
/// Load and save configuration data
|
||||
void load(const rviz_common::Config & config) override;
|
||||
void save(rviz_common::Config config) const override;
|
||||
|
||||
private Q_SLOTS:
|
||||
void startThread();
|
||||
void onStartup();
|
||||
void onShutdown();
|
||||
void onCancel();
|
||||
void onPause();
|
||||
void onResume();
|
||||
void onAccumulatedWp();
|
||||
void onAccumulatedNTP();
|
||||
void onAccumulating();
|
||||
void onNewGoal(double x, double y, double theta, QString frame);
|
||||
|
||||
private:
|
||||
void loadLogFiles();
|
||||
void onCancelButtonPressed();
|
||||
void timerEvent(QTimerEvent * event) override;
|
||||
|
||||
int unique_id {0};
|
||||
|
||||
// Call to send NavigateToPose action request for goal poses
|
||||
void startWaypointFollowing(std::vector<geometry_msgs::msg::PoseStamped> poses);
|
||||
void startNavigation(geometry_msgs::msg::PoseStamped);
|
||||
void startNavThroughPoses(std::vector<geometry_msgs::msg::PoseStamped> poses);
|
||||
using NavigationGoalHandle =
|
||||
rclcpp_action::ClientGoalHandle<nav2_msgs::action::NavigateToPose>;
|
||||
using WaypointFollowerGoalHandle =
|
||||
rclcpp_action::ClientGoalHandle<nav2_msgs::action::FollowWaypoints>;
|
||||
using NavThroughPosesGoalHandle =
|
||||
rclcpp_action::ClientGoalHandle<nav2_msgs::action::NavigateThroughPoses>;
|
||||
|
||||
// The (non-spinning) client node used to invoke the action client
|
||||
rclcpp::Node::SharedPtr client_node_;
|
||||
|
||||
// Timeout value when waiting for action servers to respnd
|
||||
std::chrono::milliseconds server_timeout_;
|
||||
|
||||
// A timer used to check on the completion status of the action
|
||||
QBasicTimer timer_;
|
||||
|
||||
// The NavigateToPose action client
|
||||
rclcpp_action::Client<nav2_msgs::action::NavigateToPose>::SharedPtr navigation_action_client_;
|
||||
rclcpp_action::Client<nav2_msgs::action::FollowWaypoints>::SharedPtr
|
||||
waypoint_follower_action_client_;
|
||||
rclcpp_action::Client<nav2_msgs::action::NavigateThroughPoses>::SharedPtr
|
||||
nav_through_poses_action_client_;
|
||||
|
||||
// Navigation action feedback subscribers
|
||||
rclcpp::Subscription<nav2_msgs::action::NavigateToPose::Impl::FeedbackMessage>::SharedPtr
|
||||
navigation_feedback_sub_;
|
||||
rclcpp::Subscription<nav2_msgs::action::NavigateThroughPoses::Impl::FeedbackMessage>::SharedPtr
|
||||
nav_through_poses_feedback_sub_;
|
||||
rclcpp::Subscription<nav2_msgs::action::NavigateToPose::Impl::GoalStatusMessage>::SharedPtr
|
||||
navigation_goal_status_sub_;
|
||||
rclcpp::Subscription<nav2_msgs::action::NavigateThroughPoses::Impl::GoalStatusMessage>::SharedPtr
|
||||
nav_through_poses_goal_status_sub_;
|
||||
|
||||
// Goal-related state
|
||||
nav2_msgs::action::NavigateToPose::Goal navigation_goal_;
|
||||
nav2_msgs::action::FollowWaypoints::Goal waypoint_follower_goal_;
|
||||
nav2_msgs::action::NavigateThroughPoses::Goal nav_through_poses_goal_;
|
||||
NavigationGoalHandle::SharedPtr navigation_goal_handle_;
|
||||
WaypointFollowerGoalHandle::SharedPtr waypoint_follower_goal_handle_;
|
||||
NavThroughPosesGoalHandle::SharedPtr nav_through_poses_goal_handle_;
|
||||
|
||||
// The client used to control the nav2 stack
|
||||
std::shared_ptr<nav2_lifecycle_manager::LifecycleManagerClient> client_nav_;
|
||||
std::shared_ptr<nav2_lifecycle_manager::LifecycleManagerClient> client_loc_;
|
||||
|
||||
QPushButton * start_reset_button_{nullptr};
|
||||
QPushButton * pause_resume_button_{nullptr};
|
||||
QPushButton * navigation_mode_button_{nullptr};
|
||||
|
||||
QLabel * navigation_status_indicator_{nullptr};
|
||||
QLabel * localization_status_indicator_{nullptr};
|
||||
QLabel * navigation_goal_status_indicator_{nullptr};
|
||||
QLabel * navigation_feedback_indicator_{nullptr};
|
||||
|
||||
QStateMachine state_machine_;
|
||||
InitialThread * initial_thread_;
|
||||
|
||||
QState * pre_initial_{nullptr};
|
||||
QState * initial_{nullptr};
|
||||
QState * idle_{nullptr};
|
||||
QState * reset_{nullptr};
|
||||
QState * paused_{nullptr};
|
||||
QState * resumed_{nullptr};
|
||||
// The following states are added to allow for the state of the button to only expose reset
|
||||
// while the NavigateToPoses action is not active. While running, the user will be allowed to
|
||||
// cancel the action. The ROSActionTransition allows for the state of the action to be detected
|
||||
// and the button state to change automatically.
|
||||
QState * running_{nullptr};
|
||||
QState * canceled_{nullptr};
|
||||
// The following states are added to allow to collect several poses to perform a waypoint-mode
|
||||
// navigation or navigate through poses mode.
|
||||
QState * accumulating_{nullptr};
|
||||
QState * accumulated_wp_{nullptr};
|
||||
QState * accumulated_nav_through_poses_{nullptr};
|
||||
|
||||
std::vector<geometry_msgs::msg::PoseStamped> acummulated_poses_;
|
||||
|
||||
// Publish the visual markers with the waypoints
|
||||
void updateWpNavigationMarkers();
|
||||
|
||||
// Create unique id numbers for markers
|
||||
int getUniqueId();
|
||||
|
||||
void resetUniqueId();
|
||||
|
||||
// create label string from goal status msg
|
||||
static inline QString getGoalStatusLabel(
|
||||
int8_t status = action_msgs::msg::GoalStatus::STATUS_UNKNOWN);
|
||||
|
||||
// create label string from feedback msg
|
||||
static inline QString getNavToPoseFeedbackLabel(
|
||||
nav2_msgs::action::NavigateToPose::Feedback msg =
|
||||
nav2_msgs::action::NavigateToPose::Feedback());
|
||||
static inline QString getNavThroughPosesFeedbackLabel(
|
||||
nav2_msgs::action::NavigateThroughPoses::Feedback =
|
||||
nav2_msgs::action::NavigateThroughPoses::Feedback());
|
||||
template<typename T>
|
||||
static inline std::string toLabel(T & msg);
|
||||
|
||||
// round off double to the specified precision and convert to string
|
||||
static inline std::string toString(double val, int precision = 0);
|
||||
|
||||
// Waypoint navigation visual markers publisher
|
||||
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr wp_navigation_markers_pub_;
|
||||
};
|
||||
|
||||
class InitialThread : public QThread
|
||||
{
|
||||
Q_OBJECT
|
||||
|
||||
public:
|
||||
using SystemStatus = nav2_lifecycle_manager::SystemStatus;
|
||||
|
||||
explicit InitialThread(
|
||||
std::shared_ptr<nav2_lifecycle_manager::LifecycleManagerClient> & client_nav,
|
||||
std::shared_ptr<nav2_lifecycle_manager::LifecycleManagerClient> & client_loc)
|
||||
: client_nav_(client_nav), client_loc_(client_loc)
|
||||
{}
|
||||
|
||||
void run() override
|
||||
{
|
||||
SystemStatus status_nav = SystemStatus::TIMEOUT;
|
||||
SystemStatus status_loc = SystemStatus::TIMEOUT;
|
||||
|
||||
while (status_nav == SystemStatus::TIMEOUT) {
|
||||
if (status_nav == SystemStatus::TIMEOUT) {
|
||||
status_nav = client_nav_->is_active(std::chrono::seconds(1));
|
||||
}
|
||||
}
|
||||
|
||||
// try to communicate twice, might not actually be up if in SLAM mode
|
||||
bool tried_loc_bringup_once = false;
|
||||
while (status_loc == SystemStatus::TIMEOUT) {
|
||||
status_loc = client_loc_->is_active(std::chrono::seconds(1));
|
||||
if (tried_loc_bringup_once) {
|
||||
break;
|
||||
}
|
||||
tried_loc_bringup_once = true;
|
||||
}
|
||||
|
||||
if (status_nav == SystemStatus::ACTIVE) {
|
||||
emit navigationActive();
|
||||
} else {
|
||||
emit navigationInactive();
|
||||
}
|
||||
|
||||
if (status_loc == SystemStatus::ACTIVE) {
|
||||
emit localizationActive();
|
||||
} else {
|
||||
emit localizationInactive();
|
||||
}
|
||||
}
|
||||
|
||||
signals:
|
||||
void navigationActive();
|
||||
void navigationInactive();
|
||||
void localizationActive();
|
||||
void localizationInactive();
|
||||
|
||||
private:
|
||||
std::shared_ptr<nav2_lifecycle_manager::LifecycleManagerClient> client_nav_;
|
||||
std::shared_ptr<nav2_lifecycle_manager::LifecycleManagerClient> client_loc_;
|
||||
};
|
||||
|
||||
} // namespace nav2_rviz_plugins
|
||||
|
||||
#endif // NAV2_RVIZ_PLUGINS__NAV2_PANEL_HPP_
|
||||
+94
@@ -0,0 +1,94 @@
|
||||
/*
|
||||
* 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.
|
||||
|
||||
#ifndef NAV2_RVIZ_PLUGINS__PARTICLE_CLOUD_DISPLAY__FLAT_WEIGHTED_ARROWS_ARRAY_HPP_
|
||||
#define NAV2_RVIZ_PLUGINS__PARTICLE_CLOUD_DISPLAY__FLAT_WEIGHTED_ARROWS_ARRAY_HPP_
|
||||
|
||||
#include <vector>
|
||||
|
||||
#include <OgreManualObject.h>
|
||||
#include <OgreMaterialManager.h>
|
||||
#include <OgreSceneNode.h>
|
||||
#include <OgreVector3.h>
|
||||
#include <OgreQuaternion.h>
|
||||
|
||||
#include "nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp"
|
||||
|
||||
namespace nav2_rviz_plugins
|
||||
{
|
||||
|
||||
struct OgrePoseWithWeight;
|
||||
|
||||
class FlatWeightedArrowsArray
|
||||
{
|
||||
public:
|
||||
explicit FlatWeightedArrowsArray(Ogre::SceneManager * scene_manager_);
|
||||
~FlatWeightedArrowsArray();
|
||||
|
||||
void createAndAttachManualObject(Ogre::SceneNode * scene_node);
|
||||
void updateManualObject(
|
||||
Ogre::ColourValue color,
|
||||
float alpha,
|
||||
float min_length,
|
||||
float max_length,
|
||||
const std::vector<nav2_rviz_plugins::OgrePoseWithWeight> & poses);
|
||||
void clear();
|
||||
|
||||
private:
|
||||
void setManualObjectMaterial();
|
||||
void setManualObjectVertices(
|
||||
const Ogre::ColourValue & color,
|
||||
float min_length,
|
||||
float max_length,
|
||||
const std::vector<nav2_rviz_plugins::OgrePoseWithWeight> & poses);
|
||||
|
||||
Ogre::SceneManager * scene_manager_;
|
||||
Ogre::ManualObject * manual_object_;
|
||||
Ogre::MaterialPtr material_;
|
||||
};
|
||||
|
||||
} // namespace nav2_rviz_plugins
|
||||
|
||||
#endif // NAV2_RVIZ_PLUGINS__PARTICLE_CLOUD_DISPLAY__FLAT_WEIGHTED_ARROWS_ARRAY_HPP_
|
||||
+158
@@ -0,0 +1,158 @@
|
||||
/*
|
||||
* 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.
|
||||
|
||||
#ifndef NAV2_RVIZ_PLUGINS__PARTICLE_CLOUD_DISPLAY__PARTICLE_CLOUD_DISPLAY_HPP_
|
||||
#define NAV2_RVIZ_PLUGINS__PARTICLE_CLOUD_DISPLAY__PARTICLE_CLOUD_DISPLAY_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
#include "nav2_msgs/msg/particle_cloud.hpp"
|
||||
|
||||
#include "rviz_rendering/objects/shape.hpp"
|
||||
#include "rviz_common/message_filter_display.hpp"
|
||||
|
||||
namespace Ogre
|
||||
{
|
||||
class ManualObject;
|
||||
} // namespace Ogre
|
||||
|
||||
namespace rviz_common
|
||||
{
|
||||
namespace properties
|
||||
{
|
||||
class EnumProperty;
|
||||
class ColorProperty;
|
||||
class FloatProperty;
|
||||
} // namespace properties
|
||||
} // namespace rviz_common
|
||||
|
||||
namespace rviz_rendering
|
||||
{
|
||||
class Arrow;
|
||||
class Axes;
|
||||
} // namespace rviz_rendering
|
||||
|
||||
namespace nav2_rviz_plugins
|
||||
{
|
||||
class FlatWeightedArrowsArray;
|
||||
struct OgrePoseWithWeight
|
||||
{
|
||||
Ogre::Vector3 position;
|
||||
Ogre::Quaternion orientation;
|
||||
float weight;
|
||||
};
|
||||
|
||||
/** @brief Displays a nav2_msgs/ParticleCloud message as a bunch of line-drawn weighted arrows. */
|
||||
class ParticleCloudDisplay : public rviz_common::MessageFilterDisplay<nav2_msgs::msg::ParticleCloud>
|
||||
{
|
||||
Q_OBJECT
|
||||
|
||||
public:
|
||||
// TODO(botteroa-si): Constructor for testing, remove once ros_nodes can be mocked and call
|
||||
// initialize instead
|
||||
ParticleCloudDisplay(
|
||||
rviz_common::DisplayContext * display_context,
|
||||
Ogre::SceneNode * scene_node);
|
||||
ParticleCloudDisplay();
|
||||
~ParticleCloudDisplay() override;
|
||||
|
||||
void processMessage(nav2_msgs::msg::ParticleCloud::ConstSharedPtr msg) override;
|
||||
void setShape(QString shape); // for testing
|
||||
|
||||
protected:
|
||||
void onInitialize() override;
|
||||
void reset() override;
|
||||
|
||||
private Q_SLOTS:
|
||||
/// Update the interface and visible shapes based on the selected shape type.
|
||||
void updateShapeChoice();
|
||||
|
||||
/// Update the arrow color.
|
||||
void updateArrowColor();
|
||||
|
||||
/// Update arrow geometry
|
||||
void updateGeometry();
|
||||
|
||||
private:
|
||||
void initializeProperties();
|
||||
bool validateFloats(const nav2_msgs::msg::ParticleCloud & msg);
|
||||
bool setTransform(std_msgs::msg::Header const & header);
|
||||
void updateDisplay();
|
||||
void updateArrows2d();
|
||||
void updateArrows3d();
|
||||
void updateAxes();
|
||||
void updateArrow3dGeometry();
|
||||
void updateAxesGeometry();
|
||||
|
||||
std::unique_ptr<rviz_rendering::Axes> makeAxes();
|
||||
std::unique_ptr<rviz_rendering::Arrow> makeArrow3d();
|
||||
|
||||
std::vector<OgrePoseWithWeight> poses_;
|
||||
std::unique_ptr<FlatWeightedArrowsArray> arrows2d_;
|
||||
std::vector<std::unique_ptr<rviz_rendering::Arrow>> arrows3d_;
|
||||
std::vector<std::unique_ptr<rviz_rendering::Axes>> axes_;
|
||||
|
||||
Ogre::SceneNode * arrow_node_;
|
||||
Ogre::SceneNode * axes_node_;
|
||||
|
||||
rviz_common::properties::EnumProperty * shape_property_;
|
||||
rviz_common::properties::ColorProperty * arrow_color_property_;
|
||||
rviz_common::properties::FloatProperty * arrow_alpha_property_;
|
||||
|
||||
rviz_common::properties::FloatProperty * arrow_min_length_property_;
|
||||
rviz_common::properties::FloatProperty * arrow_max_length_property_;
|
||||
|
||||
float min_length_;
|
||||
float max_length_;
|
||||
float length_scale_;
|
||||
float head_radius_scale_;
|
||||
float head_length_scale_;
|
||||
float shaft_radius_scale_;
|
||||
};
|
||||
|
||||
} // namespace nav2_rviz_plugins
|
||||
|
||||
#endif // NAV2_RVIZ_PLUGINS__PARTICLE_CLOUD_DISPLAY__PARTICLE_CLOUD_DISPLAY_HPP_
|
||||
@@ -0,0 +1,65 @@
|
||||
// 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.
|
||||
|
||||
#ifndef NAV2_RVIZ_PLUGINS__ROS_ACTION_QEVENT_HPP_
|
||||
#define NAV2_RVIZ_PLUGINS__ROS_ACTION_QEVENT_HPP_
|
||||
|
||||
#include <QAbstractTransition>
|
||||
|
||||
namespace nav2_rviz_plugins
|
||||
{
|
||||
|
||||
enum class QActionState
|
||||
{
|
||||
ACTIVE,
|
||||
INACTIVE
|
||||
};
|
||||
|
||||
/// Custom Event to track state of ROS Action
|
||||
struct ROSActionQEvent : public QEvent
|
||||
{
|
||||
explicit ROSActionQEvent(QActionState state)
|
||||
: QEvent(QEvent::Type(QEvent::User + 1)),
|
||||
state_(state) {}
|
||||
|
||||
QActionState state_;
|
||||
};
|
||||
|
||||
/// Custom Transition to check whether ROS Action state has changed
|
||||
class ROSActionQTransition : public QAbstractTransition
|
||||
{
|
||||
public:
|
||||
explicit ROSActionQTransition(QActionState initial_status)
|
||||
: status_(initial_status)
|
||||
{}
|
||||
|
||||
~ROSActionQTransition() {}
|
||||
|
||||
protected:
|
||||
virtual bool eventTest(QEvent * e)
|
||||
{
|
||||
if (e->type() != QEvent::Type(QEvent::User + 1)) { // ROSActionEvent
|
||||
return false;
|
||||
}
|
||||
ROSActionQEvent * action_event = static_cast<ROSActionQEvent *>(e);
|
||||
return status_ != action_event->state_;
|
||||
}
|
||||
|
||||
virtual void onTransition(QEvent *) {}
|
||||
QActionState status_;
|
||||
};
|
||||
|
||||
} // namespace nav2_rviz_plugins
|
||||
|
||||
#endif // NAV2_RVIZ_PLUGINS__ROS_ACTION_QEVENT_HPP_
|
||||
@@ -0,0 +1,41 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>nav2_rviz_plugins</name>
|
||||
<version>1.1.18</version>
|
||||
<description>Navigation 2 plugins for rviz</description>
|
||||
<maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<build_depend>qtbase5-dev</build_depend>
|
||||
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>nav2_util</depend>
|
||||
<depend>nav2_lifecycle_manager</depend>
|
||||
<depend>nav2_msgs</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>pluginlib</depend>
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rclcpp_lifecycle</depend>
|
||||
<depend>resource_retriever</depend>
|
||||
<depend>rviz_common</depend>
|
||||
<depend>rviz_default_plugins</depend>
|
||||
<depend>rviz_ogre_vendor</depend>
|
||||
<depend>rviz_rendering</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>tf2_geometry_msgs</depend>
|
||||
<depend>visualization_msgs</depend>
|
||||
|
||||
<exec_depend>libqt5-core</exec_depend>
|
||||
<exec_depend>libqt5-gui</exec_depend>
|
||||
<exec_depend>libqt5-opengl</exec_depend>
|
||||
<exec_depend>libqt5-widgets</exec_depend>
|
||||
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,21 @@
|
||||
<library path="nav2_rviz_plugins">
|
||||
|
||||
<class name="nav2_rviz_plugins/GoalTool"
|
||||
type="nav2_rviz_plugins::GoalTool"
|
||||
base_class_type="rviz_common::Tool">
|
||||
<description>A tool used to specify the navigation goal pose.</description>
|
||||
</class>
|
||||
|
||||
<class name="nav2_rviz_plugins/Navigation 2"
|
||||
type="nav2_rviz_plugins::Nav2Panel"
|
||||
base_class_type="rviz_common::Panel">
|
||||
<description>The Nav2 rviz panel.</description>
|
||||
</class>
|
||||
|
||||
<class name="nav2_rviz_plugins/ParticleCloud"
|
||||
type="nav2_rviz_plugins::ParticleCloudDisplay"
|
||||
base_class_type="rviz_common::Display">
|
||||
<description>The Particle Cloud rviz display.</description>
|
||||
</class>
|
||||
|
||||
</library>
|
||||
@@ -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)
|
||||
@@ -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)
|
||||
+141
@@ -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
|
||||
@@ -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)
|
||||
Reference in New Issue
Block a user