add multi camera depth
This commit is contained in:
@@ -131,6 +131,10 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
|
||||
double observation_keep_time = 0, expected_update_rate = 0, min_obstacle_height = 0, max_obstacle_height = 2;
|
||||
std::string topic = "map", sensor_frame = "laser_frame", data_type = "PointCloud";
|
||||
bool inf_is_valid = false, clearing=false, marking=true;
|
||||
bool frustum_clearing_enabled = false;
|
||||
int frustum_pixel_step = 8;
|
||||
double frustum_min_range = 0.2;
|
||||
double frustum_max_range = 3.0;
|
||||
|
||||
robot::NodeHandle priv_nh(nh, source);
|
||||
topic = loadParam(layer[source],"topic", topic);
|
||||
@@ -143,6 +147,10 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
|
||||
inf_is_valid = loadParam(layer[source],"inf_is_valid", false);
|
||||
clearing = loadParam(layer[source],"clearing", false);
|
||||
marking = loadParam(layer[source],"marking", true);
|
||||
frustum_clearing_enabled = loadParam(layer, "frustum_clearing_enabled", false);
|
||||
frustum_pixel_step = loadParam(layer, "frustum_clearing_pixel_step", 8);
|
||||
frustum_min_range = loadParam(layer, "frustum_min_range", 0.2);
|
||||
frustum_max_range = loadParam(layer, "frustum_max_range", 3.0);
|
||||
|
||||
if (priv_nh.hasParam("topic"))
|
||||
priv_nh.getParam("topic", topic);
|
||||
@@ -164,52 +172,83 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
|
||||
priv_nh.getParam("clearing", clearing);
|
||||
if (priv_nh.hasParam("marking"))
|
||||
priv_nh.getParam("marking", marking);
|
||||
|
||||
if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan"))
|
||||
if (priv_nh.hasParam("frustum_clearing_enabled"))
|
||||
priv_nh.getParam("frustum_clearing_enabled", frustum_clearing_enabled);
|
||||
if (priv_nh.hasParam("frustum_clearing_pixel_step"))
|
||||
{
|
||||
robot::log_error("Only topics that use point clouds or laser scans are currently supported\n");
|
||||
throw std::runtime_error("Only topics that use point clouds or laser scans are currently supported");
|
||||
priv_nh.getParam("frustum_clearing_pixel_step", frustum_pixel_step);
|
||||
frustum_pixel_step = std::max(1, frustum_pixel_step);
|
||||
}
|
||||
if (priv_nh.hasParam("frustum_min_range"))
|
||||
priv_nh.getParam("frustum_min_range", frustum_min_range);
|
||||
if (priv_nh.hasParam("frustum_max_range"))
|
||||
priv_nh.getParam("frustum_max_range", frustum_max_range);
|
||||
if (priv_nh.hasParam("frustum_depth_camera_topic"))
|
||||
priv_nh.getParam("frustum_depth_camera_topic", depth_camera_data_topic_);
|
||||
|
||||
CallBackInfo info_tmp;
|
||||
info_tmp.observation_source = source;
|
||||
info_tmp.data_type = data_type;
|
||||
info_tmp.topic = topic;
|
||||
info_tmp.inf_is_valid = inf_is_valid;
|
||||
callback_infos_.push_back(info_tmp);
|
||||
|
||||
std::string raytrace_range_param_name, obstacle_range_param_name;
|
||||
robot::log_info("frustum_clearing_enabled: %s, frustum_clearing_pixel_step: %d, frustum_min_range: %f, frustum_max_range: %f", frustum_clearing_enabled ? "true" : "false", frustum_pixel_step, frustum_min_range, frustum_max_range);
|
||||
|
||||
double obstacle_range = 2.5;
|
||||
obstacle_range = loadParam(layer[source],"obstacle_range", obstacle_range);
|
||||
double raytrace_range = 3.0;
|
||||
raytrace_range = loadParam(layer[source],"raytrace_range", raytrace_range);
|
||||
|
||||
|
||||
|
||||
if (priv_nh.hasParam("obstacle_range"))
|
||||
priv_nh.getParam("obstacle_range", obstacle_range);
|
||||
if (priv_nh.hasParam("raytrace_range"))
|
||||
priv_nh.getParam("raytrace_range", raytrace_range);
|
||||
|
||||
// enabled_ = enabled;
|
||||
if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan" || data_type == "DepthCameraData"))
|
||||
{
|
||||
robot::log_error("Only topics that use point clouds or laser scans are currently supported\n");
|
||||
throw std::runtime_error("Only topics that use point clouds or laser scans are currently supported");
|
||||
}
|
||||
|
||||
robot::log_info("Creating an observation buffer for topic %s, frame %s\n", topic.c_str(),
|
||||
priv_nh.getNamespace().c_str());
|
||||
if(!frustum_clearing_enabled)
|
||||
{
|
||||
|
||||
// create an observation buffer
|
||||
observation_buffers_.push_back(
|
||||
boost::shared_ptr < ObservationBuffer
|
||||
> (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height,
|
||||
max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_,
|
||||
sensor_frame, transform_tolerance)));
|
||||
if (marking)
|
||||
marking_buffers_.push_back(observation_buffers_.back());
|
||||
CallBackInfo info_tmp;
|
||||
info_tmp.observation_source = source;
|
||||
info_tmp.data_type = data_type;
|
||||
info_tmp.topic = topic;
|
||||
info_tmp.inf_is_valid = inf_is_valid;
|
||||
callback_infos_.push_back(info_tmp);
|
||||
|
||||
// check if we'll also add this buffer to our clearing observation buffers
|
||||
if (clearing)
|
||||
clearing_buffers_.push_back(observation_buffers_.back());
|
||||
// enabled_ = enabled;
|
||||
|
||||
robot::log_info("Creating an observation buffer for topic %s, frame %s\n", topic.c_str(),
|
||||
priv_nh.getNamespace().c_str());
|
||||
|
||||
// create an observation buffer
|
||||
observation_buffers_.push_back(
|
||||
boost::shared_ptr < ObservationBuffer
|
||||
> (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height,
|
||||
max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_,
|
||||
sensor_frame, transform_tolerance)));
|
||||
if (marking)
|
||||
marking_buffers_.push_back(observation_buffers_.back());
|
||||
|
||||
// check if we'll also add this buffer to our clearing observation buffers
|
||||
if (clearing)
|
||||
clearing_buffers_.push_back(observation_buffers_.back());
|
||||
}
|
||||
else
|
||||
{
|
||||
CallBackInfo info_tmp;
|
||||
info_tmp.observation_source = source;
|
||||
info_tmp.data_type = data_type;
|
||||
info_tmp.topic = topic;
|
||||
info_tmp.inf_is_valid = inf_is_valid;
|
||||
callback_depth_infos_.push_back(info_tmp);
|
||||
|
||||
depth_observation_buffers_.push_back(
|
||||
boost::shared_ptr < ObservationBuffer
|
||||
> (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height,
|
||||
max_obstacle_height, obstacle_range, raytrace_range, frustum_pixel_step,
|
||||
frustum_min_range, frustum_max_range, *tf_, global_frame_,
|
||||
sensor_frame, transform_tolerance)));
|
||||
|
||||
}
|
||||
robot::log_info(
|
||||
"Created an observation buffer for topic %s, global frame: %s, "
|
||||
"expected update rate: %.2f, observation persistence: %.2f\n",
|
||||
@@ -232,7 +271,85 @@ void ObstacleLayer::handleImpl(const void* data,
|
||||
{
|
||||
if(!stop_receiving_data_)
|
||||
{
|
||||
|
||||
if (type == typeid(robot_sensor_msgs::DepthCameraData::ConstPtr) )
|
||||
{
|
||||
const robot_sensor_msgs::DepthCameraData::ConstPtr& depth_camera_data_ptr =
|
||||
*static_cast<const robot_sensor_msgs::DepthCameraData::ConstPtr*>(data);
|
||||
if (!depth_camera_data_ptr)
|
||||
return;
|
||||
|
||||
const robot_sensor_msgs::DepthCameraData& depth_camera_data =
|
||||
*depth_camera_data_ptr;
|
||||
const robot_sensor_msgs::Image& depth = depth_camera_data.depth;
|
||||
const robot_sensor_msgs::CameraInfo& camera_info = depth_camera_data.camera_info;
|
||||
|
||||
std::size_t bytes_per_pixel = 0;
|
||||
if (depth.encoding == "16UC1" || depth.encoding == "mono16")
|
||||
bytes_per_pixel = 2;
|
||||
else if (depth.encoding == "32FC1")
|
||||
bytes_per_pixel = 4;
|
||||
else
|
||||
{
|
||||
robot::log_error("ObstacleLayer received unsupported depth encoding: %s\n", depth.encoding.c_str());
|
||||
return;
|
||||
}
|
||||
|
||||
const bool invalid_dimensions = depth.width == 0 || depth.height == 0 ||
|
||||
depth.step < static_cast<std::size_t>(depth.width) * bytes_per_pixel ||
|
||||
depth.data.size() < static_cast<std::size_t>(depth.step) * depth.height;
|
||||
if (invalid_dimensions)
|
||||
{
|
||||
robot::log_error("ObstacleLayer received malformed DepthCameraData image\n");
|
||||
return;
|
||||
}
|
||||
|
||||
if (camera_info.K[0] <= 0.0 || camera_info.K[4] <= 0.0)
|
||||
{
|
||||
robot::log_error("ObstacleLayer received invalid camera intrinsics for depth clearing\n");
|
||||
return;
|
||||
}
|
||||
|
||||
if ((camera_info.width != 0 && camera_info.width != depth.width) ||
|
||||
(camera_info.height != 0 && camera_info.height != depth.height))
|
||||
{
|
||||
robot::log_error("ObstacleLayer received mismatched depth image and camera info dimensions\n");
|
||||
return;
|
||||
}
|
||||
|
||||
const std::string& depth_frame = depth.header.frame_id;
|
||||
const std::string& camera_frame = camera_info.header.frame_id;
|
||||
if (!depth_frame.empty() && !camera_frame.empty() && depth_frame != camera_frame)
|
||||
{
|
||||
robot::log_error("ObstacleLayer received mismatched depth and camera-info frames: %s != %s\n",
|
||||
depth_frame.c_str(), camera_frame.c_str());
|
||||
return;
|
||||
}
|
||||
|
||||
if (depth_camera_data.header.frame_id.empty() && depth_frame.empty() && camera_frame.empty())
|
||||
{
|
||||
robot::log_error("ObstacleLayer received DepthCameraData without an optical frame\n");
|
||||
return;
|
||||
}
|
||||
|
||||
// std::lock_guard<std::mutex> lock(depth_camera_data_mutex_);
|
||||
// pending_depth_camera_data_ = depth_camera_data_ptr;
|
||||
if(depth_observation_buffers_.empty() || callback_depth_infos_.empty()) return;
|
||||
|
||||
int size_callback_depth = static_cast<int>(callback_depth_infos_.size());
|
||||
for(int i = 0; i < size_callback_depth; i++)
|
||||
{
|
||||
boost::shared_ptr<ObservationBuffer>& buffer = depth_observation_buffers_[i];
|
||||
if (type == typeid(robot_sensor_msgs::DepthCameraData::ConstPtr) &&
|
||||
topic == callback_depth_infos_[i].topic)
|
||||
{
|
||||
// robot::log_error_throttle(1.0,"TEST");
|
||||
depthImageCallback(depth_camera_data, buffer);
|
||||
}
|
||||
}
|
||||
// return;
|
||||
}
|
||||
else
|
||||
{
|
||||
if(observation_buffers_.empty() || callback_infos_.empty()) return;
|
||||
|
||||
int size_callback = static_cast<int>(callback_infos_.size());
|
||||
@@ -286,6 +403,7 @@ void ObstacleLayer::handleImpl(const void* data,
|
||||
// << "topic check: " << callback_infos_[i].topic << std::endl << std::endl;
|
||||
// }
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -392,6 +510,15 @@ void ObstacleLayer::pointCloud2Callback(const robot_sensor_msgs::PointCloud2& me
|
||||
buffer->unlock();
|
||||
}
|
||||
|
||||
void ObstacleLayer::depthImageCallback(const robot_sensor_msgs::DepthCameraData& message,
|
||||
const boost::shared_ptr<ObservationBuffer>& buffer)
|
||||
{
|
||||
buffer->lock();
|
||||
// robot::log_error_throttle(1.0, "depth data size 1: %d", (int)message.depth.data.size());
|
||||
buffer->bufferDepthCamera(message);
|
||||
buffer->unlock();
|
||||
}
|
||||
|
||||
void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
|
||||
double* min_y, double* max_x, double* max_y)
|
||||
{
|
||||
@@ -550,6 +677,23 @@ bool ObstacleLayer::getClearingObservations(std::vector<Observation>& clearing_o
|
||||
return current;
|
||||
}
|
||||
|
||||
bool ObstacleLayer::getFrustumClearingObservations(std::vector<DepthCameraObservation>& frustum_clearing_observations) const
|
||||
{
|
||||
bool current = true;
|
||||
// DepthCameraObservation depth_obs;
|
||||
|
||||
for (const boost::shared_ptr<ObservationBuffer>& buffer : depth_observation_buffers_)
|
||||
{
|
||||
buffer->lock();
|
||||
buffer->getDepthObservations(frustum_clearing_observations);
|
||||
current = buffer->isCurrent() && current;
|
||||
buffer->unlock();
|
||||
// frustum_clearing_observations.push_back(depth_obs);
|
||||
}
|
||||
|
||||
return current;
|
||||
}
|
||||
|
||||
void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y,
|
||||
double* max_x, double* max_y)
|
||||
{
|
||||
|
||||
@@ -37,7 +37,14 @@
|
||||
*********************************************************************/
|
||||
#include <robot_costmap_2d/voxel_layer.h>
|
||||
#include <robot_sensor_msgs/point_cloud2_iterator.h>
|
||||
#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
|
||||
#include <robot_geometry_msgs/Vector3.h>
|
||||
#include <tf3/exceptions.h>
|
||||
#include <boost/dll/alias.hpp>
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <cstring>
|
||||
|
||||
#define VOXEL_BITS 16
|
||||
|
||||
@@ -92,7 +99,7 @@ bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandl
|
||||
mark_threshold_ = loadParam(layer, "mark_threshold", 0);
|
||||
combination_method_ = loadParam(layer, "combination_method", 0.0);
|
||||
|
||||
int size_z, unknown_threshold, mark_threshold;
|
||||
int size_z, unknown_threshold, mark_threshold, frustum_pixel_step;
|
||||
if (nh.hasParam("enabled"))
|
||||
nh.getParam("enabled", enabled_);
|
||||
if (nh.hasParam("footprint_clearing_enabled"))
|
||||
@@ -165,6 +172,7 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
|
||||
|
||||
bool current = true;
|
||||
std::vector<Observation> observations, clearing_observations;
|
||||
std::vector<DepthCameraObservation> depth_observations;
|
||||
|
||||
// get the marking observations
|
||||
current = getMarkingObservations(observations) && current;
|
||||
@@ -172,9 +180,16 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
|
||||
// get the clearing observations
|
||||
current = getClearingObservations(clearing_observations) && current;
|
||||
|
||||
current = getFrustumClearingObservations(depth_observations) && current;
|
||||
|
||||
// update the global current status
|
||||
current_ = current;
|
||||
|
||||
for (const DepthCameraObservation& depth_observation : depth_observations)
|
||||
{
|
||||
raytraceDepthFrustum(depth_observation, min_x, min_y, max_x, max_y);
|
||||
}
|
||||
|
||||
// raytrace freespace
|
||||
for (unsigned int i = 0; i < clearing_observations.size(); ++i)
|
||||
{
|
||||
@@ -231,29 +246,6 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// if (publish_voxel_)
|
||||
// {
|
||||
// robot_costmap_2d::VoxelGrid grid_msg;
|
||||
// unsigned int size = robot_voxel_grid_.sizeX() * robot_voxel_grid_.sizeY();
|
||||
// grid_msg.size_x = robot_voxel_grid_.sizeX();
|
||||
// grid_msg.size_y = robot_voxel_grid_.sizeY();
|
||||
// grid_msg.size_z = robot_voxel_grid_.sizeZ();
|
||||
// grid_msg.data.resize(size);
|
||||
// memcpy(&grid_msg.data[0], robot_voxel_grid_.getData(), size * sizeof(unsigned int));
|
||||
|
||||
// grid_msg.origin.x = origin_x_;
|
||||
// grid_msg.origin.y = origin_y_;
|
||||
// grid_msg.origin.z = origin_z_;
|
||||
|
||||
// grid_msg.resolutions.x = resolution_;
|
||||
// grid_msg.resolutions.y = resolution_;
|
||||
// grid_msg.resolutions.z = z_resolution_;
|
||||
// grid_msg.header.frame_id = global_frame_;
|
||||
// grid_msg.header.stamp = robot::Time::now();
|
||||
// voxel_pub_.publish(grid_msg);
|
||||
// }
|
||||
|
||||
updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
|
||||
}
|
||||
|
||||
@@ -327,14 +319,6 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
|
||||
ox, oy, oz);
|
||||
return;
|
||||
}
|
||||
|
||||
// bool publish_clearing_points = (clearing_endpoints_pub_.getNumSubscribers() > 0);
|
||||
// if (publish_clearing_points)
|
||||
// {
|
||||
// clearing_endpoints_.points.clear();
|
||||
// clearing_endpoints_.points.reserve(clearing_observation_cloud_size);
|
||||
// }
|
||||
|
||||
// we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
|
||||
double map_end_x = origin_x_ + getSizeInMetersX();
|
||||
double map_end_y = origin_y_ + getSizeInMetersY();
|
||||
@@ -409,26 +393,249 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
|
||||
cell_raytrace_range);
|
||||
|
||||
updateRaytraceBounds(ox, oy, wpx, wpy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// if (publish_clearing_points)
|
||||
// {
|
||||
// robot_geometry_msgs::Point32 point;
|
||||
// point.x = wpx;
|
||||
// point.y = wpy;
|
||||
// point.z = wpz;
|
||||
// clearing_endpoints_.points.push_back(point);
|
||||
// }
|
||||
bool VoxelLayer::readDepthMeters(const robot_sensor_msgs::Image& depth, unsigned int u, unsigned int v,
|
||||
double& depth_m, bool& is_valid) const
|
||||
{
|
||||
depth_m = 0.0;
|
||||
is_valid = false;
|
||||
|
||||
if (u >= depth.width || v >= depth.height)
|
||||
return false;
|
||||
|
||||
if (depth.encoding == "16UC1" || depth.encoding == "mono16")
|
||||
{
|
||||
const std::size_t offset = static_cast<std::size_t>(v) * depth.step + static_cast<std::size_t>(u) * 2;
|
||||
if (offset + sizeof(std::uint16_t) > depth.data.size())
|
||||
return false;
|
||||
|
||||
std::uint16_t raw = 0;
|
||||
if (depth.is_bigendian)
|
||||
raw = static_cast<std::uint16_t>((depth.data[offset] << 8) | depth.data[offset + 1]);
|
||||
else
|
||||
raw = static_cast<std::uint16_t>(depth.data[offset] | (depth.data[offset + 1] << 8));
|
||||
|
||||
if (raw == 0)
|
||||
return true;
|
||||
|
||||
depth_m = static_cast<double>(raw) * 0.001;
|
||||
is_valid = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
if (depth.encoding == "32FC1")
|
||||
{
|
||||
const std::size_t offset = static_cast<std::size_t>(v) * depth.step + static_cast<std::size_t>(u) * 4;
|
||||
if (offset + sizeof(float) > depth.data.size())
|
||||
return false;
|
||||
|
||||
float raw = 0.0f;
|
||||
if (depth.is_bigendian)
|
||||
{
|
||||
unsigned char bytes[sizeof(float)] = {
|
||||
depth.data[offset + 3], depth.data[offset + 2], depth.data[offset + 1], depth.data[offset]};
|
||||
std::memcpy(&raw, bytes, sizeof(float));
|
||||
}
|
||||
else
|
||||
{
|
||||
std::memcpy(&raw, &depth.data[offset], sizeof(float));
|
||||
}
|
||||
|
||||
if (!std::isfinite(raw) || raw <= 0.0f)
|
||||
return true;
|
||||
|
||||
depth_m = static_cast<double>(raw);
|
||||
is_valid = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
robot::log_error("VoxelLayer unsupported depth encoding for frustum clearing: %s\n", depth.encoding.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
bool VoxelLayer::clipRaytraceEndpoint(double ox, double oy, double oz, double& wx, double& wy, double& wz)
|
||||
{
|
||||
double a = wx - ox;
|
||||
double b = wy - oy;
|
||||
double c = wz - oz;
|
||||
double t = 1.0;
|
||||
constexpr double kEpsilon = 1e-9;
|
||||
|
||||
if (std::fabs(a) < kEpsilon && std::fabs(b) < kEpsilon && std::fabs(c) < kEpsilon)
|
||||
return false;
|
||||
|
||||
if (wz > max_obstacle_height_ && std::fabs(c) > kEpsilon)
|
||||
t = std::max(0.0, std::min(t, (max_obstacle_height_ - 0.01 - oz) / c));
|
||||
else if (wz < origin_z_ && std::fabs(c) > kEpsilon)
|
||||
t = std::min(t, (origin_z_ - oz) / c);
|
||||
|
||||
const double map_end_x = origin_x_ + getSizeInMetersX();
|
||||
const double map_end_y = origin_y_ + getSizeInMetersY();
|
||||
|
||||
if (wx < origin_x_ && std::fabs(a) > kEpsilon)
|
||||
t = std::min(t, (origin_x_ - ox) / a);
|
||||
if (wy < origin_y_ && std::fabs(b) > kEpsilon)
|
||||
t = std::min(t, (origin_y_ - oy) / b);
|
||||
if (wx > map_end_x && std::fabs(a) > kEpsilon)
|
||||
t = std::min(t, (map_end_x - ox) / a);
|
||||
if (wy > map_end_y && std::fabs(b) > kEpsilon)
|
||||
t = std::min(t, (map_end_y - oy) / b);
|
||||
|
||||
if (!std::isfinite(t) || t <= 0.0)
|
||||
return false;
|
||||
|
||||
wx = ox + a * t;
|
||||
wy = oy + b * t;
|
||||
wz = oz + c * t;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool VoxelLayer::clearVoxelRay(double ox, double oy, double oz, double wx, double wy, double wz,
|
||||
double raytrace_range, double* min_x, double* min_y, double* max_x, double* max_y)
|
||||
{
|
||||
double sensor_x, sensor_y, sensor_z;
|
||||
if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z))
|
||||
return false;
|
||||
|
||||
if (!clipRaytraceEndpoint(ox, oy, oz, wx, wy, wz))
|
||||
return false;
|
||||
|
||||
double point_x, point_y, point_z;
|
||||
if (!worldToMap3DFloat(wx, wy, wz, point_x, point_y, point_z))
|
||||
return false;
|
||||
|
||||
robot_voxel_grid_.clearVoxelLineInMap(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_,
|
||||
unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION,
|
||||
cellDistance(raytrace_range));
|
||||
updateRaytraceBounds(ox, oy, wx, wy, raytrace_range, min_x, min_y, max_x, max_y);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
|
||||
double* min_x, double* min_y, double* max_x, double* max_y)
|
||||
{
|
||||
if (!observation.data_)
|
||||
return false;
|
||||
|
||||
const robot_sensor_msgs::DepthCameraData& depth_camera_data = *observation.data_;
|
||||
const robot_sensor_msgs::Image& depth = depth_camera_data.depth;
|
||||
const robot_sensor_msgs::CameraInfo& camera_info = depth_camera_data.camera_info;
|
||||
|
||||
if (depth.width == 0 || depth.height == 0 || depth.data.empty())
|
||||
return false;
|
||||
|
||||
const double fx = camera_info.K[0];
|
||||
const double fy = camera_info.K[4];
|
||||
const double cx = camera_info.K[2];
|
||||
const double cy = camera_info.K[5];
|
||||
if (fx <= 0.0 || fy <= 0.0)
|
||||
return false;
|
||||
|
||||
std::string depth_frame = depth.header.frame_id.empty() ? depth_camera_data.header.frame_id : depth.header.frame_id;
|
||||
if (depth_frame.empty())
|
||||
depth_frame = camera_info.header.frame_id;
|
||||
if (depth_frame.empty() || tf_ == nullptr)
|
||||
return false;
|
||||
|
||||
robot_geometry_msgs::PointStamped local_origin;
|
||||
local_origin.header = depth.header;
|
||||
local_origin.header.frame_id = depth_frame;
|
||||
if (local_origin.header.stamp.isZero())
|
||||
local_origin.header.stamp = depth_camera_data.header.stamp;
|
||||
local_origin.point.x = 0.0;
|
||||
local_origin.point.y = 0.0;
|
||||
local_origin.point.z = 0.0;
|
||||
|
||||
robot_geometry_msgs::PointStamped global_origin;
|
||||
tf3::TransformStampedMsg tfm;
|
||||
try
|
||||
{
|
||||
tfm = tf_->lookupTransform(global_frame_, depth_frame, tf3::Time());
|
||||
tf3::doTransform(local_origin, global_origin, tfm);
|
||||
}
|
||||
catch (tf3::TransformException& ex)
|
||||
{
|
||||
robot::log_error_throttle(
|
||||
5.0, "VoxelLayer depth topic [%s] TF exception from %s to %s: %s\n",
|
||||
observation.topic_.c_str(), depth_frame.c_str(), global_frame_.c_str(), ex.what());
|
||||
return false;
|
||||
}
|
||||
|
||||
const double ox = global_origin.point.x;
|
||||
const double oy = global_origin.point.y;
|
||||
const double oz = global_origin.point.z;
|
||||
|
||||
double sensor_x, sensor_y, sensor_z;
|
||||
if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z))
|
||||
{
|
||||
robot::log_error_throttle(
|
||||
5.0, "VoxelLayer depth topic [%s] origin at (%.2f, %.2f, %.2f) is outside the voxel map\n",
|
||||
observation.topic_.c_str(), ox, oy, oz);
|
||||
return false;
|
||||
}
|
||||
|
||||
const unsigned int step = std::max(1u, observation.pixel_step_);
|
||||
const double min_range = observation.min_range_;
|
||||
const double max_range = observation.max_range_;
|
||||
const double skip_dist = 2.0 * resolution_;
|
||||
const unsigned int width = std::min(depth.width, camera_info.width == 0 ? depth.width : camera_info.width);
|
||||
const unsigned int height = std::min(depth.height, camera_info.height == 0 ? depth.height : camera_info.height);
|
||||
bool cleared_any = false;
|
||||
|
||||
for (unsigned int v = 0; v < height; v += step)
|
||||
{
|
||||
for (unsigned int u = 0; u < width; u += step)
|
||||
{
|
||||
double depth_m = 0.0;
|
||||
bool valid = false;
|
||||
if (!readDepthMeters(depth, u, v, depth_m, valid))
|
||||
continue;
|
||||
|
||||
double ray_len = max_range;
|
||||
if (valid && depth_m < max_range)
|
||||
ray_len = std::max(0.0, depth_m - skip_dist);
|
||||
|
||||
if (ray_len <= min_range)
|
||||
continue;
|
||||
|
||||
double dx = (static_cast<double>(u) - cx) / fx;
|
||||
double dy = (static_cast<double>(v) - cy) / fy;
|
||||
double dz = 1.0;
|
||||
const double norm = std::sqrt(dx * dx + dy * dy + dz * dz);
|
||||
if (norm <= 0.0)
|
||||
continue;
|
||||
|
||||
robot_geometry_msgs::Vector3 local_ray;
|
||||
local_ray.x = dx / norm;
|
||||
local_ray.y = dy / norm;
|
||||
local_ray.z = dz / norm;
|
||||
|
||||
robot_geometry_msgs::Vector3 global_ray;
|
||||
tf3::doTransform(local_ray, global_ray, tfm);
|
||||
const double global_norm =
|
||||
std::sqrt(global_ray.x * global_ray.x + global_ray.y * global_ray.y + global_ray.z * global_ray.z);
|
||||
if (global_norm <= 0.0)
|
||||
continue;
|
||||
|
||||
global_ray.x /= global_norm;
|
||||
global_ray.y /= global_norm;
|
||||
global_ray.z /= global_norm;
|
||||
|
||||
const double sx = ox + global_ray.x * min_range;
|
||||
const double sy = oy + global_ray.y * min_range;
|
||||
const double sz = oz + global_ray.z * min_range;
|
||||
const double wx = ox + global_ray.x * ray_len;
|
||||
const double wy = oy + global_ray.y * ray_len;
|
||||
const double wz = oz + global_ray.z * ray_len;
|
||||
|
||||
cleared_any = clearVoxelRay(sx, sy, sz, wx, wy, wz, ray_len, min_x, min_y, max_x, max_y) || cleared_any;
|
||||
}
|
||||
}
|
||||
|
||||
// if (publish_clearing_points)
|
||||
// {
|
||||
// clearing_endpoints_.header.frame_id = global_frame_;
|
||||
// clearing_endpoints_.header.stamp = clearing_observation.cloud_->header.stamp;
|
||||
// clearing_endpoints_.header.seq = clearing_observation.cloud_->header.seq;
|
||||
|
||||
// clearing_endpoints_pub_.publish(clearing_endpoints_);
|
||||
// }
|
||||
return cleared_any;
|
||||
}
|
||||
|
||||
void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
|
||||
|
||||
Reference in New Issue
Block a user