0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 3 years have passed since last update.

Pointを送信するTool pluginを作る

Last updated at Posted at 2021-03-12

rviz2プラグインの作り方 トップページへ

実行例

image.png
マウスのポインタの場所に青のオブジェクトが現れてクリックすると灰色になり座標のトピックが送信される

ROS1版

こちらのコードをrviz2用に書き直します
ROS講座102 Pointを送信するrviz tool pluginを作る

ROS1からの主な変更点

point_tool.hpp

マウスの座標からXY平面上の座標を取得するためにViewportProjectionFinderを使います

.hpp
    std::shared_ptr<rviz_rendering::Shape> vis_shape_;
    std::shared_ptr<rviz_rendering::ViewportProjectionFinder> projection_finder_;

NodeHandl はないのでNodeを用意します

.hpp
    // ros::NodeHandle nh_;
    // ros::Publisher point_pub_;
    rclcpp::Node::SharedPtr nh_;
    rclcpp::Publisher<geometry_msgs::msg::PointStamped>::SharedPtr point_pub_;

point_tool.cpp

ViewportProjectionFinderPublisherをそれぞれ実体化します

.cpp
    projection_finder_ = std::make_shared<rviz_rendering::ViewportProjectionFinder>();
    nh_ = context_->getRosNodeAbstraction().lock()->get_raw_node();
    // point_pub_ = nh_.advertise<geometry_msgs::PointStamped>("point", 10);
    point_pub_ = nh_->create_publisher<geometry_msgs::msg::PointStamped>("point", rclcpp::QoS(10));

マウス座標と平面座標の変換はgetViewportPointProjectionOnXYPlane() で行います。
std::pair<bool, Ogre::Vector3> で帰ってくるので変換できたかどうかをbool型で判断します。

.cpp
int PointTool::processMouseEvent(rviz_common::ViewportMouseEvent& event)
{
    //   Ogre::Vector3 intersection;
    //   Ogre::Plane ground_plane(Ogre::Vector3::UNIT_Z, 0.0f);

    //   if (rviz_common::getPointOnPlaneFromWindowXY(event.viewport, ground_plane, event.x, event.y, intersection))
    //   {
    //     vis_shape_->setPosition(intersection);
    //     if (event.leftDown())
    //     {
    //     //   geometry_msgs::PointStamped point_msg;
    //     //   point_msg.header.frame_id = context_->getFrameManager()->getFixedFrame();
    //     //   point_msg.header.stamp = ros::Time::now();
    //     //   point_msg.point.x = intersection.x;
    //     //   point_msg.point.y = intersection.y;
    //     //   point_msg.point.z = intersection.z;
    //     //   point_pub_.publish(point_msg);
    //       return Render | Finished;
    //     }
    //   }

    auto point_projection_on_xy_plane = projection_finder_->getViewportPointProjectionOnXYPlane(event.panel->getRenderWindow(), event.x, event.y);
    Ogre::Vector3 intersection = point_projection_on_xy_plane.second;
    if (point_projection_on_xy_plane.first)
    {
        vis_shape_->setPosition(intersection);

        if (event.leftDown())
        {
            auto point_msg = std::make_shared<geometry_msgs::msg::PointStamped>();
            point_msg->header.frame_id = context_->getFrameManager()->getFixedFrame();
            point_msg->header.stamp = nh_->now();
            point_msg->point.x = intersection.x;
            point_msg->point.y = intersection.y;
            point_msg->point.z = intersection.z;
            point_pub_->publish(*point_msg);
            return Render | Finished;
        }
    }

    return Render;
}
0
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?