them file test static_layer
This commit is contained in:
@@ -1,185 +1,183 @@
|
||||
#include <costmap_2d/obstacle_layer.h>
|
||||
// #include <costmap_2d/costmap_math.h>
|
||||
#include <costmap_2d/costmap_math.h>
|
||||
// #include <tf2_ros/message_filter.h>
|
||||
#include <sensor_msgs/point_cloud2_iterator.h>
|
||||
#include <tf2/utils.h>
|
||||
#include <boost/dll/alias.hpp>
|
||||
|
||||
// #include <pluginlib/class_list_macros.hpp>
|
||||
// #include <sensor_msgs/point_cloud2_iterator.h>
|
||||
using costmap_2d::NO_INFORMATION;
|
||||
using costmap_2d::LETHAL_OBSTACLE;
|
||||
using costmap_2d::FREE_SPACE;
|
||||
|
||||
// PLUGINLIB_EXPORT_CLASS(costmap_2d::ObstacleLayer, costmap_2d::Layer)
|
||||
using costmap_2d::ObservationBuffer;
|
||||
using costmap_2d::Observation;
|
||||
|
||||
// using costmap_2d::NO_INFORMATION;
|
||||
// using costmap_2d::LETHAL_OBSTACLE;
|
||||
// using costmap_2d::FREE_SPACE;
|
||||
namespace costmap_2d
|
||||
{
|
||||
|
||||
// using costmap_2d::ObservationBuffer;
|
||||
// using costmap_2d::Observation;
|
||||
|
||||
// namespace costmap_2d
|
||||
// {
|
||||
|
||||
// void ObstacleLayer::onInitialize()
|
||||
// {
|
||||
void ObstacleLayer::onInitialize()
|
||||
{
|
||||
// ros::NodeHandle nh("~/" + name_), g_nh;
|
||||
// rolling_window_ = layered_costmap_->isRolling();
|
||||
rolling_window_ = layered_costmap_->isRolling();
|
||||
|
||||
// bool track_unknown_space;
|
||||
bool track_unknown_space;
|
||||
// nh.param("track_unknown_space", track_unknown_space, layered_costmap_->isTrackingUnknown());
|
||||
// if (track_unknown_space)
|
||||
// default_value_ = NO_INFORMATION;
|
||||
// else
|
||||
// default_value_ = FREE_SPACE;
|
||||
if (track_unknown_space)
|
||||
default_value_ = NO_INFORMATION;
|
||||
else
|
||||
default_value_ = FREE_SPACE;
|
||||
|
||||
// ObstacleLayer::matchSize();
|
||||
// current_ = true;
|
||||
ObstacleLayer::matchSize();
|
||||
current_ = true;
|
||||
|
||||
// global_frame_ = layered_costmap_->getGlobalFrameID();
|
||||
// double transform_tolerance;
|
||||
global_frame_ = layered_costmap_->getGlobalFrameID();
|
||||
double transform_tolerance;
|
||||
// nh.param("transform_tolerance", transform_tolerance, 0.2);
|
||||
|
||||
// std::string topics_string;
|
||||
// // get the topics that we'll subscribe to from the parameter server
|
||||
std::string topics_string;
|
||||
// get the topics that we'll subscribe to from the parameter server
|
||||
// nh.param("observation_sources", topics_string, std::string(""));
|
||||
// ROS_INFO(" Subscribed to Topics: %s", 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);
|
||||
// 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;
|
||||
while (ss >> source)
|
||||
{
|
||||
ros::NodeHandle source_node(nh, source);
|
||||
|
||||
// std::string source;
|
||||
// while (ss >> source)
|
||||
// {
|
||||
// ros::NodeHandle source_node(nh, source);
|
||||
// get the parameters for the specific topic
|
||||
double observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height;
|
||||
std::string topic, sensor_frame, data_type;
|
||||
bool inf_is_valid, clearing, marking;
|
||||
|
||||
// // get the parameters for the specific topic
|
||||
// double observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height;
|
||||
// std::string topic, sensor_frame, data_type;
|
||||
// bool inf_is_valid, clearing, marking;
|
||||
source_node.param("topic", topic, source);
|
||||
source_node.param("sensor_frame", sensor_frame, std::string(""));
|
||||
source_node.param("observation_persistence", observation_keep_time, 0.0);
|
||||
source_node.param("expected_update_rate", expected_update_rate, 0.0);
|
||||
source_node.param("data_type", data_type, std::string("PointCloud"));
|
||||
source_node.param("min_obstacle_height", min_obstacle_height, 0.0);
|
||||
source_node.param("max_obstacle_height", max_obstacle_height, 2.0);
|
||||
source_node.param("inf_is_valid", inf_is_valid, false);
|
||||
source_node.param("clearing", clearing, false);
|
||||
source_node.param("marking", marking, true);
|
||||
|
||||
// source_node.param("topic", topic, source);
|
||||
// source_node.param("sensor_frame", sensor_frame, std::string(""));
|
||||
// source_node.param("observation_persistence", observation_keep_time, 0.0);
|
||||
// source_node.param("expected_update_rate", expected_update_rate, 0.0);
|
||||
// source_node.param("data_type", data_type, std::string("PointCloud"));
|
||||
// source_node.param("min_obstacle_height", min_obstacle_height, 0.0);
|
||||
// source_node.param("max_obstacle_height", max_obstacle_height, 2.0);
|
||||
// source_node.param("inf_is_valid", inf_is_valid, false);
|
||||
// source_node.param("clearing", clearing, false);
|
||||
// source_node.param("marking", marking, true);
|
||||
if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan"))
|
||||
{
|
||||
ROS_FATAL("Only topics that use point clouds or laser scans are currently supported");
|
||||
throw std::runtime_error("Only topics that use point clouds or laser scans are currently supported");
|
||||
}
|
||||
|
||||
// if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan"))
|
||||
// {
|
||||
// ROS_FATAL("Only topics that use point clouds or laser scans are currently supported");
|
||||
// throw std::runtime_error("Only topics that use point clouds or laser scans are currently supported");
|
||||
// }
|
||||
std::string raytrace_range_param_name, obstacle_range_param_name;
|
||||
|
||||
// std::string raytrace_range_param_name, obstacle_range_param_name;
|
||||
// get the obstacle range for the sensor
|
||||
double obstacle_range = 2.5;
|
||||
if (source_node.searchParam("obstacle_range", obstacle_range_param_name))
|
||||
{
|
||||
source_node.getParam(obstacle_range_param_name, obstacle_range);
|
||||
}
|
||||
|
||||
// // get the obstacle range for the sensor
|
||||
// double obstacle_range = 2.5;
|
||||
// if (source_node.searchParam("obstacle_range", obstacle_range_param_name))
|
||||
// {
|
||||
// source_node.getParam(obstacle_range_param_name, obstacle_range);
|
||||
// }
|
||||
// get the raytrace range for the sensor
|
||||
double raytrace_range = 3.0;
|
||||
if (source_node.searchParam("raytrace_range", raytrace_range_param_name))
|
||||
{
|
||||
source_node.getParam(raytrace_range_param_name, raytrace_range);
|
||||
}
|
||||
|
||||
// // get the raytrace range for the sensor
|
||||
// double raytrace_range = 3.0;
|
||||
// if (source_node.searchParam("raytrace_range", raytrace_range_param_name))
|
||||
// {
|
||||
// source_node.getParam(raytrace_range_param_name, raytrace_range);
|
||||
// }
|
||||
printf("Creating an observation buffer for source %s, topic %s, frame %s", source.c_str(), topic.c_str(),
|
||||
sensor_frame.c_str());
|
||||
|
||||
// ROS_DEBUG("Creating an observation buffer for source %s, topic %s, frame %s", source.c_str(), 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)));
|
||||
|
||||
// // 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)));
|
||||
// check if we'll add this buffer to our marking observation buffers
|
||||
if (marking)
|
||||
marking_buffers_.push_back(observation_buffers_.back());
|
||||
|
||||
// // check if we'll add this buffer to our marking observation buffers
|
||||
// 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());
|
||||
|
||||
// // 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 source %s, topic %s, global frame: %s, "
|
||||
"expected update rate: %.2f, observation persistence: %.2f",
|
||||
source.c_str(), topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time);
|
||||
|
||||
// ROS_DEBUG(
|
||||
// "Created an observation buffer for source %s, topic %s, global frame: %s, "
|
||||
// "expected update rate: %.2f, observation persistence: %.2f",
|
||||
// source.c_str(), topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time);
|
||||
// create a callback for the topic
|
||||
if (data_type == "LaserScan")
|
||||
{
|
||||
boost::shared_ptr < message_filters::Subscriber<sensor_msgs::LaserScan>
|
||||
> sub(new message_filters::Subscriber<sensor_msgs::LaserScan>(g_nh, topic, 50));
|
||||
|
||||
// // create a callback for the topic
|
||||
// if (data_type == "LaserScan")
|
||||
// {
|
||||
// boost::shared_ptr < message_filters::Subscriber<sensor_msgs::LaserScan>
|
||||
// > sub(new message_filters::Subscriber<sensor_msgs::LaserScan>(g_nh, topic, 50));
|
||||
boost::shared_ptr<tf2_ros::MessageFilter<sensor_msgs::LaserScan> > filter(
|
||||
new tf2_ros::MessageFilter<sensor_msgs::LaserScan>(*sub, *tf_, global_frame_, 50, g_nh));
|
||||
|
||||
// boost::shared_ptr<tf2_ros::MessageFilter<sensor_msgs::LaserScan> > filter(
|
||||
// new tf2_ros::MessageFilter<sensor_msgs::LaserScan>(*sub, *tf_, global_frame_, 50, g_nh));
|
||||
if (inf_is_valid)
|
||||
{
|
||||
filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ laserScanValidInfCallback(msg, buffer); });
|
||||
}
|
||||
else
|
||||
{
|
||||
filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ laserScanCallback(msg, buffer); });
|
||||
}
|
||||
|
||||
// if (inf_is_valid)
|
||||
// {
|
||||
// filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ laserScanValidInfCallback(msg, buffer); });
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ laserScanCallback(msg, buffer); });
|
||||
// }
|
||||
observation_subscribers_.push_back(sub);
|
||||
observation_notifiers_.push_back(filter);
|
||||
|
||||
// observation_subscribers_.push_back(sub);
|
||||
// observation_notifiers_.push_back(filter);
|
||||
observation_notifiers_.back()->setTolerance(ros::Duration(0.05));
|
||||
}
|
||||
else if (data_type == "PointCloud")
|
||||
{
|
||||
boost::shared_ptr < message_filters::Subscriber<sensor_msgs::PointCloud>
|
||||
> sub(new message_filters::Subscriber<sensor_msgs::PointCloud>(g_nh, topic, 50));
|
||||
|
||||
// observation_notifiers_.back()->setTolerance(ros::Duration(0.05));
|
||||
// }
|
||||
// else if (data_type == "PointCloud")
|
||||
// {
|
||||
// boost::shared_ptr < message_filters::Subscriber<sensor_msgs::PointCloud>
|
||||
// > sub(new message_filters::Subscriber<sensor_msgs::PointCloud>(g_nh, topic, 50));
|
||||
if (inf_is_valid)
|
||||
{
|
||||
ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
|
||||
}
|
||||
|
||||
// if (inf_is_valid)
|
||||
// {
|
||||
// ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
|
||||
// }
|
||||
boost::shared_ptr < tf2_ros::MessageFilter<sensor_msgs::PointCloud>
|
||||
> filter(new tf2_ros::MessageFilter<sensor_msgs::PointCloud>(*sub, *tf_, global_frame_, 50, g_nh));
|
||||
filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ pointCloudCallback(msg, buffer); });
|
||||
|
||||
// boost::shared_ptr < tf2_ros::MessageFilter<sensor_msgs::PointCloud>
|
||||
// > filter(new tf2_ros::MessageFilter<sensor_msgs::PointCloud>(*sub, *tf_, global_frame_, 50, g_nh));
|
||||
// filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ pointCloudCallback(msg, buffer); });
|
||||
observation_subscribers_.push_back(sub);
|
||||
observation_notifiers_.push_back(filter);
|
||||
}
|
||||
else
|
||||
{
|
||||
boost::shared_ptr < message_filters::Subscriber<sensor_msgs::PointCloud2>
|
||||
> sub(new message_filters::Subscriber<sensor_msgs::PointCloud2>(g_nh, topic, 50));
|
||||
|
||||
// observation_subscribers_.push_back(sub);
|
||||
// observation_notifiers_.push_back(filter);
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// boost::shared_ptr < message_filters::Subscriber<sensor_msgs::PointCloud2>
|
||||
// > sub(new message_filters::Subscriber<sensor_msgs::PointCloud2>(g_nh, topic, 50));
|
||||
if (inf_is_valid)
|
||||
{
|
||||
ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
|
||||
}
|
||||
|
||||
// if (inf_is_valid)
|
||||
// {
|
||||
// ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
|
||||
// }
|
||||
boost::shared_ptr < tf2_ros::MessageFilter<sensor_msgs::PointCloud2>
|
||||
> filter(new tf2_ros::MessageFilter<sensor_msgs::PointCloud2>(*sub, *tf_, global_frame_, 50, g_nh));
|
||||
filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ pointCloud2Callback(msg, buffer); });
|
||||
|
||||
// boost::shared_ptr < tf2_ros::MessageFilter<sensor_msgs::PointCloud2>
|
||||
// > filter(new tf2_ros::MessageFilter<sensor_msgs::PointCloud2>(*sub, *tf_, global_frame_, 50, g_nh));
|
||||
// filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ pointCloud2Callback(msg, buffer); });
|
||||
|
||||
// observation_subscribers_.push_back(sub);
|
||||
// observation_notifiers_.push_back(filter);
|
||||
// }
|
||||
|
||||
// if (sensor_frame != "")
|
||||
// {
|
||||
// std::vector < std::string > target_frames;
|
||||
// target_frames.push_back(global_frame_);
|
||||
// target_frames.push_back(sensor_frame);
|
||||
// observation_notifiers_.back()->setTargetFrames(target_frames);
|
||||
// }
|
||||
// }
|
||||
observation_subscribers_.push_back(sub);
|
||||
observation_notifiers_.push_back(filter);
|
||||
}
|
||||
|
||||
if (sensor_frame != "")
|
||||
{
|
||||
std::vector < std::string > target_frames;
|
||||
target_frames.push_back(global_frame_);
|
||||
target_frames.push_back(sensor_frame);
|
||||
observation_notifiers_.back()->setTargetFrames(target_frames);
|
||||
}
|
||||
}
|
||||
*/
|
||||
// dsrv_ = NULL;
|
||||
// setupDynamicReconfigure(nh);
|
||||
// }
|
||||
}
|
||||
|
||||
// void ObstacleLayer::setupDynamicReconfigure(ros::NodeHandle& nh)
|
||||
// {
|
||||
@@ -189,11 +187,9 @@
|
||||
// dsrv_->setCallback(cb);
|
||||
// }
|
||||
|
||||
// ObstacleLayer::~ObstacleLayer()
|
||||
// {
|
||||
// if (dsrv_)
|
||||
// delete dsrv_;
|
||||
// }
|
||||
ObstacleLayer::~ObstacleLayer()
|
||||
{}
|
||||
|
||||
// void ObstacleLayer::reconfigureCB(costmap_2d::ObstaclePluginConfig &config, uint32_t level)
|
||||
// {
|
||||
// enabled_ = config.enabled;
|
||||
@@ -300,246 +296,246 @@
|
||||
// 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);
|
||||
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;
|
||||
bool current = true;
|
||||
std::vector<Observation> observations, clearing_observations;
|
||||
|
||||
// // get the marking observations
|
||||
// current = current && getMarkingObservations(observations);
|
||||
// get the marking observations
|
||||
current = current && getMarkingObservations(observations);
|
||||
|
||||
// // get the clearing observations
|
||||
// current = current && getClearingObservations(clearing_observations);
|
||||
// get the clearing observations
|
||||
current = current && getClearingObservations(clearing_observations);
|
||||
|
||||
// // update the global current status
|
||||
// current_ = current;
|
||||
// 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);
|
||||
// }
|
||||
// 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;
|
||||
// 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 sensor_msgs::PointCloud2& cloud = *(obs.cloud_);
|
||||
const sensor_msgs::PointCloud2& cloud = *(obs.cloud_);
|
||||
|
||||
// double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;
|
||||
double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;
|
||||
|
||||
// sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
|
||||
// sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
|
||||
// sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
|
||||
|
||||
// for (; iter_x !=iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
|
||||
// {
|
||||
// double px = *iter_x, py = *iter_y, pz = *iter_z;
|
||||
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_)
|
||||
// {
|
||||
// ROS_DEBUG("The point is too high");
|
||||
// continue;
|
||||
// }
|
||||
// if the obstacle is too high or too far away from the robot we won't add it
|
||||
if (pz > max_obstacle_height_)
|
||||
{
|
||||
printf("The point is too high");
|
||||
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);
|
||||
// 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)
|
||||
// {
|
||||
// ROS_DEBUG("The point is too far away");
|
||||
// continue;
|
||||
// }
|
||||
// if the point is far enough away... we won't consider it
|
||||
if (sq_dist >= sq_obstacle_range)
|
||||
{
|
||||
printf("The point is too far away");
|
||||
continue;
|
||||
}
|
||||
|
||||
// // now we need to compute the map coordinates for the observation
|
||||
// unsigned int mx, my;
|
||||
// if (!worldToMap(px, py, mx, my))
|
||||
// {
|
||||
// ROS_DEBUG("Computing map coords failed");
|
||||
// continue;
|
||||
// }
|
||||
// now we need to compute the map coordinates for the observation
|
||||
unsigned int mx, my;
|
||||
if (!worldToMap(px, py, mx, my))
|
||||
{
|
||||
printf("Computing map coords failed");
|
||||
continue;
|
||||
}
|
||||
|
||||
// unsigned int index = getIndex(mx, my);
|
||||
// costmap_[index] = LETHAL_OBSTACLE;
|
||||
// touch(px, py, min_x, min_y, max_x, max_y);
|
||||
// }
|
||||
// }
|
||||
unsigned int index = getIndex(mx, my);
|
||||
costmap_[index] = LETHAL_OBSTACLE;
|
||||
touch(px, py, min_x, min_y, max_x, max_y);
|
||||
}
|
||||
}
|
||||
|
||||
// updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
|
||||
// }
|
||||
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_);
|
||||
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);
|
||||
// }
|
||||
// }
|
||||
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(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
|
||||
// {
|
||||
// if (footprint_clearing_enabled_)
|
||||
// {
|
||||
// setConvexPolygonCost(transformed_footprint_, costmap_2d::FREE_SPACE);
|
||||
// }
|
||||
void ObstacleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
|
||||
{
|
||||
if (footprint_clearing_enabled_)
|
||||
{
|
||||
setConvexPolygonCost(transformed_footprint_, 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;
|
||||
// }
|
||||
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(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::addStaticObservation(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();
|
||||
// }
|
||||
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::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::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;
|
||||
}
|
||||
|
||||
// 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 sensor_msgs::PointCloud2 &cloud = *(clearing_observation.cloud_);
|
||||
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 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))
|
||||
// {
|
||||
// ROS_WARN_THROTTLE(
|
||||
// 1.0, "The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.",
|
||||
// ox, oy);
|
||||
// return;
|
||||
// }
|
||||
// get the map coordinates of the origin of the sensor
|
||||
unsigned int x0, y0;
|
||||
if (!worldToMap(ox, oy, x0, y0))
|
||||
{
|
||||
printf(
|
||||
"The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.",
|
||||
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_;
|
||||
// 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);
|
||||
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
|
||||
// sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
|
||||
// sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
|
||||
// for each point in the cloud, we want to trace a line from the origin and clear obstacles along it
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
|
||||
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;
|
||||
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;
|
||||
// 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 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;
|
||||
// }
|
||||
// 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;
|
||||
// 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;
|
||||
// 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);
|
||||
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);
|
||||
// }
|
||||
// }
|
||||
updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
|
||||
}
|
||||
}
|
||||
|
||||
// void ObstacleLayer::activate()
|
||||
// {
|
||||
@@ -565,15 +561,15 @@
|
||||
// }
|
||||
// }
|
||||
|
||||
// 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::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()
|
||||
// {
|
||||
@@ -583,4 +579,12 @@
|
||||
// activate();
|
||||
// }
|
||||
|
||||
// } // namespace costmap_2d
|
||||
// Export factory function
|
||||
static PluginPtr create_obstacle_plugin() {
|
||||
return std::make_shared<ObstacleLayer>();
|
||||
}
|
||||
|
||||
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
|
||||
BOOST_DLL_ALIAS(create_obstacle_plugin, create_plugin)
|
||||
|
||||
} // namespace costmap_2d
|
||||
|
||||
Reference in New Issue
Block a user