865 lines
34 KiB
C++
Executable File
865 lines
34 KiB
C++
Executable File
/*********************************************************************
|
|
*
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* Copyright (c) 2008, 2013, Willow Garage, Inc.
|
|
* All rights reserved.
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without
|
|
* modification, are permitted provided that the following conditions
|
|
* are met:
|
|
*
|
|
* * Redistributions of source code must retain the above copyright
|
|
* notice, this list of conditions and the following disclaimer.
|
|
* * Redistributions in binary form must reproduce the above
|
|
* copyright notice, this list of conditions and the following
|
|
* disclaimer in the documentation and/or other materials provided
|
|
* with the distribution.
|
|
* * Neither the name of Willow Garage, Inc. nor the names of its
|
|
* contributors may be used to endorse or promote products derived
|
|
* from this software without specific prior written permission.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
|
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
|
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
*
|
|
* Author: Eitan Marder-Eppstein
|
|
* David V. Lu!!
|
|
*********************************************************************/
|
|
#include <robot_costmap_2d/obstacle_layer.h>
|
|
#include <robot_costmap_2d/costmap_math.h>
|
|
|
|
#include <robot_sensor_msgs/point_cloud2_iterator.h>
|
|
|
|
#include <tf3/exceptions.h>
|
|
#include <boost/dll/alias.hpp>
|
|
|
|
|
|
using robot_costmap_2d::NO_INFORMATION;
|
|
using robot_costmap_2d::LETHAL_OBSTACLE;
|
|
using robot_costmap_2d::FREE_SPACE;
|
|
|
|
using robot_costmap_2d::ObservationBuffer;
|
|
using robot_costmap_2d::Observation;
|
|
|
|
namespace robot_costmap_2d
|
|
{
|
|
|
|
void ObstacleLayer::onInitialize()
|
|
{
|
|
robot::NodeHandle nh("~");
|
|
robot::NodeHandle priv_nh(nh, name_);
|
|
|
|
rolling_window_ = layered_costmap_->isRolling();
|
|
|
|
ObstacleLayer::matchSize();
|
|
current_ = true;
|
|
stop_receiving_data_ = false;
|
|
global_frame_ = layered_costmap_->getGlobalFrameID();
|
|
std::string config_file_name = "obstacle_layer_params.yaml";
|
|
getParams(config_file_name, priv_nh);
|
|
}
|
|
|
|
ObstacleLayer::~ObstacleLayer()
|
|
{}
|
|
|
|
bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh)
|
|
{
|
|
try {
|
|
const char *env_config = std::getenv("PNKX_NAV_CORE_CONFIG_DIR");
|
|
std::string folder;
|
|
if (env_config && std::filesystem::exists(env_config))
|
|
{
|
|
folder = std::string(env_config);
|
|
// robot::log_error("config_directory: %s", folder.c_str());
|
|
}
|
|
// robot::log_error("folder: %s", folder.c_str());
|
|
std::string path_source = getSourceFile(folder,config_file_name);
|
|
|
|
YAML::Node config = YAML::LoadFile(path_source);
|
|
YAML::Node layer = config["obstacle_layer"];
|
|
|
|
bool track_unknown_space = loadParam(layer, "track_unknown_space", layered_costmap_->isTrackingUnknown());
|
|
double transform_tolerance = loadParam(layer,"transform_tolerance", 0.2);
|
|
bool footprint_clearing_enabled = loadParam(layer, "footprint_clearing_enabled", true);
|
|
double max_obstacle_height = loadParam(layer,"max_obstacle_height", 2.0);
|
|
int combination_method = loadParam(layer, "combination_method", 1);
|
|
|
|
if (nh.hasParam("track_unknown_space"))
|
|
nh.getParam("track_unknown_space", track_unknown_space);
|
|
if (nh.hasParam("transform_tolerance"))
|
|
nh.getParam("transform_tolerance", transform_tolerance);
|
|
if (nh.hasParam("footprint_clearing_enabled"))
|
|
nh.getParam("footprint_clearing_enabled", footprint_clearing_enabled);
|
|
if (nh.hasParam("max_obstacle_height"))
|
|
nh.getParam("max_obstacle_height", max_obstacle_height);
|
|
if (nh.hasParam("combination_method"))
|
|
nh.getParam("combination_method", combination_method);
|
|
|
|
if (track_unknown_space)
|
|
default_value_ = NO_INFORMATION;
|
|
else
|
|
default_value_ = FREE_SPACE;
|
|
|
|
footprint_clearing_enabled_ = footprint_clearing_enabled;
|
|
max_obstacle_height_ = max_obstacle_height;
|
|
combination_method_ = combination_method;
|
|
|
|
// get the topics that we'll subscribe to from the parameter server
|
|
std::string topics_string = loadParam(layer,"observation_sources", std::string(""));
|
|
if (nh.hasParam("observation_sources"))
|
|
nh.getParam("observation_sources", topics_string);
|
|
robot::log_info("Subscribed to Topics: %s\n", topics_string.c_str());
|
|
|
|
// now we need to split the topics based on whitespace which we can use a stringstream for
|
|
std::stringstream ss(topics_string);
|
|
|
|
std::string source;
|
|
callback_infos_.clear();
|
|
while (ss >> source)
|
|
{
|
|
// get the parameters for the specific topic
|
|
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;
|
|
DepthFrustumConfig frustum_config;
|
|
|
|
robot::NodeHandle priv_nh(nh, source);
|
|
topic = loadParam(layer[source],"topic", topic);
|
|
sensor_frame = loadParam(layer[source],"sensor_frame", std::string(""));
|
|
observation_keep_time = loadParam(layer[source],"observation_persistence", 0.0);
|
|
expected_update_rate = loadParam(layer[source],"expected_update_rate", 0.0);
|
|
data_type = loadParam(layer[source],"data_type", std::string("PointCloud"));
|
|
min_obstacle_height = loadParam(layer[source],"min_obstacle_height", 0.0);
|
|
max_obstacle_height = loadParam(layer[source],"max_obstacle_height", 2.0);
|
|
inf_is_valid = loadParam(layer[source],"inf_is_valid", false);
|
|
clearing = loadParam(layer[source],"clearing", false);
|
|
marking = loadParam(layer[source],"marking", true);
|
|
// frustum params are per-source; the layer-level key is kept as a
|
|
// fallback for older YAMLs
|
|
frustum_clearing_enabled = loadParam(layer[source], "frustum_clearing_enabled",
|
|
loadParam(layer, "frustum_clearing_enabled", false));
|
|
frustum_pixel_step = loadParam(layer[source], "frustum_clearing_pixel_step",
|
|
loadParam(layer, "frustum_clearing_pixel_step", 8));
|
|
frustum_config.min_range = loadParam(layer[source], "frustum_min_range",
|
|
loadParam(layer, "frustum_min_range", 0.2));
|
|
frustum_config.max_range = loadParam(layer[source], "frustum_max_range",
|
|
loadParam(layer, "frustum_max_range", 3.0));
|
|
frustum_config.skip_distance =
|
|
loadParam(layer[source], "frustum_skip_distance", frustum_config.skip_distance);
|
|
frustum_config.column_clearing =
|
|
loadParam(layer[source], "frustum_column_clearing", frustum_config.column_clearing);
|
|
frustum_config.column_min_height =
|
|
loadParam(layer[source], "column_clear_min_height", frustum_config.column_min_height);
|
|
frustum_config.column_max_height =
|
|
loadParam(layer[source], "column_clear_max_height", frustum_config.column_max_height);
|
|
frustum_config.column_skip_distance =
|
|
loadParam(layer[source], "column_skip_distance", frustum_config.column_skip_distance);
|
|
frustum_config.column_cover_distance =
|
|
loadParam(layer[source], "column_cover_distance", frustum_config.column_cover_distance);
|
|
|
|
if (priv_nh.hasParam("topic"))
|
|
priv_nh.getParam("topic", topic);
|
|
if (priv_nh.hasParam("sensor_frame"))
|
|
priv_nh.getParam("sensor_frame", sensor_frame);
|
|
if (priv_nh.hasParam("observation_persistence"))
|
|
priv_nh.getParam("observation_persistence", observation_keep_time);
|
|
if (priv_nh.hasParam("expected_update_rate"))
|
|
priv_nh.getParam("expected_update_rate", expected_update_rate);
|
|
if (priv_nh.hasParam("data_type"))
|
|
priv_nh.getParam("data_type", data_type);
|
|
if (priv_nh.hasParam("min_obstacle_height"))
|
|
priv_nh.getParam("min_obstacle_height", min_obstacle_height);
|
|
if (priv_nh.hasParam("max_obstacle_height"))
|
|
priv_nh.getParam("max_obstacle_height", max_obstacle_height);
|
|
if (priv_nh.hasParam("inf_is_valid"))
|
|
priv_nh.getParam("inf_is_valid", inf_is_valid);
|
|
if (priv_nh.hasParam("clearing"))
|
|
priv_nh.getParam("clearing", clearing);
|
|
if (priv_nh.hasParam("marking"))
|
|
priv_nh.getParam("marking", marking);
|
|
if (priv_nh.hasParam("frustum_clearing_enabled"))
|
|
priv_nh.getParam("frustum_clearing_enabled", frustum_clearing_enabled);
|
|
if (priv_nh.hasParam("frustum_clearing_pixel_step"))
|
|
{
|
|
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_config.min_range);
|
|
if (priv_nh.hasParam("frustum_max_range"))
|
|
priv_nh.getParam("frustum_max_range", frustum_config.max_range);
|
|
if (priv_nh.hasParam("frustum_skip_distance"))
|
|
priv_nh.getParam("frustum_skip_distance", frustum_config.skip_distance);
|
|
if (priv_nh.hasParam("frustum_column_clearing"))
|
|
priv_nh.getParam("frustum_column_clearing", frustum_config.column_clearing);
|
|
if (priv_nh.hasParam("column_clear_min_height"))
|
|
priv_nh.getParam("column_clear_min_height", frustum_config.column_min_height);
|
|
if (priv_nh.hasParam("column_clear_max_height"))
|
|
priv_nh.getParam("column_clear_max_height", frustum_config.column_max_height);
|
|
if (priv_nh.hasParam("column_skip_distance"))
|
|
priv_nh.getParam("column_skip_distance", frustum_config.column_skip_distance);
|
|
if (priv_nh.hasParam("column_cover_distance"))
|
|
priv_nh.getParam("column_cover_distance", frustum_config.column_cover_distance);
|
|
if (priv_nh.hasParam("frustum_depth_camera_topic"))
|
|
priv_nh.getParam("frustum_depth_camera_topic", depth_camera_data_topic_);
|
|
|
|
frustum_config.pixel_step = static_cast<unsigned int>(std::max(1, frustum_pixel_step));
|
|
|
|
robot::log_info("source %s: frustum_clearing_enabled: %s, pixel_step: %u, range: [%.2f, %.2f] m, "
|
|
"skip: %.3f m, column_clearing: %s, column_band: [%.2f, %.2f] m, "
|
|
"column_skip: %.3f m, column_cover: %.2f m\n",
|
|
source.c_str(), frustum_clearing_enabled ? "true" : "false",
|
|
frustum_config.pixel_step, frustum_config.min_range, frustum_config.max_range,
|
|
frustum_config.skip_distance, frustum_config.column_clearing ? "true" : "false",
|
|
frustum_config.column_min_height, frustum_config.column_max_height,
|
|
frustum_config.column_skip_distance, frustum_config.column_cover_distance);
|
|
|
|
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);
|
|
|
|
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");
|
|
}
|
|
|
|
if(!frustum_clearing_enabled)
|
|
{
|
|
|
|
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);
|
|
|
|
// 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_config,
|
|
*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",
|
|
topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time);
|
|
|
|
}
|
|
}
|
|
catch (const YAML::BadFile& e) {
|
|
robot::log_error("Cannot open YAML file: %s\n", e.what());
|
|
return false;
|
|
}
|
|
|
|
|
|
return true;
|
|
}
|
|
|
|
void ObstacleLayer::handleImpl(const void* data,
|
|
const std::type_info& type,
|
|
const std::string& topic)
|
|
{
|
|
if (!enabled_ || stop_receiving_data_)
|
|
return;
|
|
|
|
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_ptr, buffer);
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (observation_buffers_.empty() || callback_infos_.empty())
|
|
return;
|
|
|
|
int size_callback = static_cast<int>(callback_infos_.size());
|
|
for (int i = 0; i < size_callback; i++)
|
|
{
|
|
boost::shared_ptr<ObservationBuffer>& buffer = observation_buffers_[i];
|
|
|
|
if (type == typeid(robot_sensor_msgs::LaserScan) &&
|
|
callback_infos_[i].data_type == "LaserScan" &&
|
|
topic == callback_infos_[i].topic &&
|
|
!callback_infos_[i].inf_is_valid)
|
|
{
|
|
// if(topic == "/f_scan") robot::log_error("DATA front laser! %d",i);
|
|
// if(topic == "/b_scan") robot::log_error("DATA back laser! %d",i);
|
|
laserScanCallback(*static_cast<const robot_sensor_msgs::LaserScan*>(data), buffer);
|
|
}
|
|
else if (type == typeid(robot_sensor_msgs::LaserScan) &&
|
|
callback_infos_[i].data_type == "LaserScan" &&
|
|
topic == callback_infos_[i].topic &&
|
|
callback_infos_[i].inf_is_valid)
|
|
{
|
|
|
|
laserScanValidInfCallback(*static_cast<const robot_sensor_msgs::LaserScan*>(data), buffer);
|
|
}
|
|
else if (type == typeid(robot_sensor_msgs::PointCloud) &&
|
|
callback_infos_[i].data_type == "PointCloud" &&
|
|
topic == callback_infos_[i].topic)
|
|
{
|
|
if (callback_infos_[i].inf_is_valid)
|
|
{
|
|
robot::log_error("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.\n");
|
|
}
|
|
pointCloudCallback(*static_cast<const robot_sensor_msgs::PointCloud*>(data), buffer);
|
|
}
|
|
else if (type == typeid(robot_sensor_msgs::PointCloud2) &&
|
|
callback_infos_[i].data_type == "PointCloud2" &&
|
|
topic == callback_infos_[i].topic)
|
|
{
|
|
if (callback_infos_[i].inf_is_valid)
|
|
{
|
|
robot::log_error("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.\n");
|
|
}
|
|
pointCloud2Callback(*static_cast<const robot_sensor_msgs::PointCloud2*>(data), buffer);
|
|
}
|
|
// else
|
|
// {
|
|
// std::cout << "obstacle_layer: check type: " << (type == typeid(robot_sensor_msgs::LaserScan)) << std::endl
|
|
// << "obstacle_layer: inf_is_valid: " << callback_infos_[i].inf_is_valid << std::endl
|
|
// << "data type: " << callback_infos_[i].data_type << std::endl
|
|
// << "topic: " << topic << std::endl
|
|
// << "topic check: " << callback_infos_[i].topic << std::endl << std::endl;
|
|
// }
|
|
}
|
|
}
|
|
}
|
|
|
|
void ObstacleLayer::laserScanCallback(const robot_sensor_msgs::LaserScan& message,
|
|
const boost::shared_ptr<ObservationBuffer>& buffer)
|
|
{
|
|
// project the laser into a point cloud
|
|
robot_sensor_msgs::PointCloud2 cloud;
|
|
cloud.header = message.header;
|
|
|
|
// project the scan into a point cloud
|
|
try
|
|
{
|
|
projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, *tf_);
|
|
}
|
|
catch (tf3::TransformException &ex)
|
|
{
|
|
robot::log_error("High fidelity enabled, but TF returned a transform exception to frame %s: %s\n", global_frame_.c_str(),
|
|
ex.what());
|
|
projector_.projectLaser(message, cloud);
|
|
}
|
|
catch (std::runtime_error &ex)
|
|
{
|
|
robot::log_error("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s\n", ex.what());
|
|
return; //ignore this message
|
|
}
|
|
|
|
// buffer the point cloud
|
|
buffer->lock();
|
|
buffer->bufferCloud(cloud);
|
|
buffer->unlock();
|
|
}
|
|
|
|
void ObstacleLayer::laserScanValidInfCallback(const robot_sensor_msgs::LaserScan& raw_message,
|
|
const boost::shared_ptr<ObservationBuffer>& buffer)
|
|
{
|
|
// Filter positive infinities ("Inf"s) to max_range.
|
|
float epsilon = 0.0001; // a tenth of a millimeter
|
|
robot_sensor_msgs::LaserScan message = raw_message;
|
|
for (size_t i = 0; i < message.ranges.size(); i++)
|
|
{
|
|
float range = message.ranges[ i ];
|
|
if (!std::isfinite(range) && range > 0)
|
|
{
|
|
message.ranges[ i ] = message.range_max - epsilon;
|
|
}
|
|
}
|
|
|
|
// project the laser into a point cloud
|
|
robot_sensor_msgs::PointCloud2 cloud;
|
|
cloud.header = message.header;
|
|
|
|
// project the scan into a point cloud
|
|
try
|
|
{
|
|
projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, *tf_);
|
|
}
|
|
catch (tf3::TransformException &ex)
|
|
{
|
|
robot::log_error("High fidelity enabled, but TF returned a transform exception to frame %s: %s\n",
|
|
global_frame_.c_str(), ex.what());
|
|
projector_.projectLaser(message, cloud);
|
|
}
|
|
catch (std::runtime_error &ex)
|
|
{
|
|
robot::log_error("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s\n", ex.what());
|
|
return; //ignore this message
|
|
}
|
|
|
|
// buffer the point cloud
|
|
buffer->lock();
|
|
buffer->bufferCloud(cloud);
|
|
buffer->unlock();
|
|
}
|
|
|
|
void ObstacleLayer::pointCloudCallback(const robot_sensor_msgs::PointCloud& message,
|
|
const boost::shared_ptr<ObservationBuffer>& buffer)
|
|
{
|
|
robot_sensor_msgs::PointCloud2 cloud2;
|
|
|
|
if (!robot_sensor_msgs::convertPointCloudToPointCloud2(message, cloud2))
|
|
{
|
|
robot::log_error("Failed to convert a PointCloud to a PointCloud2, dropping message\n");
|
|
return;
|
|
}
|
|
|
|
// buffer the point cloud
|
|
buffer->lock();
|
|
buffer->bufferCloud(cloud2);
|
|
buffer->unlock();
|
|
}
|
|
|
|
void ObstacleLayer::pointCloud2Callback(const robot_sensor_msgs::PointCloud2& message,
|
|
const boost::shared_ptr<ObservationBuffer>& buffer)
|
|
{
|
|
// buffer the point cloud
|
|
buffer->lock();
|
|
buffer->bufferCloud(message);
|
|
buffer->unlock();
|
|
}
|
|
|
|
void ObstacleLayer::depthImageCallback(robot_sensor_msgs::DepthCameraData::ConstPtr message,
|
|
const boost::shared_ptr<ObservationBuffer>& buffer)
|
|
{
|
|
buffer->lock();
|
|
buffer->bufferDepthCamera(std::move(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)
|
|
{
|
|
if (rolling_window_)
|
|
updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
|
|
useExtraBounds(min_x, min_y, max_x, max_y);
|
|
|
|
bool current = true;
|
|
std::vector<Observation> observations, clearing_observations;
|
|
|
|
// get the marking observations
|
|
current = current && getMarkingObservations(observations);
|
|
|
|
// get the clearing observations
|
|
current = current && getClearingObservations(clearing_observations);
|
|
|
|
// update the global current status
|
|
current_ = current;
|
|
|
|
// raytrace freespace
|
|
for (unsigned int i = 0; i < clearing_observations.size(); ++i)
|
|
{
|
|
raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
|
|
}
|
|
|
|
// place the new obstacles into a priority queue... each with a priority of zero to begin with
|
|
for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it)
|
|
{
|
|
const Observation& obs = *it;
|
|
|
|
const robot_sensor_msgs::PointCloud2& cloud = *(obs.cloud_);
|
|
|
|
double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;
|
|
|
|
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
|
|
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
|
|
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
|
|
|
|
std::size_t rejected_height = 0;
|
|
std::size_t rejected_range = 0;
|
|
std::size_t rejected_bounds = 0;
|
|
for (; iter_x !=iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
|
|
{
|
|
double px = *iter_x, py = *iter_y, pz = *iter_z;
|
|
|
|
// if the obstacle is too high or too far away from the robot we won't add it
|
|
if (pz > max_obstacle_height_)
|
|
{
|
|
++rejected_height;
|
|
continue;
|
|
}
|
|
|
|
// compute the squared distance from the hitpoint to the pointcloud's origin
|
|
double sq_dist = (px - obs.origin_.x) * (px - obs.origin_.x) + (py - obs.origin_.y) * (py - obs.origin_.y)
|
|
+ (pz - obs.origin_.z) * (pz - obs.origin_.z);
|
|
|
|
// if the point is far enough away... we won't consider it
|
|
if (sq_dist >= sq_obstacle_range)
|
|
{
|
|
++rejected_range;
|
|
continue;
|
|
}
|
|
|
|
// now we need to compute the map coordinates for the observation
|
|
unsigned int mx, my;
|
|
if (!worldToMap(px, py, mx, my))
|
|
{
|
|
++rejected_bounds;
|
|
continue;
|
|
}
|
|
|
|
unsigned int index = getIndex(mx, my);
|
|
costmap_[index] = LETHAL_OBSTACLE;
|
|
touch(px, py, min_x, min_y, max_x, max_y);
|
|
}
|
|
|
|
if (rejected_height + rejected_range + rejected_bounds > 0)
|
|
{
|
|
robot::log_info_throttle(
|
|
5.0,
|
|
"ObstacleLayer filtered points: height=%zu range=%zu outside_map=%zu\n",
|
|
rejected_height, rejected_range, rejected_bounds);
|
|
}
|
|
}
|
|
|
|
updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
|
|
}
|
|
|
|
void ObstacleLayer::updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
|
|
double* max_x, double* max_y)
|
|
{
|
|
if (!footprint_clearing_enabled_) return;
|
|
transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_);
|
|
|
|
for (unsigned int i = 0; i < transformed_footprint_.size(); i++)
|
|
{
|
|
touch(transformed_footprint_[i].x, transformed_footprint_[i].y, min_x, min_y, max_x, max_y);
|
|
}
|
|
}
|
|
|
|
void ObstacleLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
|
|
{
|
|
if (footprint_clearing_enabled_)
|
|
{
|
|
setConvexPolygonCost(transformed_footprint_, robot_costmap_2d::FREE_SPACE);
|
|
}
|
|
|
|
switch (combination_method_)
|
|
{
|
|
case 0: // Overwrite
|
|
updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j);
|
|
break;
|
|
case 1: // Maximum
|
|
updateWithMax(master_grid, min_i, min_j, max_i, max_j);
|
|
break;
|
|
default: // Nothing
|
|
break;
|
|
}
|
|
|
|
}
|
|
|
|
void ObstacleLayer::addStaticObservation(robot_costmap_2d::Observation& obs, bool marking, bool clearing)
|
|
{
|
|
if (marking)
|
|
static_marking_observations_.push_back(obs);
|
|
if (clearing)
|
|
static_clearing_observations_.push_back(obs);
|
|
}
|
|
|
|
void ObstacleLayer::clearStaticObservations(bool marking, bool clearing)
|
|
{
|
|
if (marking)
|
|
static_marking_observations_.clear();
|
|
if (clearing)
|
|
static_clearing_observations_.clear();
|
|
}
|
|
|
|
bool ObstacleLayer::getMarkingObservations(std::vector<Observation>& marking_observations) const
|
|
{
|
|
bool current = true;
|
|
// get the marking observations
|
|
for (unsigned int i = 0; i < marking_buffers_.size(); ++i)
|
|
{
|
|
marking_buffers_[i]->lock();
|
|
marking_buffers_[i]->getObservations(marking_observations);
|
|
current = marking_buffers_[i]->isCurrent() && current;
|
|
marking_buffers_[i]->unlock();
|
|
}
|
|
marking_observations.insert(marking_observations.end(),
|
|
static_marking_observations_.begin(), static_marking_observations_.end());
|
|
return current;
|
|
}
|
|
|
|
bool ObstacleLayer::getClearingObservations(std::vector<Observation>& clearing_observations) const
|
|
{
|
|
bool current = true;
|
|
// get the clearing observations
|
|
for (unsigned int i = 0; i < clearing_buffers_.size(); ++i)
|
|
{
|
|
clearing_buffers_[i]->lock();
|
|
clearing_buffers_[i]->getObservations(clearing_observations);
|
|
current = clearing_buffers_[i]->isCurrent() && current;
|
|
clearing_buffers_[i]->unlock();
|
|
}
|
|
clearing_observations.insert(clearing_observations.end(),
|
|
static_clearing_observations_.begin(), static_clearing_observations_.end());
|
|
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)
|
|
{
|
|
double ox = clearing_observation.origin_.x;
|
|
double oy = clearing_observation.origin_.y;
|
|
const robot_sensor_msgs::PointCloud2 &cloud = *(clearing_observation.cloud_);
|
|
|
|
// get the map coordinates of the origin of the sensor
|
|
unsigned int x0, y0;
|
|
if (!worldToMap(ox, oy, x0, y0))
|
|
{
|
|
robot::log_error(
|
|
"The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.\n",
|
|
ox, oy);
|
|
return;
|
|
}
|
|
|
|
// we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
|
|
double origin_x = origin_x_, origin_y = origin_y_;
|
|
double map_end_x = origin_x + size_x_ * resolution_;
|
|
double map_end_y = origin_y + size_y_ * resolution_;
|
|
|
|
|
|
touch(ox, oy, min_x, min_y, max_x, max_y);
|
|
|
|
// for each point in the cloud, we want to trace a line from the origin and clear obstacles along it
|
|
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
|
|
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
|
|
|
|
for (; iter_x != iter_x.end(); ++iter_x, ++iter_y)
|
|
{
|
|
double wx = *iter_x;
|
|
double wy = *iter_y;
|
|
|
|
// now we also need to make sure that the enpoint we're raytracing
|
|
// to isn't off the costmap and scale if necessary
|
|
double a = wx - ox;
|
|
double b = wy - oy;
|
|
|
|
// the minimum value to raytrace from is the origin
|
|
if (wx < origin_x)
|
|
{
|
|
double t = (origin_x - ox) / a;
|
|
wx = origin_x;
|
|
wy = oy + b * t;
|
|
}
|
|
if (wy < origin_y)
|
|
{
|
|
double t = (origin_y - oy) / b;
|
|
wx = ox + a * t;
|
|
wy = origin_y;
|
|
}
|
|
|
|
// the maximum value to raytrace to is the end of the map
|
|
if (wx > map_end_x)
|
|
{
|
|
double t = (map_end_x - ox) / a;
|
|
wx = map_end_x - .001;
|
|
wy = oy + b * t;
|
|
}
|
|
if (wy > map_end_y)
|
|
{
|
|
double t = (map_end_y - oy) / b;
|
|
wx = ox + a * t;
|
|
wy = map_end_y - .001;
|
|
}
|
|
|
|
// now that the vector is scaled correctly... we'll get the map coordinates of its endpoint
|
|
unsigned int x1, y1;
|
|
|
|
// check for legality just in case
|
|
if (!worldToMap(wx, wy, x1, y1))
|
|
continue;
|
|
|
|
unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
|
|
MarkCell marker(costmap_, FREE_SPACE);
|
|
// and finally... we can execute our trace to clear obstacles along that line
|
|
raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range);
|
|
|
|
updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
|
|
}
|
|
}
|
|
|
|
void ObstacleLayer::activate()
|
|
{
|
|
stop_receiving_data_ = false;
|
|
for (unsigned int i = 0; i < observation_buffers_.size(); ++i)
|
|
{
|
|
if (observation_buffers_[i])
|
|
observation_buffers_[i]->resetLastUpdated();
|
|
}
|
|
}
|
|
void ObstacleLayer::deactivate()
|
|
{
|
|
stop_receiving_data_ = true;
|
|
}
|
|
|
|
void ObstacleLayer::updateRaytraceBounds(double ox, double oy, double wx, double wy, double range,
|
|
double* min_x, double* min_y, double* max_x, double* max_y)
|
|
{
|
|
double dx = wx-ox, dy = wy-oy;
|
|
double full_distance = hypot(dx, dy);
|
|
double scale = std::min(1.0, range / full_distance);
|
|
double ex = ox + dx * scale, ey = oy + dy * scale;
|
|
touch(ex, ey, min_x, min_y, max_x, max_y);
|
|
}
|
|
|
|
void ObstacleLayer::reset()
|
|
{
|
|
deactivate();
|
|
resetMaps();
|
|
current_ = true;
|
|
activate();
|
|
}
|
|
|
|
// Export factory function
|
|
static boost::shared_ptr<Layer> create_obstacle_plugin() {
|
|
return boost::make_shared<ObstacleLayer>();
|
|
}
|
|
|
|
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
|
|
BOOST_DLL_ALIAS(create_obstacle_plugin, ObstacleLayer)
|
|
|
|
} // namespace robot_costmap_2d
|