Skip to content

Commit

Permalink
fix problems
Browse files Browse the repository at this point in the history
  • Loading branch information
@Bey9434
Bey9434 committed Aug 29, 2024
1 parent 38d16ba commit 1977b32
Show file tree
Hide file tree
Showing 5 changed files with 70 additions and 73 deletions.
14 changes: 10 additions & 4 deletions src/sensing/pointcloud_preprocessor/config/pointcloud_preprocessor.param.yaml
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,11 @@
/**:
pointcloud_preprocessor:
ros__parameters:
target_frame_id: base_link
leaf_size: 0.1
ros__parameters:
target_frame_id: base_link
leaf_size: 0.15
crop_area_min_x: -5.0
crop_area_min_y: -5.0
crop_area_min_z: -5.0
crop_area_max_x: 5.0
crop_area_max_y: 5.0
crop_area_max_z: 5.0
crop_area_w: 1.0
97 changes: 46 additions & 51 deletions src/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/cropbox_filter.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,73 +44,68 @@ class CropBoxFilter
// robot_infoからのパラメータの読み取りを行う。
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);

// 必要であれば、debug_parameters()を呼び出して詳細をログ出力
debug_parameters();
}

pcl::PointCloud<pcl::PointXYZ>::Ptr crop_box(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & pcl_output,
pcl::PointCloud<pcl::PointXYZ>::Ptr & cropped_cloud)
{
Eigen::Vector4f self_min_pt(
-robot_info_.raw_info.rear_overhang_m, // x軸負方向（後方）
-robot_info_.raw_info.right_overhang_m, // y軸負方向（左側）
-robot_info_.raw_info.robot_height_m, // z軸負方向（下方）
self_min_pt_ = Eigen::Vector4f(
-robot_info_.raw_info.rear_overhang_m, // x軸負方向（後方）
-robot_info_.raw_info.right_overhang_m -
robot_info_.raw_info.wheel_tread_m / 2.0, // y軸負方向（右側）
-robot_info_.raw_info.robot_height_m -
robot_info_.raw_info.wheel_radius_m, // z軸負方向（下方）
1.0);

Eigen::Vector4f self_max_pt(
self_max_pt_ = Eigen::Vector4f(
robot_info_.raw_info.front_overhang_m, // x軸正方向（前方）
robot_info_.raw_info.left_overhang_m, // y軸正方向（右側）
robot_info_.raw_info.robot_height_m, // z軸正方向（上方）
robot_info_.raw_info.left_overhang_m +
robot_info_.raw_info.wheel_tread_m / 2.0, // y軸正方向（左側）
robot_info_.raw_info.robot_height_m -
robot_info_.raw_info.wheel_radius_m, // z軸正方向（上方）
1.0);

pcl::CropBox<pcl::PointXYZ> self_crop_box_filter;
min_x_ = node.declare_parameter<double>("crop_area_min_x", -1.0);
min_y_ = node.declare_parameter<double>("crop_area_min_y", -1.0);
min_z_ = node.declare_parameter<double>("crop_area_min_z", -1.0);
max_x_ = node.declare_parameter<double>("crop_area_max_x", 1.0);
max_y_ = node.declare_parameter<double>("crop_area_max_y", 1.0);
max_z_ = node.declare_parameter<double>("crop_area_max_z", 1.0);
crop_area_w_ = node.declare_parameter<double>("crop_area_w", 1.0);

self_crop_box_filter.setInputCloud(pcl_output);
self_crop_box_filter.setMin(self_min_pt);
self_crop_box_filter.setMax(self_max_pt);
area_min_pt_ = Eigen::Vector4f(min_x_, min_y_, min_z_, crop_area_w_);
area_max_pt_ = Eigen::Vector4f(max_x_, max_y_, max_z_, crop_area_w_);
}

pcl::PointCloud<pcl::PointXYZ>::Ptr crop_box(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & pcl_input_cloud,
pcl::PointCloud<pcl::PointXYZ>::Ptr & pcl_cropped_cloud)
{
self_crop_box_filter.setInputCloud(pcl_input_cloud);
self_crop_box_filter.setMin(self_min_pt_);
self_crop_box_filter.setMax(self_max_pt_);
pcl::PointCloud<pcl::PointXYZ>::Ptr self_cropped_cloud(new pcl::PointCloud<pcl::PointXYZ>());
self_crop_box_filter.setNegative(true);
self_crop_box_filter.filter(*self_cropped_cloud);

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);
area_crop_box_filter.setMin(area_min_pt);
area_crop_box_filter.setMax(area_max_pt);
area_crop_box_filter.filter(*cropped_cloud);
area_crop_box_filter.setMin(area_min_pt_);
area_crop_box_filter.setMax(area_max_pt_);
area_crop_box_filter.filter(*pcl_cropped_cloud);

return cropped_cloud;
}
// デバッグ用。パラメーターの取得の確認。
void debug_parameters() const
{
RCLCPP_INFO(
rclcpp::get_logger("CropBoxFilter"), "Wheel Radius: %f", robot_info_.raw_info.wheel_radius_m);
RCLCPP_INFO(
rclcpp::get_logger("CropBoxFilter"), "Wheel Tread: %f", robot_info_.raw_info.wheel_tread_m);
RCLCPP_INFO(
rclcpp::get_logger("CropBoxFilter"), "Front Overhang: %f",
robot_info_.raw_info.front_overhang_m);
RCLCPP_INFO(
rclcpp::get_logger("CropBoxFilter"), "Rear Overhang: %f",
robot_info_.raw_info.rear_overhang_m);
RCLCPP_INFO(
rclcpp::get_logger("CropBoxFilter"), "Left Overhang: %f",
robot_info_.raw_info.left_overhang_m);
RCLCPP_INFO(
rclcpp::get_logger("CropBoxFilter"), "Right Overhang: %f",
robot_info_.raw_info.right_overhang_m);
RCLCPP_INFO(
rclcpp::get_logger("CropBoxFilter"), "Robot Height: %f", robot_info_.raw_info.robot_height_m);
return pcl_cropped_cloud;
}

private:
wheel_stuck_robot_utils::RobotInfo robot_info_;
Eigen::Vector4f self_min_pt_;
Eigen::Vector4f self_max_pt_;
Eigen::Vector4f area_min_pt_;
Eigen::Vector4f area_max_pt_;
pcl::CropBox<pcl::PointXYZ> self_crop_box_filter;
pcl::CropBox<pcl::PointXYZ> area_crop_box_filter;
double min_x_;
double min_y_;
double min_z_;
double max_x_;
double max_y_;
double max_z_;
double crop_area_w_;
};

} // namespace cropbox_filter
Expand Down
3 changes: 3 additions & 0 deletions ...nsing/pointcloud_preprocessor/include/pointcloud_preprocessor/pointcloud_preprocessor.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,8 @@
#ifndef POINTCLOUD_PREPROCESSOR__POINTCLOUD_PREPROCESSOR_HPP_
#define POINTCLOUD_PREPROCESSOR__POINTCLOUD_PREPROCESSOR_HPP_

#include "pointcloud_preprocessor/cropbox_filter.hpp"

#include <Eigen/Core>
#include <rclcpp/rclcpp.hpp>
#include <wheel_stuck_common_utils/geometry/conversion.hpp>
Expand Down Expand Up @@ -51,6 +53,7 @@ class PointCloudPreprocessor : public rclcpp::Node
tf2_ros::TransformListener tf_listener_;
std::string target_frame_id_;
double leaf_size_;
cropbox_filter::CropBoxFilter crop_box_filter;
};

} // namespace pointcloud_preprocessor
Expand Down
4 changes: 1 addition & 3 deletions src/sensing/pointcloud_preprocessor/launch/pointcloud_preprocessor.launch.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,12 +2,10 @@
<arg name="points_topic" default="/velodyne_points"/>
<arg name="filtered_points_topic" default="~/output/filtered_points"/>
<arg name="config_file" default="$(find-pkg-share pointcloud_preprocessor)/config/pointcloud_preprocessor.param.yaml"/>
<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)"/>
<remap from="/velodyne_points" to="$(var points_topic)"/>
<remap from="~/output/filtered_points" to="$(var filtered_points_topic)"/>
<param from="$(var config_file)"/>
<param from="$(var config_file2)"/>
</node>
</launch>
25 changes: 10 additions & 15 deletions src/sensing/pointcloud_preprocessor/src/pointcloud_preprocessor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,16 +14,15 @@

#include "pointcloud_preprocessor/pointcloud_preprocessor.hpp"

#include "pointcloud_preprocessor/cropbox_filter.hpp"

#include <wheel_stuck_robot_utils/robot_info.hpp>

namespace pointcloud_preprocessor
{
PointCloudPreprocessor::PointCloudPreprocessor(const rclcpp::NodeOptions & options)
: Node("pointcloud_preprocessor", options),
tf_buffer_(this->get_clock()), // tf2_ros::Bufferの初期化
tf_listener_(tf_buffer_) // tf2_ros::TransformListenerの初期化
tf_listener_(tf_buffer_), // tf2_ros::TransformListenerの初期化
crop_box_filter(*this)

{
// パラメーターの宣言
Expand All @@ -41,7 +40,6 @@ PointCloudPreprocessor::PointCloudPreprocessor(const rclcpp::NodeOptions & optio

void PointCloudPreprocessor::process_pointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
{
// std::cout << "Frame ID: " << msg->header.frame_id << std::endl;
geometry_msgs::msg::TransformStamped transform_stamped;

try {
Expand All @@ -62,26 +60,23 @@ void PointCloudPreprocessor::process_pointcloud(const sensor_msgs::msg::PointClo
transform_matrix = to_matrix4f(transform_stamped.transform);

// 点群データの座標変換
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_output(new pcl::PointCloud<pcl::PointXYZ>());
pcl::transformPointCloud(*pcl_input, *pcl_output, transform_matrix);
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_transformed(new pcl::PointCloud<pcl::PointXYZ>());
pcl::transformPointCloud(*pcl_input, *pcl_transformed, transform_matrix);

// クロッピング処理
cropbox_filter::CropBoxFilter crop_box_filter(*this);
// デバッグ用のパラメータ確認メソッドを呼び出す。デバッグ用。
// crop_box_filter.debug_parameters();
pcl::PointCloud<pcl::PointXYZ>::Ptr cropped_cloud(new pcl::PointCloud<pcl::PointXYZ>());
crop_box_filter.crop_box(pcl_output, cropped_cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cropped(new pcl::PointCloud<pcl::PointXYZ>());
crop_box_filter.crop_box(pcl_transformed, pcl_cropped);

// ダウンサンプリング
pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;
voxel_grid.setInputCloud(cropped_cloud);
voxel_grid.setInputCloud(pcl_cropped);
voxel_grid.setLeafSize(leaf_size_, leaf_size_, leaf_size_);
pcl::PointCloud<pcl::PointXYZ> downsampled_cloud;
voxel_grid.filter(downsampled_cloud);
pcl::PointCloud<pcl::PointXYZ> pcl_downsampled;
voxel_grid.filter(pcl_downsampled);

// 座標変換した点群データを再びROSメッセージ形式に変換
PointCloud2 output_msg;
pcl::toROSMsg(downsampled_cloud, output_msg);
pcl::toROSMsg(pcl_downsampled, output_msg);

// メッセージのヘッダーを設定
output_msg.header = msg->header;
Expand Down

0 comments on commit 1977b32

Please sign in to comment.