実行例
マウスのポインタの場所に青のオブジェクトが現れてクリックすると灰色になり座標のトピックが送信される
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
ViewportProjectionFinder
とPublisher
をそれぞれ実体化します
.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;
}