Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix(dummy_perception_publisher): publish multiple layers of pointcloud #882

Merged
merged 2 commits into from
May 11, 2022
Merged

fix(dummy_perception_publisher): publish multiple layers of pointcloud #882

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
25166e6
fix: single -> multiple layers pointcloud
HiroIshida May 10, 2022
d49b56a
refactor: share common among different pcloud creators
HiroIshida May 10, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
56 changes: 41 additions & 15 deletions simulator/dummy_perception_publisher/src/pointcloud_creator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,28 +25,34 @@
#include <limits>
#include <memory>

namespace
{

static constexpr double epsilon = 0.001;
static constexpr double step = 0.05;
static constexpr double vertical_theta_step = (1.0 / 180.0) * M_PI;
static constexpr double vertical_min_theta = (-15.0 / 180.0) * M_PI;
static constexpr double vertical_max_theta = (15.0 / 180.0) * M_PI;
static constexpr double horizontal_theta_step = (0.1 / 180.0) * M_PI;
static constexpr double horizontal_min_theta = (-180.0 / 180.0) * M_PI;
static constexpr double horizontal_max_theta = (180.0 / 180.0) * M_PI;

pcl::PointXYZ getPointWrtBaseLink(
const tf2::Transform & tf_base_link2moved_object, double x, double y, double z)
{
const auto p_wrt_base = tf_base_link2moved_object(tf2::Vector3(x, y, z));
return pcl::PointXYZ(p_wrt_base.x(), p_wrt_base.y(), p_wrt_base.z());
}

} // namespace

void ObjectCentricPointCloudCreator::create_object_pointcloud(
const ObjectInfo & obj_info, const tf2::Transform & tf_base_link2map,
std::mt19937 & random_generator, pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud) const
{
std::normal_distribution<> x_random(0.0, obj_info.std_dev_x);
std::normal_distribution<> y_random(0.0, obj_info.std_dev_y);
std::normal_distribution<> z_random(0.0, obj_info.std_dev_z);
const double epsilon = 0.001;
const double step = 0.05;
const double vertical_theta_step = (1.0 / 180.0) * M_PI;
const double vertical_min_theta = (-15.0 / 180.0) * M_PI;
const double vertical_max_theta = (15.0 / 180.0) * M_PI;
const double horizontal_theta_step = (0.1 / 180.0) * M_PI;
const double horizontal_min_theta = (-180.0 / 180.0) * M_PI;
const double horizontal_max_theta = (180.0 / 180.0) * M_PI;

const auto tf_base_link2moved_object = tf_base_link2map * obj_info.tf_map2moved_object;

Expand Down Expand Up @@ -206,7 +212,18 @@ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> EgoCentricPointCloudCreator::cr
pointclouds.at(i) = (pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
}

const double horizontal_theta_step = 0.25 * M_PI / 180.0;
std::vector<double> min_zs(obj_infos.size());
std::vector<double> max_zs(obj_infos.size());

for (size_t idx = 0; idx < obj_infos.size(); ++idx) {
const auto & obj_info = obj_infos.at(idx);
const auto tf_base_link2moved_object = tf_base_link2map * obj_info.tf_map2moved_object;
const double min_z = -1.0 * (obj_info.height / 2.0) + tf_base_link2moved_object.getOrigin().z();
const double max_z = 1.0 * (obj_info.height / 2.0) + tf_base_link2moved_object.getOrigin().z();
min_zs.at(idx) = min_z;
max_zs.at(idx) = max_z;
}

double angle = 0.0;
const auto n_scan = static_cast<size_t>(std::floor(2 * M_PI / horizontal_theta_step));
for (size_t i = 0; i < n_scan; ++i) {
Expand All @@ -217,13 +234,22 @@ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> EgoCentricPointCloudCreator::cr
const auto x_hit = dist * cos(angle);
const auto y_hit = dist * sin(angle);
const auto idx_hit = composite_sdf.nearest_sdf_index(x_hit, y_hit);
const auto obj_info_here = obj_infos.at(idx_hit);
const auto min_z_here = min_zs.at(idx_hit);
const auto max_z_here = max_zs.at(idx_hit);
std::normal_distribution<> x_random(0.0, obj_info_here.std_dev_x);
std::normal_distribution<> y_random(0.0, obj_info_here.std_dev_y);
std::normal_distribution<> z_random(0.0, obj_info_here.std_dev_z);

std::normal_distribution<> x_random(0.0, obj_infos.at(idx_hit).std_dev_x);
std::normal_distribution<> y_random(0.0, obj_infos.at(idx_hit).std_dev_y);
std::normal_distribution<> z_random(0.0, obj_infos.at(idx_hit).std_dev_z);
pointclouds.at(idx_hit)->push_back(pcl::PointXYZ(
x_hit + x_random(random_generator), y_hit + y_random(random_generator),
z_random(random_generator)));
for (double vertical_theta = vertical_min_theta;
vertical_theta <= vertical_max_theta + epsilon; vertical_theta += vertical_theta_step) {
const double z = dist * std::tan(vertical_theta);
if (min_z_here <= z && z <= max_z_here + epsilon) {
pointclouds.at(idx_hit)->push_back(pcl::PointXYZ(
x_hit + x_random(random_generator), y_hit + y_random(random_generator),
z + z_random(random_generator)));
}
}
}
}

Expand Down