Rvizでinitial poseを指定するプラグインを自作する

# 概要
Rviz上部に表示されるrviz tool pluginのうち、initialpose (geometry_msgs::PoseWithCovarianceStamped)をpublishするプラグインを自作する．
↓Sample initial pose
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/696594/1446e2de-945d-06ab-93af-9d8d5e174a2e.png)

# 環境
| 項目 | 値 |
| -------- | -------- |
| Ubuntu     | 18.04     |
| ROS     | Melodic     |

# コード
https://github.com/Decwest/sample_rviz_arrow_plugin

試す場合は

```shell
cd ~/catkin_ws/src
git clone https://github.com/Decwest/sample_rviz_arrow_plugin
cd ../
catkin_make
```
![rviz_arrow_plugin.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/696594/a1908eb3-4102-e15c-2f6a-ec06bad97901.gif)

# コードについて
```sample_initialpose_tool.cpp
 /*
  * Copyright (c) 2012, Willow Garage, Inc.
  * 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.
  */
 
 #include <tf/transform_listener.h>
 
 #include <geometry_msgs/PoseWithCovarianceStamped.h>
 
 #include "rviz/display_context.h"
 #include "rviz/properties/string_property.h"
 #include "rviz/properties/float_property.h"
 
 #include "sample_initialpose_tool.h"
 
 namespace rviz
 {
 SampleInitialPoseTool::SampleInitialPoseTool()
 {
   shortcut_key_ = 's';
   std_dev_x_ = new FloatProperty("X std deviation", 0.5, "X standard deviation for initial pose [m]", getPropertyContainer());
   std_dev_y_ = new FloatProperty("Y std deviation", 0.5, "Y standard deviation for initial pose [m]", getPropertyContainer());
   std_dev_theta_ = new FloatProperty("Theta std deviation", M_PI / 12.0, "Theta standard deviation for initial pose [rad]", getPropertyContainer());

   std_dev_x_->setMin(0);
   std_dev_y_->setMin(0);
   std_dev_theta_->setMin(0);

 }
 
 void SampleInitialPoseTool::onInitialize()
 {
   PoseTool::onInitialize();
   setName("Sample initial pose");
   updateTopic();
 }
 
 void SampleInitialPoseTool::updateTopic()
 {
   try
   {
     pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("sample_initialpose", 1);
   }
   catch (const ros::Exception& e)
   {
     ROS_ERROR_STREAM_NAMED("SampleInitialPoseTool", e.what());
   }
 }
 
 void SampleInitialPoseTool::onPoseSet(double x, double y, double theta)
 {
   std::string fixed_frame = context_->getFixedFrame().toStdString();
   geometry_msgs::PoseWithCovarianceStamped pose;
   pose.header.frame_id = fixed_frame;
   pose.header.stamp = ros::Time::now();
   pose.pose.pose.position.x = x;
   pose.pose.pose.position.y = y;
 
   tf::Quaternion quat;
   quat.setRPY(0.0, 0.0, theta);
   tf::quaternionTFToMsg(quat, pose.pose.pose.orientation);
   pose.pose.covariance[6 * 0 + 0] = std::pow(std_dev_x_->getFloat(), 2);
   pose.pose.covariance[6 * 1 + 1] = std::pow(std_dev_y_->getFloat(), 2);
   pose.pose.covariance[6 * 5 + 5] = std::pow(std_dev_theta_->getFloat(), 2);
   ROS_INFO("Setting sample initialpose: %.3f %.3f %.3f [frame=%s]", x, y, theta, fixed_frame.c_str());
   pub_.publish(pose);
 }
 
 } // end namespace rviz
 
 #include <pluginlib/class_list_macros.hpp>
 PLUGINLIB_EXPORT_CLASS(rviz::SampleInitialPoseTool, rviz::Tool)
```

- コンストラクタの`shortcut_key_=''`でショートカットキーを登録可能
- `void SampleInitialPoseTool::onInitialize()`の`setName()`でRvizに表示されるプラグインの名前を指定
- `void SampleInitialPoseTool::updateTopic()`の`pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("sample_initialpose", 1);`
でpublisherを定義
    - ここ次第でpublishするトピックの型やトピック名を変更可能
- `void SampleInitialPoseTool::onPoseSet(double x, double y, double theta)`
でトピックに中身を代入


```CMakeLists.txt
cmake_minimum_required(VERSION 3.0.2)
project(sample_rviz_arrow_plugin)

## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++11)

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  tf
  pluginlib
  rviz
)

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES plugin_lecture
#  CATKIN_DEPENDS roscpp rospy std_msgs tf
#  DEPENDS system_lib
)

#Qt5 configulation
set(CMAKE_AUTOMOC ON)
find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED
  Core
  Widgets
)
set(QT_LIBRARIES Qt5::Widgets)
add_definitions(-DQT_NO_KEYWORDS)

include_directories(
#  include
  ${catkin_INCLUDE_DIRS}
)

## Declare a C++ library
add_library(${PROJECT_NAME}
  src/rviz/sample_initialpose_tool.cpp
) 

target_link_libraries(${PROJECT_NAME}
  ${catkin_LIBRARIES}
  ${QT_LIBRARIES}
)
```
- Qtに関する記述などを追記している

```package.xml
<?xml version="1.0"?>
<package format="2">
  <name>sample_rviz_arrow_plugin</name>
  <version>0.0.0</version>
  <description>The sample_rviz_arrow_plugin package</description>

  <!-- One maintainer tag required, multiple allowed, one person per tag -->
  <!-- Example:  -->
  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
  <maintainer email="fumiyaonishi1016@gmail.com">decwest</maintainer>

- 
  <!-- One license tag required, multiple allowed, one license per tag -->
  <!-- Commonly used license strings: -->
  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
  <license>TODO</license>

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>rospy</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>rospy</exec_depend>
  <exec_depend>std_msgs</exec_depend>

  <depend>pluginlib</depend>
  <depend>rviz</depend>

  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <rviz plugin="${prefix}/plugin_description.xml"/>
  </export>

</package>
```
- `<depend>rviz</depend>`のようにrvizへの依存関係を示さないと，rviz上に作成したプラグインが表示されないので注意

```plugin_description.xml
<library path="lib/libsample_rviz_arrow_plugin">
  <class name="rviz/SampleInitialPoseTool" type="rviz::SampleInitialPoseTool" base_class_type="rviz::Tool">
    <description>put sample initial pose</description>
  </class>
</library>
```
- rvizにプラグインの場所を教えている

# 参考
[rviz::InitialPoseTool Class Reference](http://docs.ros.org/en/melodic/api/rviz/html/c++/classrviz_1_1InitialPoseTool.html)
[ROS講座102 Pointを送信するrviz tool pluginを作る](https://qiita.com/srs/items/adddd3dbd0da2d52d68b)
[ROS講座59 ROSにおけるpluginの書き方](https://qiita.com/srs/items/c258bd38588dac1223d0)