feat(navigation2): add RViz charger tool

navigation2/nav2_rviz_plugins/CMakeLists.txt

@@ -36,6 +36,7 @@ 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/charger_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
@@ -49,6 +50,7 @@ set(library_name ${PROJECT_NAME})
 
 add_library(${library_name} SHARED
   src/goal_tool.cpp
+  src/charger_tool.cpp
   src/nav2_panel.cpp
   src/particle_cloud_display/flat_weighted_arrows_array.cpp
   src/particle_cloud_display/particle_cloud_display.cpp

navigation2/nav2_rviz_plugins/include/nav2_rviz_plugins/charger_tool.hpp

@@ -0,0 +1,69 @@
+// 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__CHARGER_TOOL_HPP_
+#define NAV2_RVIZ_PLUGINS__CHARGER_TOOL_HPP_
+
+#include <QObject>
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/qos.hpp"
+#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;
+class QosProfileProperty;
+}  // namespace properties
+}  // namespace rviz_common
+
+namespace nav2_rviz_plugins
+{
+
+class RVIZ_DEFAULT_PLUGINS_PUBLIC ChargerTool : public rviz_default_plugins::tools::PoseTool
+{
+  Q_OBJECT
+
+public:
+  ChargerTool();
+  ~ChargerTool() override;
+
+  void onInitialize() override;
+
+protected:
+  void onPoseSet(double x, double y, 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 nav2_rviz_plugins
+
+#endif  // NAV2_RVIZ_PLUGINS__CHARGER_TOOL_HPP_

navigation2/nav2_rviz_plugins/plugins_description.xml

@@ -6,6 +6,12 @@
     <description>A tool used to specify the navigation goal pose.</description>
   </class>
 
+  <class name="nav2_rviz_plugins/ChargerTool"
+         type="nav2_rviz_plugins::GoalTool"
+         base_class_type="rviz_common::Tool">
+    <description>A tool used to specify the charging stations goal pose.</description>
+  </class>
+
   <class name="nav2_rviz_plugins/Navigation 2"
          type="nav2_rviz_plugins::Nav2Panel"
          base_class_type="rviz_common::Panel">

navigation2/nav2_rviz_plugins/src/charger_tool.cpp

@@ -0,0 +1,87 @@
+// 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/charger_tool.hpp"
+
+#include <memory>
+#include <string>
+
+#include "rviz_common/display_context.hpp"
+#include "rviz_common/load_resource.hpp"
+#include "rviz_common/properties/string_property.hpp"
+#include "rviz_common/properties/qos_profile_property.hpp"
+
+namespace nav2_rviz_plugins
+{
+
+ChargerTool::ChargerTool()
+: rviz_default_plugins::tools::PoseTool(), qos_profile_(5)
+{
+  shortcut_key_ = 'c';
+  topic_property_ = new rviz_common::properties::StringProperty(
+    "Topic", "charger_position_update",
+    "The topic on which to publish goals.",
+    getPropertyContainer(), SLOT(updateTopic()), this);
+
+  qos_profile_property_ = new rviz_common::properties::QosProfileProperty(
+    topic_property_, qos_profile_);
+}
+
+ChargerTool::~ChargerTool()
+{
+}
+
+void ChargerTool::onInitialize()
+{
+  PoseTool::onInitialize();
+  setName("Charger Update");
+  setIcon(rviz_common::loadPixmap("package://rviz_default_plugins/icons/classes/SetGoal.png"));
+  updateTopic();
+}
+
+void ChargerTool::updateTopic()
+{
+  rclcpp::Node::SharedPtr raw_node =
+    context_->getRosNodeAbstraction().lock()->get_raw_node();
+  // TODO(anhosi, wjwwood): replace with abstraction for publishers once available
+  publisher_ = raw_node->
+    template create_publisher<geometry_msgs::msg::PoseStamped>(
+    topic_property_->getStdString(), qos_profile_);
+  clock_ = raw_node->get_clock();
+}
+
+void
+ChargerTool::onPoseSet(double x, double y, 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 = 0.0;
+
+  goal.pose.orientation = orientationAroundZAxis(theta);
+
+  logPose("goal", goal.pose.position, goal.pose.orientation, theta, fixed_frame);
+
+  publisher_->publish(goal);
+}
+
+}  // namespace nav2_rviz_plugins
+
+#include <pluginlib/class_list_macros.hpp>  // NOLINT
+PLUGINLIB_EXPORT_CLASS(nav2_rviz_plugins::ChargerTool, rviz_common::Tool)