From f823a42192ce368c665d26c1cca09cf4dcea7edc Mon Sep 17 00:00:00 2001 From: Connor Brew Date: Thu, 30 Jul 2015 13:36:02 -0700 Subject: [PATCH 1/2] Raytrace range for jade-devel, has deprecation warning on max_range --- .../pointcloud_octomap_updater.h | 4 +++- .../src/pointcloud_octomap_updater.cpp | 21 +++++++++++++++---- 2 files changed, 20 insertions(+), 5 deletions(-) diff --git a/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h b/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h index 66f4fa189a..4fcecae451 100644 --- a/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h +++ b/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h @@ -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_; diff --git a/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp b/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp index f3e3091f83..a4e797f7bf 100644 --- a/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp +++ b/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp @@ -48,7 +48,8 @@ PointCloudOctomapUpdater::PointCloudOctomapUpdater() : OccupancyMapUpdater("Poin private_nh_("~"), scale_(1.0), padding_(0.0), - max_range_(std::numeric_limits::infinity()), + obstacle_range_(std::numeric_limits::infinity()), + raytrace_range_(std::numeric_limits::infinity()), point_subsample_(1), point_cloud_subscriber_(NULL), point_cloud_filter_(NULL) @@ -68,7 +69,13 @@ bool PointCloudOctomapUpdater::setParams(XmlRpc::XmlRpcValue ¶ms) return false; point_cloud_topic_ = static_cast(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_); + readXmlParam(params, "raytrace_range", &raytrace_range_); readXmlParam(params, "padding_offset", &padding_); readXmlParam(params, "padding_scale", &scale_); readXmlParam(params, "point_subsample", &point_subsample_); @@ -200,7 +207,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; @@ -233,6 +240,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; @@ -258,7 +266,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())); From 4d8175927eb6084f7a826d393c4b4bb44e17f21b Mon Sep 17 00:00:00 2001 From: Connor Brew Date: Fri, 14 Aug 2015 12:28:00 -0700 Subject: [PATCH 2/2] Added default for raytrace_range --- .../src/pointcloud_octomap_updater.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp b/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp index a4e797f7bf..e9a3dd7d85 100644 --- a/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp +++ b/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp @@ -75,6 +75,7 @@ bool PointCloudOctomapUpdater::setParams(XmlRpc::XmlRpcValue ¶ms) 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_);