Point 보내기 Tool plugin 만들기

8549 단어 ROS2ROS
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


ViewportProjectionFinderPublisher 를 각각 실체화
    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;
}

좋은 웹페이지 즐겨찾기