Pointを送信するTool pluginを作る

9122 ワード
ROS2 ROS ROS テキストリンク

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

実行例


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

ROS1版

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

ROS1からの主な変更点

point_tool.hpp

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

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

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

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

point_tool.cpp

ViewportProjectionFinder とPublisherをそれぞれ実体化します

    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型で判断します。

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;
}