update obstacle_layer
This commit is contained in:
@@ -77,118 +77,145 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
|
||||
try {
|
||||
std::string folder = ROBOT_COSTMAP_2D_DIR;
|
||||
std::string path_source = getSourceFile(folder,config_file_name);
|
||||
|
||||
|
||||
YAML::Node config = YAML::LoadFile(path_source);
|
||||
YAML::Node layer = config["obstacle_layer"];
|
||||
YAML::Node layer = config["obstacle_layer"];
|
||||
|
||||
bool track_unknown_space = loadParam(layer, "track_unknown_space", layered_costmap_->isTrackingUnknown());
|
||||
if (track_unknown_space)
|
||||
default_value_ = NO_INFORMATION;
|
||||
else
|
||||
default_value_ = FREE_SPACE;
|
||||
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);
|
||||
|
||||
double transform_tolerance = loadParam(layer,"transform_tolerance", 0.2);
|
||||
// // get the topics that we'll subscribe to from the parameter server
|
||||
// std::string topics_string = loadParam(layer,"observation_sources", std::string(""));
|
||||
// printf("Subscribed to Topics: %s\n", topics_string.c_str());
|
||||
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);
|
||||
|
||||
// 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;
|
||||
topic = loadParam(layer,"topic", topic);
|
||||
sensor_frame = loadParam(layer,"sensor_frame", std::string(""));
|
||||
observation_keep_time = loadParam(layer,"observation_persistence", 0.0);
|
||||
expected_update_rate = loadParam(layer,"expected_update_rate", 0.0);
|
||||
data_type = loadParam(layer,"data_type", std::string("PointCloud"));
|
||||
min_obstacle_height = loadParam(layer,"min_obstacle_height", 0.0);
|
||||
max_obstacle_height = loadParam(layer,"max_obstacle_height", 2.0);
|
||||
inf_is_valid = loadParam(layer,"inf_is_valid", false);
|
||||
clearing = loadParam(layer,"clearing", false);
|
||||
marking = loadParam(layer,"marking", true);
|
||||
if (track_unknown_space)
|
||||
default_value_ = NO_INFORMATION;
|
||||
else
|
||||
default_value_ = FREE_SPACE;
|
||||
|
||||
if (nh.hasParam("transform_tolerance"))
|
||||
nh.getParam("transform_tolerance", transform_tolerance);
|
||||
if (nh.hasParam("topic"))
|
||||
nh.getParam("topic", topic);
|
||||
if (nh.hasParam("sensor_frame"))
|
||||
nh.getParam("sensor_frame", sensor_frame);
|
||||
if (nh.hasParam("observation_persistence"))
|
||||
nh.getParam("observation_persistence", observation_keep_time);
|
||||
if (nh.hasParam("expected_update_rate"))
|
||||
nh.getParam("expected_update_rate", expected_update_rate);
|
||||
if (nh.hasParam("data_type"))
|
||||
nh.getParam("data_type", data_type);
|
||||
if (nh.hasParam("min_obstacle_height"))
|
||||
nh.getParam("min_obstacle_height", min_obstacle_height);
|
||||
if (nh.hasParam("max_obstacle_height"))
|
||||
nh.getParam("max_obstacle_height", max_obstacle_height);
|
||||
if (nh.hasParam("inf_is_valid"))
|
||||
nh.getParam("inf_is_valid", inf_is_valid);
|
||||
if (nh.hasParam("clearing"))
|
||||
nh.getParam("clearing", clearing);
|
||||
if (nh.hasParam("marking"))
|
||||
nh.getParam("marking", marking);
|
||||
footprint_clearing_enabled_ = footprint_clearing_enabled;
|
||||
max_obstacle_height_ = max_obstacle_height;
|
||||
combination_method_ = combination_method;
|
||||
|
||||
if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan"))
|
||||
{
|
||||
printf("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");
|
||||
// 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_error("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;
|
||||
|
||||
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);
|
||||
|
||||
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 (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan"))
|
||||
{
|
||||
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");
|
||||
}
|
||||
|
||||
CallBackInfo info_tmp;
|
||||
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;
|
||||
|
||||
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;
|
||||
|
||||
printf("Creating an observation buffer for topic %s, frame %s\n", topic.c_str(),
|
||||
sensor_frame.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());
|
||||
|
||||
printf(
|
||||
"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) {
|
||||
std::cerr << "Cannot open YAML file: " << e.what() << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
std::string raytrace_range_param_name, obstacle_range_param_name;
|
||||
|
||||
double obstacle_range = 2.5;
|
||||
obstacle_range = loadParam(layer,"obstacle_range", obstacle_range);
|
||||
double raytrace_range = 3.0;
|
||||
raytrace_range = loadParam(layer,"raytrace_range", raytrace_range);
|
||||
|
||||
bool footprint_clearing_enabled = loadParam(layer, "footprint_clearing_enabled", true);
|
||||
int combination_method = loadParam(layer, "combination_method", 1);
|
||||
|
||||
if (nh.hasParam("obstacle_range"))
|
||||
nh.getParam("obstacle_range", obstacle_range);
|
||||
if (nh.hasParam("raytrace_range"))
|
||||
nh.getParam("raytrace_range", raytrace_range);
|
||||
if (nh.hasParam("footprint_clearing_enabled"))
|
||||
nh.getParam("footprint_clearing_enabled", footprint_clearing_enabled);
|
||||
if (nh.hasParam("combination_method"))
|
||||
nh.getParam("combination_method", combination_method);
|
||||
|
||||
// enabled_ = enabled;
|
||||
footprint_clearing_enabled_ = footprint_clearing_enabled;
|
||||
max_obstacle_height_ = max_obstacle_height;
|
||||
combination_method_ = combination_method;
|
||||
|
||||
printf("Creating an observation buffer for topic %s, frame %s\n", topic.c_str(),
|
||||
sensor_frame.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());
|
||||
|
||||
printf(
|
||||
"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) {
|
||||
std::cerr << "Cannot open YAML file: " << e.what() << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
return true;
|
||||
}
|
||||
|
||||
void ObstacleLayer::handleImpl(const void* data,
|
||||
@@ -197,18 +224,52 @@ void ObstacleLayer::handleImpl(const void* data,
|
||||
{
|
||||
if(!stop_receiving_data_)
|
||||
{
|
||||
if(observation_buffers_.empty()) return;
|
||||
boost::shared_ptr<ObservationBuffer> buffer = observation_buffers_.back();
|
||||
if (type == typeid(robot_sensor_msgs::LaserScan) && topic == "laser") {
|
||||
|
||||
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)
|
||||
{
|
||||
laserScanCallback(*static_cast<const robot_sensor_msgs::LaserScan*>(data), buffer);
|
||||
} else if (type == typeid(robot_sensor_msgs::LaserScan) && topic == "laser_valid") {
|
||||
}
|
||||
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) && topic == "pcl_cb") {
|
||||
}
|
||||
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)
|
||||
{
|
||||
printf("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
|
||||
}
|
||||
pointCloudCallback(*static_cast<const robot_sensor_msgs::PointCloud*>(data), buffer);
|
||||
} else if (type == typeid(robot_sensor_msgs::PointCloud2) && topic == "pcl2_cb") {
|
||||
}
|
||||
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)
|
||||
{
|
||||
printf("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
|
||||
}
|
||||
pointCloud2Callback(*static_cast<const robot_sensor_msgs::PointCloud2*>(data), buffer);
|
||||
} else {
|
||||
std::cout << "[Plugin] Unknown type: " << type.name() << std::endl;
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cout << "obstacle_layer: Unknown type: " << type.name() << std::endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
|
||||
Reference in New Issue
Block a user