NavMap Goal Pose

navmap_rviz_plugin/CMakeLists.txt

@@ -4,34 +4,49 @@ project(navmap_rviz_plugin)
 # 1) Qt y AUTOMOC
 find_package(Qt5 REQUIRED COMPONENTS Core Widgets)
 set(CMAKE_AUTOMOC ON)
-add_definitions(-DQT_NO_KEYWORDS)
+set(CMAKE_AUTORCC ON)
+set(CMAKE_AUTOUIC ON)
+
+# add_definitions(-DQT_NO_KEYWORDS)
 
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rviz_common REQUIRED)
 find_package(rviz_rendering REQUIRED)
+find_package(rviz_default_plugins REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rosidl_default_runtime REQUIRED)
 find_package(navmap_ros_interfaces REQUIRED)
+find_package(navmap_ros REQUIRED)
+find_package(navmap_core REQUIRED)
 
 set(CMAKE_CXX_STANDARD 23)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 
 qt5_wrap_cpp(NAVMAP_MOC_SRCS
   include/navmap_rviz_plugin/NavMapDisplay.hpp
+  include/navmap_rviz_plugin/navmap_goal_tool.hpp
+  include/navmap_rviz_plugin/navmap_pose_tool.hpp
 )
 
 add_library(${PROJECT_NAME} SHARED
-  src/NavMapDisplay.cpp
+  src/navmap_rviz_plugin/NavMapDisplay.cpp
+  src/navmap_rviz_plugin/navmap_goal_tool.cpp
+  src/navmap_rviz_plugin/navmap_pose_tool.cpp
   include/navmap_rviz_plugin/NavMapDisplay.hpp
+  include/navmap_rviz_plugin/navmap_goal_tool.hpp
+  include/navmap_rviz_plugin/navmap_pose_tool.hpp
   ${NAVMAP_MOC_SRCS}
 )
 target_link_libraries(navmap_rviz_plugin PUBLIC
   ${navmap_ros_interfaces_TARGETS}
+  navmap_ros::navmap_ros
+  navmap_core::navmap_core
   pluginlib::pluginlib
   rclcpp::rclcpp
   rviz_common::rviz_common
   rviz_rendering::rviz_rendering
+  rviz_default_plugins::rviz_default_plugins
   Qt5::Core
   Qt5::Widgets
 )
@@ -50,6 +65,11 @@ install(TARGETS
   RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
 
+install(
+  DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/icons"
+  DESTINATION "share/${PROJECT_NAME}"
+)
+
 install(FILES resource/navmap_rviz_plugin_description.xml
   DESTINATION share/${PROJECT_NAME}
 )

navmap_rviz_plugin/include/navmap_rviz_plugin/NavMapDisplay.hpp

@@ -40,6 +40,8 @@
 #include <navmap_ros_interfaces/msg/nav_map.hpp>
 #include <navmap_ros_interfaces/msg/nav_map_layer.hpp>
 
+#include "navmap_core/NavMap.hpp"
+
 #if defined _WIN32 || defined __CYGWIN__
   #ifdef __GNUC__
     #define NAVMAP_RVIZ_PLUGIN_EXPORT __attribute__ ((dllexport))
@@ -74,6 +76,8 @@ class HardwareVertexBuffer;
 namespace navmap_rviz_plugin
 {
 
+inline navmap::NavMap received_navmap;
+
 class NAVMAP_RVIZ_PLUGIN_PUBLIC NavMapDisplay
   : public rviz_common::MessageFilterDisplay<navmap_ros_interfaces::msg::NavMap>
 {

navmap_rviz_plugin/include/navmap_rviz_plugin/navmap_goal_tool.hpp

@@ -0,0 +1,70 @@
+// Copyright 2025 Intelligent Robotics Lab
+//
+// This file is part of the project Easy Navigation (EasyNav in short)
+// 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 NAVMAP_RVIZ_PLUGIN__NAVMAP_GOAL_TOOL_HPP_
+#define NAVMAP_RVIZ_PLUGIN__NAVMAP_GOAL_TOOL_HPP_
+
+#include <QObject>
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/qos.hpp"
+
+#include "navmap_rviz_plugin/navmap_pose_tool.hpp"
+#include "rviz_default_plugins/visibility_control.hpp"
+
+namespace rviz_common
+{
+class DisplayContext;
+namespace properties
+{
+class StringProperty;
+class QosProfileProperty;
+}  // namespace properties
+}  // namespace rviz_common
+
+namespace navmap_rviz_plugin
+{
+class RVIZ_DEFAULT_PLUGINS_PUBLIC NavMapGoalTool : public NavMapPoseTool
+{
+  Q_OBJECT
+
+public:
+  NavMapGoalTool();
+
+  ~NavMapGoalTool() override;
+
+  void onInitialize() override;
+
+protected:
+  void onPoseSet(double x, double y, double z, double theta) override;
+
+private Q_SLOTS:
+  void updateTopic();
+
+private:
+  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr publisher_;
+  rclcpp::Clock::SharedPtr clock_;
+
+  rviz_common::properties::StringProperty * topic_property_;
+  rviz_common::properties::QosProfileProperty * qos_profile_property_;
+
+  rclcpp::QoS qos_profile_;
+};
+
+}  // namespace navmap_rviz_plugin
+
+#endif  // NAVMAP_RVIZ_PLUGIN__NAVMAP_GOAL_TOOL_HPP_

navmap_rviz_plugin/include/navmap_rviz_plugin/navmap_pose_tool.hpp

@@ -0,0 +1,96 @@
+// Copyright 2025 Intelligent Robotics Lab
+//
+// This file is part of the project Easy Navigation (EasyNav in short)
+// 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 NAVMAP_RVIZ_PLUGIN__NAVMAP_POSE_TOOL_HPP_
+#define NAVMAP_RVIZ_PLUGIN__NAVMAP_POSE_TOOL_HPP_
+
+#include <memory>
+#include <string>
+#include <utility>
+
+#include <OgreVector.h>
+
+#include <QCursor>  // NOLINT cpplint cannot handle include order here
+
+#include "geometry_msgs/msg/point.hpp"
+#include "geometry_msgs/msg/quaternion.hpp"
+
+#include "rviz_common/tool.hpp"
+#include "rviz_rendering/viewport_projection_finder.hpp"
+#include "rviz_default_plugins/visibility_control.hpp"
+
+#include "navmap_rviz_plugin/NavMapDisplay.hpp"
+
+
+namespace rviz_rendering
+{
+class Arrow;
+}  // namespace rviz_rendering
+
+namespace navmap_rviz_plugin
+{
+
+class RVIZ_DEFAULT_PLUGINS_PUBLIC NavMapPoseTool : public rviz_common::Tool
+{
+public:
+  NavMapPoseTool();
+
+  ~NavMapPoseTool() override;
+
+  void onInitialize() override;
+
+  void activate() override;
+
+  void deactivate() override;
+
+  int processMouseEvent(rviz_common::ViewportMouseEvent & event) override;
+
+protected:
+  virtual void onPoseSet(double x, double y, double z, double theta) = 0;
+
+  geometry_msgs::msg::Quaternion orientationAroundZAxis(double angle);
+
+  void logPose(
+    std::string designation,
+    geometry_msgs::msg::Point position,
+    geometry_msgs::msg::Quaternion orientation,
+    double angle,
+    std::string frame);
+
+  std::shared_ptr<rviz_rendering::Arrow> arrow_;
+
+  enum State
+  {
+    Position,
+    Orientation
+  };
+  State state_;
+  double angle_;
+
+  Ogre::Vector3 arrow_position_;
+  std::shared_ptr<rviz_rendering::ViewportProjectionFinder> projection_finder_;
+
+private:
+  int processMouseLeftButtonPressed(std::pair<bool, Ogre::Vector3> xy_plane_intersection);
+  int processMouseMoved(std::pair<bool, Ogre::Vector3> xy_plane_intersection);
+  int processMouseLeftButtonReleased();
+  void makeArrowVisibleAndSetOrientation(double angle);
+  double calculateAngle(Ogre::Vector3 start_point, Ogre::Vector3 end_point);
+};
+
+}  // namespace navmap_rviz_plugin
+
+#endif  // NAVMAP_RVIZ_PLUGIN__NAVMAP_POSE_TOOL_HPP_

navmap_rviz_plugin/package.xml

@@ -17,6 +17,8 @@
   <depend>rviz_rendering</depend>
   <depend>rviz_default_plugins</depend>
   <depend>navmap_ros_interfaces</depend>
+  <depend>navmap_ros</depend>
+  <depend>navmap_core</depend>
   <depend>geometry_msgs</depend>
   <depend>std_msgs</depend>
   <depend>sensor_msgs</depend>

navmap_rviz_plugin/resource/navmap_rviz_plugin_description.xml

@@ -5,4 +5,15 @@
       base_class_type="rviz_common::Display">
     <description>Visualize NavMap triangles and layers, with optional normals and alpha.</description>
   </class>
+
+  <class
+    name="navmap_rviz_plugin/SetGoal"
+    type="navmap_rviz_plugin::NavMapGoalTool"
+    base_class_type="rviz_common::Tool"
+  >
+    <description>
+      Publish a goal pose for the robot.  After one use, reverts to default tool.
+    </description>
+  </class>
+
 </library>

navmap_rviz_plugin/src/NavMapDisplay.cpp → navmap_rviz_plugin/src/navmap_rviz_plugin/NavMapDisplay.cpp

@@ -39,6 +39,10 @@
 #include <rviz_common/uniform_string_stream.hpp>
 #include <rviz_common/validate_floats.hpp>
 
+#include "navmap_core/NavMap.hpp"
+#include "navmap_ros/conversions.hpp"
+
+
 #include <QCoreApplication>
 #include <cmath>
 #include <sstream>
@@ -223,6 +227,7 @@ void NavMapDisplay::processMessage(const NavMapMsg::ConstSharedPtr msg)
   root_node_->setOrientation(orientation);
 
   last_msg_ = std::make_shared<NavMapMsg>(*msg);
+  received_navmap = navmap_ros::from_msg(*last_msg_);
 
   ++navmap_msg_count_;
   last_navmap_stamp_ = rviz_ros_node_.lock()->get_raw_node()->now();

navmap_rviz_plugin/src/navmap_rviz_plugin/navmap_goal_tool.cpp

@@ -0,0 +1,88 @@
+// Copyright 2025 Intelligent Robotics Lab
+//
+// This file is part of the project Easy Navigation (EasyNav in short)
+// 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 "navmap_rviz_plugin/navmap_goal_tool.hpp"
+
+#include <string>
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+
+#include "rviz_common/display_context.hpp"
+#include "rviz_common/logging.hpp"
+#include "rviz_common/properties/string_property.hpp"
+#include "rviz_common/properties/qos_profile_property.hpp"
+
+namespace navmap_rviz_plugin
+{
+
+NavMapGoalTool::NavMapGoalTool()
+: navmap_rviz_plugin::NavMapPoseTool(), qos_profile_(5)
+{
+  shortcut_key_ = 'g';
+
+  topic_property_ = new rviz_common::properties::StringProperty(
+    "Topic", "goal_pose",
+    "The topic on which to publish goals.",
+    getPropertyContainer(), SLOT(updateTopic()), this);
+
+  qos_profile_property_ = new rviz_common::properties::QosProfileProperty(
+    topic_property_, qos_profile_);
+}
+
+NavMapGoalTool::~NavMapGoalTool() = default;
+
+void NavMapGoalTool::onInitialize()
+{
+  NavMapPoseTool::onInitialize();
+  qos_profile_property_->initialize(
+    [this](rclcpp::QoS profile) {this->qos_profile_ = profile;});
+  setName("NavMap Goal Pose");
+  updateTopic();
+}
+
+void NavMapGoalTool::updateTopic()
+{
+  rclcpp::Node::SharedPtr raw_node =
+    context_->getRosNodeAbstraction().lock()->get_raw_node();
+  publisher_ = raw_node->
+    template create_publisher<geometry_msgs::msg::PoseStamped>(
+    topic_property_->getStdString(), qos_profile_);
+  clock_ = raw_node->get_clock();
+}
+
+void NavMapGoalTool::onPoseSet(double x, double y, double z, double theta)
+{
+  std::string fixed_frame = context_->getFixedFrame().toStdString();
+
+  geometry_msgs::msg::PoseStamped goal;
+  goal.header.stamp = clock_->now();
+  goal.header.frame_id = fixed_frame;
+
+  goal.pose.position.x = x;
+  goal.pose.position.y = y;
+  goal.pose.position.z = z;
+
+  goal.pose.orientation = orientationAroundZAxis(theta);
+
+  logPose("goal", goal.pose.position, goal.pose.orientation, theta, fixed_frame);
+
+  publisher_->publish(goal);
+}
+
+}  // namespace navmap_rviz_plugin
+
+#include <pluginlib/class_list_macros.hpp>  // NOLINT
+PLUGINLIB_EXPORT_CLASS(navmap_rviz_plugin::NavMapGoalTool, rviz_common::Tool)