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 (Jade) #606

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

Add a max range to raytracing in PointCloudOctomapUpdater (Jade) #606

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
f823a42
Raytrace range for jade-devel, has deprecation warning on max_range
TheBrewCrew Jul 30, 2015
4d81759
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
22 changes: 18 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,14 @@ 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_);
if (params.hasMember("max_range"))
{
ROS_WARN("max_range is deprecated, use obstacle_range instead");
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 +208,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 +241,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 +267,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