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)
|
||||
{
|
||||
|
||||
Reference in New Issue
Block a user