DO NOT MERGE:Feat/sensing/pointcloud preprocessor#125
Conversation
There was a problem hiding this comment.
PointXYZ(もしくはPointXYZI)で良いので不要
| #include <wheel_stuck_common_utils/geometry/conversion.hpp> | ||
| #include <wheel_stuck_common_utils/pointcloud/transform_pointcloud.hpp> | ||
| #include <wheel_stuck_common_utils/ros/function_timer.hpp> | ||
| #include <wheel_stuck_common_utils/ros/no_callback_subscription.hpp> |
|
|
||
| #ifndef LASERS_NUM | ||
| #define LASERS_NUM 32 | ||
| #endif |
|
|
||
| class PointCloudPreprocessor : public rclcpp::Node | ||
| { | ||
| private: |
| explicit PointCloudPreprocessor(const rclcpp::NodeOptions & options); | ||
|
|
||
| private: | ||
| void processCloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg); |
src/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/pointcloud_preprocessor.hpp
Show resolved
Hide resolved
Autumn60
left a comment
Autumn60
left a comment
There was a problem hiding this comment.
メンバ変数である必要がないものはローカル変数に。
+@
どこかで"base_link"はパラメータ化したい
…l-Stuck/wheel_stuck_ws into feat/sensing/pointcloud_preprocessor
…l-Stuck/wheel_stuck_ws into feat/sensing/pointcloud_preprocessor
|
指摘箇所修正しました。 |
|
・クロッピング機能を追加しました。 colcon build --packages-select wheel_stuck_robot_utils です。よろしくおねがいします。 |
| wheel_stuck_robot_utils::RobotInfoUtils robot_info_utils(node); | ||
| robot_info_ = robot_info_utils.get_info(); | ||
| // パラメーターが読み込まれた後のログ出力 | ||
| RCLCPP_INFO(node.get_logger(), "Front Overhang: %f", robot_info_.raw_info.front_overhang_m); |
| } | ||
|
|
||
| pcl::PointCloud<pcl::PointXYZ>::Ptr crop_box( | ||
| const pcl::PointCloud<pcl::PointXYZ>::Ptr & pcl_output, |
There was a problem hiding this comment.
関数の引数の命名は「そのオブジェクトから見た命名」にするべきなので、outputではなくinputであるべきです。
+@ 可能なら第2引数と命名を揃えてください。
| const pcl::PointCloud<pcl::PointXYZ>::Ptr & pcl_output, | ||
| pcl::PointCloud<pcl::PointXYZ>::Ptr & cropped_cloud) | ||
| { | ||
| Eigen::Vector4f self_min_pt( |
There was a problem hiding this comment.
min_ptとmax_ptは毎ループ変わるわけではないので、メンバ変数としてキャッシュするべきです。
| pcl::CropBox<pcl::PointXYZ> area_crop_box_filter; | ||
| area_crop_box_filter.setInputCloud(self_cropped_cloud); | ||
| Eigen::Vector4f area_min_pt(-5.0, -5.0, -5.0, 1.0); | ||
| Eigen::Vector4f area_max_pt(5.0, 5.0, 5.0, 1.0); |
There was a problem hiding this comment.
robot_infoはwheel_stuck_robot_launcherパッケージのload_robot_info.launch.pyによって配信されるため、このlaunchでは扱わないでください。
|
|
||
| // 受信機を作る。 | ||
| pc_sub_ = this->create_subscription<PointCloud2>( | ||
| "/velodyne_points", 10, |
There was a problem hiding this comment.
.launch.xml内のremapのトピック名と一致していません。
| <arg name="config_file2" default="$(find-pkg-share wheel_stuck_robot_utils)/config/robot_info.param.yaml"/> | ||
|
|
||
| <node pkg="pointcloud_preprocessor" exec="pointcloud_preprocessor_node" name="pointcloud_preprocessor" output="screen"> | ||
| <remap from="~/input/points" to="$(var points_topic)"/> |
| transform_matrix = to_matrix4f(transform_stamped.transform); | ||
|
|
||
| // 点群データの座標変換 | ||
| pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_output(new pcl::PointCloud<pcl::PointXYZ>()); |
There was a problem hiding this comment.
変数名として、pcl_transformedあたりが無難(?)
| pcl::transformPointCloud(*pcl_input, *pcl_output, transform_matrix); | ||
|
|
||
| // クロッピング処理 | ||
| cropbox_filter::CropBoxFilter crop_box_filter(*this); |
There was a problem hiding this comment.
crop_box_filterの設定(クロップ範囲など)はループ毎に不変なので、crop_box_filterはメンバとしてキャッシュするべきです。
| cropbox_filter::CropBoxFilter crop_box_filter(*this); | ||
| // デバッグ用のパラメータ確認メソッドを呼び出す。デバッグ用。 | ||
| // crop_box_filter.debug_parameters(); | ||
| pcl::PointCloud<pcl::PointXYZ>::Ptr cropped_cloud(new pcl::PointCloud<pcl::PointXYZ>()); |
| pcl::VoxelGrid<pcl::PointXYZ> voxel_grid; | ||
| voxel_grid.setInputCloud(cropped_cloud); | ||
| voxel_grid.setLeafSize(leaf_size_, leaf_size_, leaf_size_); | ||
| pcl::PointCloud<pcl::PointXYZ> downsampled_cloud; |
There was a problem hiding this comment.
命名を揃えるならばpcl_downsampled?
| return cropped_cloud; | ||
| } | ||
| // デバッグ用。パラメーターの取得の確認。 | ||
| void debug_parameters() const |
There was a problem hiding this comment.
print_debug_parameters()のほうがより関数の目的が説明できそう？
There was a problem hiding this comment.
+@
CropBoxFilterのパラメータのprintではなく、RobotInfoのパラメータのprintになっている。
WheelRadius, WheelTreadなどのパラメータをprintする関数はRobotInfo側に実装するべきで、CropBoxFilter側で実装するprint関数はCropBoxのパラメータ(self_min_pt)とかをprintするべき。
3 participants
pointcloud_preprocessorのtfを用いた座標変換のプログラミングを書いたので、改善点などのレビューをお願いします。
ノード立ち上げ（wheel_stuck_common_utilsのビルドも必要）
colcon build --packages-select pointcloud_preprocessor
source install/setup.bash
ros2 run pointcloud_preprocessor pointcloud_preprocessor_node
点群データはrosbagとrvizを使って確認しています。