Migrate pose estimate tool

README.md

@@ -3,13 +3,13 @@
 This branch is currently contained in the main `ros2.repos` file of ROS 2 and can be used for ROS 2.
 The latest release will be available with your ROS 2 download.
 
-ROS 2 does not have a wiki yet. To learn about RViz and its functionality, please refer to the ROS RViz [wiki page](http://www.ros.org/wiki/rviz). 
+ROS 2 does not have a wiki yet. To learn about RViz and its functionality, please refer to the ROS RViz [wiki page](http://www.ros.org/wiki/rviz).
 
 ## Features
 
 ### Already ported
 These features have already been ported from `ros-visualization/rviz` to `ros2/rviz`.
-The basic documentation can still be found on the RViz [wiki page](http://www.ros.org/wiki/rviz). 
+The basic documentation can still be found on the RViz [wiki page](http://www.ros.org/wiki/rviz).
 For some displays, the [documentation is updated](docs/FEATURES.md).
 
 | Displays              | Tools         | View Controller       | Panels          |
@@ -20,7 +20,7 @@ For some displays, the [documentation is updated](docs/FEATURES.md).
 | Image                 | Select        | Third Person Follower | Tool Properties |
 | Laser Scan            | 2D Nav Goal   | Top Down Orthographic | Views           |
 | Map                   | Publish Point |                       |                 |
-| Marker                |
+| Marker                | Initial Pose  |
 | Marker Array          |
 | Odometry              |
 | Point Cloud (1 and 2) |
@@ -36,8 +36,8 @@ These features have not been ported to `ros2/rviz` yet.
 
 | Displays             | Tools        | Panels |
 | -------------------- | ------------ | ------ |
-| Axes                 | Initial Pose | Time   |
-| DepthCloud           | Interact     |        |
+| Axes                 | Interact     | Time   |
+| DepthCloud           |
 | Effort               |
 | Fluid Pressure       |
 | Illuminance          |

rviz_common/default.rviz

@@ -61,8 +61,8 @@ Visualization Manager:
     - Class: rviz_default_plugins/Select
     - Class: rviz_default_plugins/FocusCamera
     - Class: rviz_default_plugins/Measure
-    # - Class: rviz/SetInitialPose
-    #   Topic: /initialpose
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
     - Class: rviz_default_plugins/SetGoal
       Topic: /move_base_simple/goal
     - Class: rviz_default_plugins/PublishPoint

rviz_common/help/help.html

@@ -38,10 +38,10 @@ <h3>Switching between tools</h3>
     <td class="key">G</td>
     <td>Switch to the 2D Nav Goal tool.</td>
   </tr>
-  <!--<tr class="odd">-->
-    <!--<td class="key">P</td>-->
-    <!--<td>Switch to the 2D Pose Estimate tool.</td>-->
-  <!--</tr>-->
+  <tr class="even">
+    <td class="key">P</td>
+    <td>Switch to the 2D Pose Estimate tool.</td>
+  </tr>
 </table>
 </p>
 

rviz_default_plugins/CMakeLists.txt

@@ -106,6 +106,7 @@ set(rviz_default_plugins_headers_to_moc
   include/rviz_default_plugins/tools/measure/measure_tool.hpp
   include/rviz_default_plugins/tools/nav_goal/goal_tool.hpp
   include/rviz_default_plugins/tools/pose/pose_tool.hpp
+  include/rviz_default_plugins/tools/pose_estimate/initial_pose_tool.hpp
   include/rviz_default_plugins/tools/point/point_tool.hpp
   include/rviz_default_plugins/view_controllers/orbit/orbit_view_controller.hpp
   include/rviz_default_plugins/view_controllers/ortho/fixed_orientation_ortho_view_controller.hpp
@@ -171,6 +172,7 @@ set(rviz_default_plugins_source_files
   src/rviz_default_plugins/tools/move/move_tool.cpp
   src/rviz_default_plugins/tools/nav_goal/goal_tool.cpp
   src/rviz_default_plugins/tools/pose/pose_tool.cpp
+  src/rviz_default_plugins/tools/pose_estimate/initial_pose_tool.cpp
   src/rviz_default_plugins/tools/point/point_tool.cpp
   src/rviz_default_plugins/tools/select/selection_tool.cpp
   src/rviz_default_plugins/view_controllers/follower/third_person_follower_view_controller.cpp

rviz_default_plugins/src/rviz_default_plugins/tools/focus/focus_tool.hpp → rviz_default_plugins/include/rviz_default_plugins/tools/focus/focus_tool.hpp

@@ -35,14 +35,16 @@
 #include "rviz_common/tool.hpp"
 #include "rviz_common/viewport_mouse_event.hpp"
 
+#include "rviz_default_plugins/visibility_control.hpp"
+
 namespace rviz_default_plugins
 {
 namespace tools
 {
 
 //! The Focus Tool allows the user to set the focal point of the current
 //! view controller with a single mouse click.
-class FocusTool : public rviz_common::Tool
+class RVIZ_DEFAULT_PLUGINS_PUBLIC FocusTool : public rviz_common::Tool
 {
 public:
   FocusTool();

rviz_default_plugins/include/rviz_default_plugins/tools/nav_goal/goal_tool.hpp

@@ -36,7 +36,7 @@
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "rclcpp/node.hpp"
 
-#include "../pose/pose_tool.hpp"
+#include "rviz_default_plugins/tools/pose/pose_tool.hpp"
 #include "rviz_default_plugins/visibility_control.hpp"
 
 namespace rviz_common
@@ -47,11 +47,11 @@ namespace properties
 class StringProperty;
 }  // namespace properties
 }  // namespace rviz_common
+
 namespace rviz_default_plugins
 {
 namespace tools
 {
-
 class RVIZ_DEFAULT_PLUGINS_PUBLIC GoalTool : public PoseTool
 {
   Q_OBJECT

rviz_default_plugins/include/rviz_default_plugins/tools/pose/pose_tool.hpp

@@ -31,6 +31,7 @@
 #define RVIZ_DEFAULT_PLUGINS__TOOLS__POSE__POSE_TOOL_HPP_
 
 #include <memory>
+#include <string>
 #include <utility>
 
 #ifdef __clang__
@@ -46,6 +47,9 @@
 
 #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"
@@ -77,6 +81,14 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC PoseTool : public rviz_common::Tool
 protected:
   virtual void onPoseSet(double x, double y, double theta) = 0;
 
+  geometry_msgs::msg::Quaternion orientationAroundZAxis(double angle);
+
+  void logPose(
+    geometry_msgs::msg::Point position,
+    geometry_msgs::msg::Quaternion orientation,
+    double angle,
+    std::string frame);
+
   std::shared_ptr<rviz_rendering::Arrow> arrow_;
 
   enum State

rviz/src/rviz/default_plugin/tools/initial_pose_tool.h → rviz_default_plugins/include/rviz_default_plugins/tools/pose_estimate/initial_pose_tool.hpp

@@ -1,5 +1,6 @@
 /*
  * 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
@@ -27,46 +28,54 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#ifndef RVIZ_INITIAL_POSE_TOOL_H
-#define RVIZ_INITIAL_POSE_TOOL_H
+#ifndef RVIZ_DEFAULT_PLUGINS__TOOLS__POSE_ESTIMATE__INITIAL_POSE_TOOL_HPP_
+#define RVIZ_DEFAULT_PLUGINS__TOOLS__POSE_ESTIMATE__INITIAL_POSE_TOOL_HPP_
 
-#ifndef Q_MOC_RUN  // See: https://bugreports.qt-project.org/browse/QTBUG-22829
-# include <QObject>
+#include <QObject>
 
-# include <ros/ros.h>
+#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+#include "rclcpp/node.hpp"
 
-# include "rviz/default_plugin/tools/pose_tool.h"
-#endif
+#include "rviz_default_plugins/tools/pose/pose_tool.hpp"
+#include "rviz_default_plugins/visibility_control.hpp"
 
-namespace rviz
+namespace rviz_common
 {
-class Arrow;
 class DisplayContext;
+namespace properties
+{
 class StringProperty;
+}  // namespace properties
+}  // namespace rviz_common
 
-class InitialPoseTool: public PoseTool
+namespace rviz_default_plugins
+{
+namespace tools
 {
-Q_OBJECT
+class RVIZ_DEFAULT_PLUGINS_PUBLIC InitialPoseTool : public PoseTool
+{
+  Q_OBJECT
+
 public:
   InitialPoseTool();
-  virtual ~InitialPoseTool() {}
-  virtual void onInitialize();
+
+  ~InitialPoseTool() override;
+
+  void onInitialize() override;
 
 protected:
-  virtual void onPoseSet(double x, double y, double theta);
+  void onPoseSet(double x, double y, double theta) override;
 
 private Q_SLOTS:
   void updateTopic();
 
 private:
-  ros::NodeHandle nh_;
-  ros::Publisher pub_;
+  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr publisher_;
 
-  StringProperty* topic_property_;
+  rviz_common::properties::StringProperty * topic_property_;
 };
 
-}
-
-#endif
-
+}  // namespace tools
+}  // namespace rviz_default_plugins
 
+#endif  // RVIZ_DEFAULT_PLUGINS__TOOLS__POSE_ESTIMATE__INITIAL_POSE_TOOL_HPP_

rviz_default_plugins/plugins_description.xml

@@ -261,6 +261,16 @@
     </description>
   </class>
 
+  <class
+    name="rviz_default_plugins/SetInitialPose"
+    type="rviz_default_plugins::tools::InitialPoseTool"
+    base_class_type="rviz_common::Tool"
+  >
+    <description>
+      Publish an initial pose for the robot.  After one use, reverts to default tool.
+    </description>
+  </class>
+
   <!-- View Controller plugins -->
   <class
     name="rviz_default_plugins/FPS"

rviz_default_plugins/src/rviz_default_plugins/tools/focus/focus_tool.cpp

@@ -57,7 +57,7 @@
 #include "rviz_common/view_controller.hpp"
 #include "rviz_rendering/render_window.hpp"
 
-#include "./focus_tool.hpp"
+#include "rviz_default_plugins/tools/focus/focus_tool.hpp"
 
 
 namespace rviz_default_plugins

rviz_default_plugins/src/rviz_default_plugins/tools/nav_goal/goal_tool.cpp

@@ -81,16 +81,9 @@ void GoalTool::onPoseSet(double x, double y, double theta)
   goal.pose.position.y = y;
   goal.pose.position.z = 0.0;
 
-  // set quaternion from yaw only
-  goal.pose.orientation.x = 0.0;
-  goal.pose.orientation.y = 0.0;
-  goal.pose.orientation.z = sin(theta) / (2 * cos(theta / 2));
-  goal.pose.orientation.w = cos(theta / 2);
-
-  RVIZ_COMMON_LOG_INFO_STREAM("Setting goal: Frame:" << fixed_frame << ", Position(" <<
-    goal.pose.position.x << ", " << goal.pose.position.y << ", " << goal.pose.position.z <<
-    "), Orientation(" << goal.pose.orientation.x << ", " << goal.pose.orientation.y << ", " <<
-    goal.pose.orientation.z << ", " << goal.pose.orientation.w << ") = Angle: " << theta);
+  goal.pose.orientation = orientationAroundZAxis(theta);
+
+  logPose(goal.pose.position, goal.pose.orientation, theta, fixed_frame);
 
   publisher_->publish(goal);
 }

rviz_default_plugins/src/rviz_default_plugins/tools/pose/pose_tool.cpp

@@ -30,6 +30,7 @@
 #include "rviz_default_plugins/tools/pose/pose_tool.hpp"
 
 #include <memory>
+#include <string>
 #include <utility>
 
 #ifndef _WIN32
@@ -52,11 +53,12 @@
 # pragma warning(pop)
 #endif
 
-#include "rviz_rendering/render_window.hpp"
 #include "rviz_rendering/geometry.hpp"
 #include "rviz_rendering/objects/arrow.hpp"
-#include "rviz_common/viewport_mouse_event.hpp"
+#include "rviz_rendering/render_window.hpp"
+#include "rviz_common/logging.hpp"
 #include "rviz_common/render_panel.hpp"
+#include "rviz_common/viewport_mouse_event.hpp"
 
 namespace rviz_default_plugins
 {
@@ -163,5 +165,28 @@ double PoseTool::calculateAngle(Ogre::Vector3 start_point, Ogre::Vector3 end_poi
   return atan2(start_point.y - end_point.y, start_point.x - end_point.x);
 }
 
+geometry_msgs::msg::Quaternion PoseTool::orientationAroundZAxis(double angle)
+{
+  auto orientation = geometry_msgs::msg::Quaternion();
+  orientation.x = 0.0;
+  orientation.y = 0.0;
+  orientation.z = sin(angle) / (2 * cos(angle / 2));
+  orientation.w = cos(angle / 2);
+  return orientation;
+}
+
+void
+PoseTool::logPose(
+  geometry_msgs::msg::Point position,
+  geometry_msgs::msg::Quaternion orientation,
+  double angle,
+  std::string frame)
+{
+  RVIZ_COMMON_LOG_INFO_STREAM("Setting goal: Frame:" << frame << ", Position(" <<
+    position.x << ", " << position.y << ", " << position.z << "), Orientation(" <<
+    orientation.x << ", " << orientation.y << ", " << orientation.z << ", " << orientation.w <<
+    ") = Angle: " << angle);
+}
+
 }  // namespace tools
 }  // namespace rviz_default_plugins