void PotentialField::init() {
ROS_INFO("Created map");
map_.setFrameId("/potential_field_link");
map_.setGeometry(Length(map_x_size_, map_y_size_), map_resolution_);
for (GridMapIterator it(map_); !it.isPastEnd(); ++it) {
Position position;
map_.getPosition(*it, position);
map_.at("obstacle_field", *it) = 0.0;
map_.at("target_waypoint_field", *it) = 0.0;
map_.at("vscan_points_field", *it) = 0.0;
map_.at("potential_field", *it) = 0.0;
}
ROS_INFO("Created map with size %f x %f m (%i x %i cells).",
map_.getLength().x(), map_.getLength().y(), map_.getSize()(0),
map_.getSize()(1));
}
void populateMap(GridMap &map, string layer,string file_path, float scale,float res)
{
cv_bridge::CvImage cv_image;
Mat img = imread(file_path,CV_LOAD_IMAGE_GRAYSCALE);
cv::Size img_size= img.size();
cv::resize(img,img,cv::Size(),scale,scale, CV_INTER_CUBIC);
map.setGeometry(Length(img.rows*res,img.cols*res),res);
for (GridMapIterator it(map); !it.isPastEnd(); ++it)
{
Position position;
map.getPosition(*it, position);
int x = (img.rows/2) + position.x()/res+res/2.0;
int y = (img.cols/2) + position.y()/res+res/2.0;
map.at(layer, *it) = img.at<uchar>(x,y)<200?1:0;
}
map.setTimestamp(ros::Time::now().toNSec());
ROS_INFO("Created map with size %f x %f m (%i x %i cells).",map.getLength().x(), map.getLength().y(),map.getSize()(0), map.getSize()(1));
}
TEST(GridMapCvProcessing, changeResolution)
{
// Create grid map.
GridMap mapIn;
mapIn.setGeometry(grid_map::Length(2.0, 1.0), 0.01);
mapIn.add("layer", 1.0);
for (grid_map::CircleIterator iterator(mapIn, mapIn.getPosition(), 0.2); !iterator.isPastEnd(); ++iterator) {
mapIn.at("layer", *iterator) = 2.0;
}
// Change resolution.
GridMap mapOut;
EXPECT_TRUE(GridMapCvProcessing::changeResolution(mapIn, mapOut, 0.1));
// Check data.
EXPECT_TRUE((mapIn.getLength() == mapOut.getLength()).all());
EXPECT_TRUE(mapIn.getPosition() == mapOut.getPosition());
EXPECT_TRUE((mapIn.getSize() == mapOut.getSize() * 10).all());
EXPECT_EQ(mapIn["layer"](0, 0), mapOut["layer"](0, 0)); // Corner.
EXPECT_EQ(mapIn.atPosition("layer", mapIn.getPosition()), mapOut.atPosition("layer", mapOut.getPosition())); // Center.
}
EllipseIterator::EllipseIterator(const GridMap& gridMap, const Position& center, const Length& length, const double rotation)
: center_(center)
{
semiAxisSquare_ = (0.5 * length).square();
double sinRotation = sin(rotation);
double cosRotation = cos(rotation);
transformMatrix_ << cosRotation, sinRotation, sinRotation, -cosRotation;
mapLength_ = gridMap.getLength();
mapPosition_ = gridMap.getPosition();
resolution_ = gridMap.getResolution();
bufferSize_ = gridMap.getSize();
bufferStartIndex_ = gridMap.getStartIndex();
Index submapStartIndex;
Index submapBufferSize;
findSubmapParameters(center, length, rotation, submapStartIndex, submapBufferSize);
internalIterator_ = std::shared_ptr<SubmapIterator>(new SubmapIterator(gridMap, submapStartIndex, submapBufferSize));
if(!isInside()) ++(*this);
}
void PotentialField::vscan_points_callback(
sensor_msgs::PointCloud2::ConstPtr vscan_msg) {
static VscanPointsFieldParamater param;
double around_x(param.around_x);
double around_y(param.around_y);
ros::Time time = ros::Time::now();
pcl::PointCloud<pcl::PointXYZ> pcl_vscan;
pcl::fromROSMsg(*vscan_msg, pcl_vscan);
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_vscan_ptr(
new pcl::PointCloud<pcl::PointXYZ>(pcl_vscan));
double length_x = map_.getLength().x() / 2.0;
double length_y = map_.getLength().y() / 2.0;
for (GridMapIterator it(map_); !it.isPastEnd(); ++it) {
Position position;
map_.getPosition(*it, position);
map_.at("vscan_points_field", *it) = 0.0;
for (int i(0); i < (int)pcl_vscan.size(); ++i) {
double point_x = pcl_vscan.at(i).x - map_x_offset_;
if (3.0 < pcl_vscan.at(i).z + tf_z_ || pcl_vscan.at(i).z + tf_z_ < 0.3)
continue;
if (length_x < point_x && point_x < -1.0 * length_x)
continue;
if (length_y < pcl_vscan.at(i).y && pcl_vscan.at(i).y < -1.0 * length_y)
continue;
if ((point_x + tf_x_) - around_x < position.x() &&
position.x() < point_x + tf_x_ + around_x) {
if (pcl_vscan.at(i).y - around_y < position.y() &&
position.y() < pcl_vscan.at(i).y + around_y) {
map_.at("vscan_points_field", *it) = 1.0; // std::exp(0.0) ;
}
}
}
}
map_.setTimestamp(time.toNSec());
publish_potential_field();
}
