|
| 1 | +// Copyright 2025 Autoware Foundation |
| 2 | +// |
| 3 | +// Licensed under the Apache License, Version 2.0 (the "License"); |
| 4 | +// you may not use this file except in compliance with the License. |
| 5 | +// You may obtain a copy of the License at |
| 6 | +// |
| 7 | +// http://www.apache.org/licenses/LICENSE-2.0 |
| 8 | +// |
| 9 | +// Unless required by applicable law or agreed to in writing, software |
| 10 | +// distributed under the License is distributed on an "AS IS" BASIS, |
| 11 | +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 12 | +// See the License for the specific language governing permissions and |
| 13 | +// limitations under the License. |
| 14 | + |
| 15 | +#include "pointcloud_saver.hpp" |
| 16 | + |
| 17 | +#include "rviz_common/display_context.hpp" |
| 18 | +#include "rviz_common/interaction/view_picker_iface.hpp" |
| 19 | +#include "rviz_common/load_resource.hpp" |
| 20 | +#include "rviz_common/properties/color_property.hpp" |
| 21 | +#include "rviz_common/properties/parse_color.hpp" |
| 22 | +#include "rviz_common/render_panel.hpp" |
| 23 | +#include "rviz_common/tool.hpp" |
| 24 | +#include "rviz_common/view_manager.hpp" |
| 25 | +#include "rviz_common/viewport_mouse_event.hpp" |
| 26 | +#include "rviz_rendering/objects/line.hpp" |
| 27 | + |
| 28 | +#include <QString> // NOLINT: cpplint is unable to handle the include order here |
| 29 | +#include <autoware_lanelet2_extension/utility/message_conversion.hpp> |
| 30 | +#include <autoware_lanelet2_extension/utility/query.hpp> |
| 31 | +#include <autoware_lanelet2_extension/utility/utilities.hpp> |
| 32 | +#include <autoware_utils/geometry/alt_geometry.hpp> |
| 33 | +#include <autoware_utils_geometry/geometry.hpp> |
| 34 | +#include <autoware_utils_pcl/transforms.hpp> |
| 35 | +#include <rclcpp/rclcpp.hpp> |
| 36 | +#include <tf2_eigen/tf2_eigen.hpp> |
| 37 | + |
| 38 | +#include <geometry_msgs/msg/point.hpp> |
| 39 | + |
| 40 | +#include <OgreBillboardSet.h> |
| 41 | +#include <OgreManualObject.h> |
| 42 | +#include <OgreRay.h> |
| 43 | +#include <OgreSceneManager.h> |
| 44 | +#include <OgreSceneNode.h> |
| 45 | +#include <lanelet2_core/LaneletMap.h> |
| 46 | +#include <lanelet2_core/primitives/Lanelet.h> |
| 47 | +#include <pcl/io/pcd_io.h> |
| 48 | +#include <pcl/point_types.h> |
| 49 | +#include <pcl_conversions/pcl_conversions.h> |
| 50 | + |
| 51 | +#include <algorithm> |
| 52 | +#include <cmath> |
| 53 | +#include <iostream> |
| 54 | +#include <memory> |
| 55 | +#include <sstream> |
| 56 | +#include <string> |
| 57 | +#include <vector> |
| 58 | + |
| 59 | +namespace autoware |
| 60 | +{ |
| 61 | + |
| 62 | +namespace pointcloud_saver_rviz_plugin |
| 63 | +{ |
| 64 | + |
| 65 | +PointCloudSaver::PointCloudSaver() : tf2_listener_(tf2_buffer_) |
| 66 | +{ |
| 67 | + // shortcut_key_ = 'n'; |
| 68 | + |
| 69 | + color_property_ = new rviz_common::properties::ColorProperty( |
| 70 | + "Line color", Qt::darkYellow, "The topic on which to publish points.", getPropertyContainer(), |
| 71 | + SLOT(updateLineColor()), this); |
| 72 | +} |
| 73 | + |
| 74 | +void PointCloudSaver::onInitialize() |
| 75 | +{ |
| 76 | + line_viz = std::make_shared<rviz_rendering::BillboardLine>(context_->getSceneManager()); |
| 77 | + updateLineColor(); |
| 78 | + |
| 79 | + std_cursor_ = rviz_common::getDefaultCursor(); |
| 80 | + hit_cursor_ = rviz_common::makeIconCursor("package://rviz_common/icons/crosshair.svg"); |
| 81 | + |
| 82 | + projection_finder_ = std::make_shared<rviz_rendering::ViewportProjectionFinder>(); |
| 83 | + rclcpp::Node::SharedPtr raw_node = context_->getRosNodeAbstraction().lock()->get_raw_node(); |
| 84 | + |
| 85 | + sub_sensor_points_ = raw_node->create_subscription<sensor_msgs::msg::PointCloud2>( |
| 86 | + "/sensing/lidar/concatenated/pointcloud", rclcpp::SensorDataQoS().keep_last(1), |
| 87 | + std::bind(&PointCloudSaver::onPointCloud, this, std::placeholders::_1)); |
| 88 | +} |
| 89 | + |
| 90 | +void PointCloudSaver::onPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & msg_ptr) |
| 91 | +{ |
| 92 | + sensor_points_ = std::make_shared<pcl::PointCloud<PointType>>(); |
| 93 | + pcl::fromROSMsg(*msg_ptr, *sensor_points_); |
| 94 | +} |
| 95 | + |
| 96 | +void PointCloudSaver::activate() |
| 97 | +{ |
| 98 | +} |
| 99 | + |
| 100 | +void PointCloudSaver::deactivate() |
| 101 | +{ |
| 102 | +} |
| 103 | + |
| 104 | +int PointCloudSaver::processMouseEvent(rviz_common::ViewportMouseEvent & event) |
| 105 | +{ |
| 106 | + if (sensor_points_ == nullptr) { |
| 107 | + return 0; |
| 108 | + } |
| 109 | + |
| 110 | + auto point_projection_on_xy_plane = projection_finder_->getViewportPointProjectionOnXYPlane( |
| 111 | + event.panel->getRenderWindow(), event.x, event.y); |
| 112 | + Ogre::Vector3 pos = point_projection_on_xy_plane.second; |
| 113 | + |
| 114 | + static bool drawing = false; |
| 115 | + static std::vector<Ogre::Vector3> line_points; |
| 116 | + |
| 117 | + if (event.leftDown() && !drawing) { |
| 118 | + line_points.clear(); |
| 119 | + } |
| 120 | + |
| 121 | + if (event.leftDown()) { |
| 122 | + drawing = true; |
| 123 | + line_points.push_back(pos); |
| 124 | + |
| 125 | + line_viz->clear(); |
| 126 | + for (const auto & line_point : line_points) { |
| 127 | + line_viz->addPoint(line_point, color_); |
| 128 | + } |
| 129 | + } |
| 130 | + |
| 131 | + if (event.type == QEvent::MouseMove && drawing) { |
| 132 | + line_viz->clear(); |
| 133 | + for (const auto & line_point : line_points) { |
| 134 | + line_viz->addPoint(line_point, color_); |
| 135 | + } |
| 136 | + line_viz->addPoint(pos, color_); |
| 137 | + } |
| 138 | + |
| 139 | + if (event.rightDown() && drawing) { |
| 140 | + line_viz->clear(); |
| 141 | + for (const auto & line_point : line_points) { |
| 142 | + line_viz->addPoint(line_point, color_); |
| 143 | + } |
| 144 | + line_viz->addPoint(line_points.front(), color_); |
| 145 | + |
| 146 | + drawing = false; |
| 147 | + savePointCloud(line_points); |
| 148 | + return Render; |
| 149 | + } |
| 150 | + |
| 151 | + return 0; |
| 152 | +} |
| 153 | + |
| 154 | +struct Point |
| 155 | +{ |
| 156 | + double x; |
| 157 | + double y; |
| 158 | +}; |
| 159 | + |
| 160 | +bool pointOnSegment(const Point & a, const Point & b, const Point & p, double eps = 1e-12) |
| 161 | +{ |
| 162 | + double cross = (p.y - a.y) * (b.x - a.x) - (p.x - a.x) * (b.y - a.y); |
| 163 | + if (std::fabs(cross) > eps) return false; |
| 164 | + double dot = (p.x - a.x) * (p.x - b.x) + (p.y - a.y) * (p.y - b.y); |
| 165 | + return dot <= eps; |
| 166 | +} |
| 167 | + |
| 168 | +bool pointInPolygon( const std::vector<Point> & poly, const Point & p, bool includeBoundary = true) |
| 169 | +{ |
| 170 | + int n = static_cast<int>(poly.size()); |
| 171 | + if (n < 3) return false; |
| 172 | + if (includeBoundary) { |
| 173 | + for (int i = 0; i < n; ++i) { |
| 174 | + const Point & a = poly[i]; |
| 175 | + const Point & b = poly[(i + 1) % n]; |
| 176 | + if (pointOnSegment(a, b, p)) return true; |
| 177 | + } |
| 178 | + } |
| 179 | + |
| 180 | + bool inside = false; |
| 181 | + for (int i = 0, j = n - 1; i < n; j = i++) { |
| 182 | + const Point & vi = poly[i]; |
| 183 | + const Point & vj = poly[j]; |
| 184 | + bool intersect = ((vi.y > p.y) != (vj.y > p.y)); |
| 185 | + if (intersect) { |
| 186 | + double xIntersect = vj.x + (vi.x - vj.x) * ((p.y - vj.y) / (vi.y - vj.y)); |
| 187 | + if (p.x < xIntersect) inside = !inside; |
| 188 | + } |
| 189 | + } |
| 190 | + return inside; |
| 191 | +} |
| 192 | + |
| 193 | +void PointCloudSaver::savePointCloud(std::vector<Ogre::Vector3> line_points) |
| 194 | +{ |
| 195 | + std::vector<Point> poly; |
| 196 | + for (const auto & line_point : line_points) { |
| 197 | + poly.push_back(Point{static_cast<double>(line_point.x), static_cast<double>(line_point.y)}); |
| 198 | + } |
| 199 | + |
| 200 | + pcl::shared_ptr<pcl::PointCloud<PointType>> sensor_points_in_fixed_frame( |
| 201 | + new pcl::PointCloud<PointType>); |
| 202 | + try { |
| 203 | + transform_sensor_measurement( |
| 204 | + sensor_points_->header.frame_id, context_->getFixedFrame().toStdString(), sensor_points_, |
| 205 | + sensor_points_in_fixed_frame); |
| 206 | + } catch (const std::exception & ex) { |
| 207 | + std::stringstream message; |
| 208 | + message << ex.what() << ". Please publish TF " << sensor_points_->header.frame_id << " to " |
| 209 | + << context_->getFixedFrame().toStdString(); |
| 210 | + // RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); |
| 211 | + } |
| 212 | + |
| 213 | + pcl::PointCloud<PointType> save_points; |
| 214 | + for (const auto & sensor_point : sensor_points_in_fixed_frame->points) { |
| 215 | + bool inside = pointInPolygon(poly, Point{sensor_point.x, sensor_point.y}); |
| 216 | + if (inside) { |
| 217 | + save_points.push_back(sensor_point); |
| 218 | + } |
| 219 | + } |
| 220 | + |
| 221 | + std::cout << "save_points.points.size() " << save_points.points.size() << std::endl; |
| 222 | + |
| 223 | + if (!save_points.points.empty()) { |
| 224 | + pcl::io::savePCDFileBinary("saved_pointcloud.pcd", save_points); |
| 225 | + } |
| 226 | +} |
| 227 | + |
| 228 | +Eigen::Affine3d pose_to_affine3d(const geometry_msgs::msg::Pose & ros_pose) |
| 229 | +{ |
| 230 | + Eigen::Affine3d eigen_pose; |
| 231 | + tf2::fromMsg(ros_pose, eigen_pose); |
| 232 | + return eigen_pose; |
| 233 | +} |
| 234 | + |
| 235 | +Eigen::Matrix4f pose_to_matrix4f(const geometry_msgs::msg::Pose & ros_pose) |
| 236 | +{ |
| 237 | + Eigen::Affine3d eigen_pose_affine = pose_to_affine3d(ros_pose); |
| 238 | + Eigen::Matrix4f eigen_pose_matrix = eigen_pose_affine.matrix().cast<float>(); |
| 239 | + return eigen_pose_matrix; |
| 240 | +} |
| 241 | + |
| 242 | +void PointCloudSaver::transform_sensor_measurement( |
| 243 | + const std::string & source_frame, const std::string & target_frame, |
| 244 | + const std::shared_ptr<pcl::PointCloud<PointType>> & sensor_points_input_ptr, |
| 245 | + std::shared_ptr<pcl::PointCloud<PointType>> & sensor_points_output_ptr) |
| 246 | +{ |
| 247 | + if (source_frame == target_frame) { |
| 248 | + sensor_points_output_ptr = sensor_points_input_ptr; |
| 249 | + return; |
| 250 | + } |
| 251 | + |
| 252 | + geometry_msgs::msg::TransformStamped transform; |
| 253 | + try { |
| 254 | + transform = tf2_buffer_.lookupTransform(target_frame, source_frame, tf2::TimePointZero); |
| 255 | + } catch (const tf2::TransformException & ex) { |
| 256 | + throw; |
| 257 | + } |
| 258 | + |
| 259 | + const geometry_msgs::msg::PoseStamped target_to_source_pose_stamped = |
| 260 | + autoware_utils_geometry::transform2pose(transform); |
| 261 | + const Eigen::Matrix4f base_to_sensor_matrix = |
| 262 | + pose_to_matrix4f(target_to_source_pose_stamped.pose); |
| 263 | + autoware_utils_pcl::transform_pointcloud( |
| 264 | + *sensor_points_input_ptr, *sensor_points_output_ptr, base_to_sensor_matrix); |
| 265 | +} |
| 266 | + |
| 267 | +void PointCloudSaver::updateLineColor() |
| 268 | +{ |
| 269 | + color_ = rviz_common::properties::qtToOgre(color_property_->getColor()); |
| 270 | + // line_->setColor(color); |
| 271 | + color_ = Ogre::ColourValue(0, 1, 0, 1); |
| 272 | + |
| 273 | + line_viz->setLineWidth(0.3); |
| 274 | +} |
| 275 | + |
| 276 | +} // namespace pointcloud_saver_rviz_plugin |
| 277 | +} // namespace autoware |
| 278 | + |
| 279 | +#include <pluginlib/class_list_macros.hpp> // NOLINT |
| 280 | +PLUGINLIB_EXPORT_CLASS(autoware::pointcloud_saver_rviz_plugin::PointCloudSaver, rviz_common::Tool) |
![pre-commit-ci[bot]](https://avatars.githubusercontent.com/in/68672?v=4&size=40){kind=avatar}
0 commit comments