Skip to content
This repository has been archived by the owner on Nov 13, 2017. It is now read-only.

Add a max range to raytracing in PointCloudOctomapUpdater #597

Open
wants to merge 2 commits into
base: indigo-devel
Choose a base branch
from
Open

Add a max range to raytracing in PointCloudOctomapUpdater #597

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
eeb4580
Added a max range to raytracing
TheBrewCrew Jul 29, 2015
73b18f9
Added default for raytrace_range
TheBrewCrew Aug 14, 2015
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
4 changes: 3 additions & 1 deletion ...ud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -81,7 +81,9 @@ class PointCloudOctomapUpdater : public OccupancyMapUpdater
std::string point_cloud_topic_;
double scale_;
double padding_;
double max_range_;
// Ranges have same meaning as in obstacle layer in costmap_2d of the nav stack
double obstacle_range_; // Max range at which obstacle will be inserted
double raytrace_range_; // Max range to ray trace free space
unsigned int point_subsample_;
std::string filtered_cloud_topic_;
ros::Publisher filtered_cloud_publisher_;
Expand Down
18 changes: 14 additions & 4 deletions perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,8 @@ PointCloudOctomapUpdater::PointCloudOctomapUpdater() : OccupancyMapUpdater("Poin
private_nh_("~"),
scale_(1.0),
padding_(0.0),
max_range_(std::numeric_limits<double>::infinity()),
obstacle_range_(std::numeric_limits<double>::infinity()),
raytrace_range_(std::numeric_limits<double>::infinity()),
point_subsample_(1),
point_cloud_subscriber_(NULL),
point_cloud_filter_(NULL)
Expand All @@ -68,7 +69,10 @@ bool PointCloudOctomapUpdater::setParams(XmlRpc::XmlRpcValue &params)
return false;
point_cloud_topic_ = static_cast<const std::string&>(params["point_cloud_topic"]);

readXmlParam(params, "max_range", &max_range_);
readXmlParam(params, "max_range", &obstacle_range_);
readXmlParam(params, "obstacle_range", &obstacle_range_);
raytrace_range_ = 1.1*obstacle_range_;
readXmlParam(params, "raytrace_range", &raytrace_range_);
readXmlParam(params, "padding_offset", &padding_);
readXmlParam(params, "padding_scale", &scale_);
readXmlParam(params, "point_subsample", &point_subsample_);
Expand Down Expand Up @@ -200,7 +204,7 @@ void PointCloudOctomapUpdater::cloudMsgCallback(const sensor_msgs::PointCloud2::
}

/* mask out points on the robot */
shape_mask_->maskContainment(*cloud_msg, sensor_origin_eigen, 0.0, max_range_, mask_);
shape_mask_->maskContainment(*cloud_msg, sensor_origin_eigen, 0.0, obstacle_range_, mask_);
updateMask(*cloud_msg, sensor_origin_eigen, mask_);

octomap::KeySet free_cells, occupied_cells, model_cells, clip_cells;
Expand Down Expand Up @@ -233,6 +237,7 @@ void PointCloudOctomapUpdater::cloudMsgCallback(const sensor_msgs::PointCloud2::
{
/* do ray tracing to find which cells this point cloud indicates should be free, and which it indicates
* should be occupied */
double raytrace_range_2 = raytrace_range_ * raytrace_range_;
for (unsigned int row = 0; row < cloud_msg->height; row += point_subsample_)
{
unsigned int row_c = row * cloud_msg->width;
Expand All @@ -258,7 +263,12 @@ void PointCloudOctomapUpdater::cloudMsgCallback(const sensor_msgs::PointCloud2::
if (mask_[row_c + col] == point_containment_filter::ShapeMask::INSIDE)
model_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ()));
else if (mask_[row_c + col] == point_containment_filter::ShapeMask::CLIP)
clip_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ()));
{
octomap::point3d clip_end(point_tf.getX(), point_tf.getY(), point_tf.getZ());
if (point_tf.length2() > raytrace_range_2)
clip_end = sensor_origin + (clip_end - sensor_origin).normalized() * (float)raytrace_range_;
clip_cells.insert(tree_->coordToKey(clip_end));
}
else
{
occupied_cells.insert(tree_->coordToKey(point_tf.getX(), point_tf.getY(), point_tf.getZ()));
Expand Down