Skip to content
Snippets Groups Projects
Commit ecd68092 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

compila

parent eed6534b
No related branches found
No related tags found
3 merge requests!4new release,!3New release,!2Occupancy maps
Hide whitespace changes
Inline Side-by-side
include/publisher_occupancy_map.h
+ 25
− 3
View file @ ecd68092
#ifndef PUBLISHER_OCCUPANCY_MAP_H
#define PUBLISHER_OCCUPANCY_MAP_H
/**************************
* ROS includes *
**************************/
#include <nav_msgs/OccupancyGrid.h>
/**************************
* WOLF includes *
**************************/
#include <core/common/wolf.h>
#include <core/problem/problem.h>
#include <laser/capture/capture_laser_2d.h>
#include "publisher.h"
......@@ -19,16 +25,21 @@ class PublisherOccupancyMap: public Publisher
double Lfree_, Lobst_, Lobst_thres_, Lfree_thres_;
int max_n_cells_;
Eigen::Vector2i n_cells_;
double grid_size_, laser_ray_incr_, obstacle_ray_incr_;
double grid_size_, laser_ray_incr_;
int n_obstacle_samples_;
double update_dist_th_, update_angle_th_;
bool resized_;
unsigned int last_step_;
Eigen::Vector2d map_origin_;
FrameBasePtr last_frame_, first_frame_;
Eigen::ArrayXXd logodds_grid_;
Eigen::ArrayXXd logodds_array_;
std::map<FrameBasePtr,Eigen::Vector3d> last_trajectory_poses_;
std::map<FrameBasePtr,std::list<CaptureLaser2dPtr>> trajectory_scans_;
std::map<FrameBasePtr,Eigen::Vector3d> trajectory_poses_, last_trajectory_poses_;
nav_msgs::OccupancyGrid occupancy_grid_;
public:
PublisherOccupancyMap(const std::string& _unique_name,
......@@ -38,5 +49,16 @@ class PublisherOccupancyMap: public Publisher
virtual ~PublisherOccupancyMap(){};
void initialize(ros::NodeHandle &nh, const std::string& topic) override;
void publishDerived() override;
protected:
bool updateTrajectory();
void resetOccupancyMap();
bool recomputeMap();
void addScanToMap(const CaptureLaser2dPtr& capture_laser, const Eigen::Vector3d& pose);
void addRayToMap(const double& theta, const double& range, const Eigen::Vector3d& laser_pose, bool _obstacle);
Eigen::Vector2i vectorToCell(const Eigen::Vector2d& p);
void resizeMap(const int& dim, const unsigned int& oversize, const bool& back);
void updateMsg();
void resizeMsg();
};
#endif
src/publisher_occupancy_map.cpp
+ 130
− 147
View file @ ecd68092
#include "publisher_occupancy_map.h"
#include "core/common/wolf.h"
#include "core/feature/feature_base.h"
#include <laser/capture/capture_laser_2d.h>
#include <laser/sensor/sensor_laser_2d.h>
#include <laser/feature/feature_icp_align.h>
#include <laser_scan_utils/laser_scan.h>
#include <laser_scan_utils/laser_scan_utils.h>
......@@ -20,33 +16,37 @@ void PublisherOccupancyMap::initialize(ros::NodeHandle& nh, const std::string& t
}
void PublisherOccupancyMap::publishDerived()
{
// Recompute occupancy grid
recomputeOccupancyGrid(*msg);
// update trajectory (scans and poses)
bool recompute = updateTrajectory();
// reset occupancy map
if (recompute)
resetOccupancyMap();
// Recompute occupancy map
if(recomputeMap())
updateMsg();
// publish
publisher_.publish(occupancy_grid_);
}
void PublisherOccupancyMap::recomputeOccupancyGrid()
bool PublisherOccupancyMap::updateTrajectory()
{
//std::cout << "PublisherOccupancyMap::recomputeOccupancyGrid: " << std::endl;
//std::cout << "PublisherOccupancyMap::updateTrajectory: " << std::endl;
tf::Transform T_map_base;
unsigned int keyframe_idx;
Eigen::Vector3d kf_pose;
int prev_kf_idx = -1;
trajectory_poses_.clear();
trajectory_scans_.clear();
// copy new trajectory poses and scans
std::map<FrameBasePtr,Eigen::Vector3d> trajectory_poses;
std::map<FrameBasePtr,CaptureLaser2dPtrList> trajectory_scans;
auto frame_map = problem_->getTrajetory()->getFrameMap(); // via copy for thread-safe
auto frame_map = problem_->getTrajectory()->getFrameMap(); // copy for thread-safe
for (auto frame_pair : frame_map)
{
if (not frame_pair.second->isRemoving())
{
// insert frame to maps
trajectory_poses.insert(frame_pair.second, frame_pair->getState());
trajectory_scans.insert(frame_pair.second, CaptureLaser2dPtrList());
trajectory_poses_[frame_pair.second] = frame_pair.second->getState("PO").vector("PO");
trajectory_scans_[frame_pair.second] = std::list<CaptureLaser2dPtr>();
// search all CaptureLaser2d
auto cap_list = frame_pair.second->getCaptureList();
......@@ -55,121 +55,108 @@ void PublisherOccupancyMap::recomputeOccupancyGrid()
auto cap_laser = std::dynamic_pointer_cast<CaptureLaser2d>(cap);
if (cap_laser and not cap_laser->isRemoving())
{
trajectory_scans.at(frame_pair.second).push_back(cap_laser);
trajectory_scans_.at(frame_pair.second).push_back(cap_laser);
}
}
// remove frames without scans
if (trajectory_scans_.at(frame_pair.second).empty())
{
trajectory_scans_.erase(frame_pair.second);
trajectory_poses_.erase(frame_pair.second);
}
}
}
// check if trajectory changed enough
for ()
bool recompute = false;
for (auto frame_pos : last_trajectory_poses_)
{
int kf_idx = scan_kfidx_from_it->second;
if (kf_idx == prev_kf_idx)
continue;
// without any pose enough changed reached a new trajectory pose
if (last_trajectory_poses_.count(kf_idx) == 0)
// any old frame was removed -> recompute occupancy map
if (trajectory_poses_.count(frame_pos.first) == 0)
{
recompute = true;
break;
}
// load KF pose
kf_pose << trajectory.key_frames.at(kf_idx).frame_pose.pose.position.x,
trajectory.key_frames.at(kf_idx).frame_pose.pose.position.y,
tf::getYaw(trajectory.key_frames.at(kf_idx).frame_pose.pose.orientation);
// any old frame moved significantly (angle or position) -> recompute occupancy map
const Eigen::Vector3d& old_frame_pos = frame_pos.second;
const Eigen::Vector3d& new_frame_pos = trajectory_poses_.at(frame_pos.first);
//std::cout << "kf_pose " << kf_pose.transpose() << std::endl;
//std::cout << "stored pose " << last_trajectory_poses_.at(kf_idx).transpose() << std::endl;
//std::cout << "old_frame_pos " << old_frame_pos.transpose() << std::endl;
//std::cout << "new_frame_pos " << new_frame_pos.transpose() << std::endl;
// If any pose changed enough, update map from scratch
if ((kf_pose-last_trajectory_poses_.at(kf_idx)).head<2>().norm() > update_dist_th_ ||
std::abs((kf_pose-last_trajectory_poses_.at(kf_idx))(2)) > update_angle_th_)
if ((old_frame_pos-new_frame_pos).head<2>().norm() > update_dist_th_ ||
std::abs(old_frame_pos(2)-new_frame_pos(2)) > update_angle_th_)
{
scan_kfidx_from_it = keyscan_to_keyframe_idx_.begin();
recompute = true;
break;
}
prev_kf_idx = kf_idx;
}
if (scan_kfidx_from_it == keyscan_to_keyframe_idx_.end())
return;
// Reset logodds_grid_ (if update from scratch)
if (scan_kfidx_from_it == keyscan_to_keyframe_idx_.begin())
logodds_grid_ = Eigen::Array<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>::Zero(n_cells_(0),n_cells_(1));
// Update the logodds_grid_
prev_kf_idx = -1;
for (auto scan_kfidx_it = scan_kfidx_from_it; scan_kfidx_it != keyscan_to_keyframe_idx_.end(); scan_kfidx_it++)
{
auto const& kf_idx = scan_kfidx_it->second;
auto const& scan = scan_kfidx_it->first;
assert(kf_idx < trajectory.key_frames.size());
return recompute;
}
ROS_DEBUG("TR 2 OCCGRID: Adding scan %u from pose: %u - %f, %f, %f", scan.header.seq, kf_idx,
trajectory.key_frames.at(kf_idx).frame_pose.pose.position.x,
trajectory.key_frames.at(kf_idx).frame_pose.pose.position.y,
tf::getYaw(trajectory.key_frames.at(kf_idx).frame_pose.pose.orientation));
void PublisherOccupancyMap::resetOccupancyMap()
{
// Reset logodds_grid_
logodds_array_ = Eigen::Array<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>::Zero(n_cells_(0),n_cells_(1));
// only if not the previous KF (no new pose
if (kf_idx != prev_kf_idx)
{
// compute tf::Transform
tf::poseMsgToTF(trajectory.key_frames.at(kf_idx).frame_pose.pose, T_map_base);
// reset last trajectory poses
last_trajectory_poses_.clear();
}
// load KF pose
kf_pose << trajectory.key_frames.at(kf_idx).frame_pose.pose.position.x,
trajectory.key_frames.at(kf_idx).frame_pose.pose.position.y,
tf::getYaw(trajectory.key_frames.at(kf_idx).frame_pose.pose.orientation);
bool PublisherOccupancyMap::recomputeMap()
{
bool updated = false;
// store pose used
last_trajectory_poses_[kf_idx] = kf_pose;
}
// Update the map with captures of frames in trajectory_poses_ that are not in last_trajectory_poses_
for (auto frame_pose : trajectory_poses_)
{
if (last_trajectory_poses_.count(frame_pose.first) == 1)
continue;
// log odds
addScanToLogodds(scan, T_map_base);
// update trajectory pose
last_trajectory_poses_.at(frame_pose.first) = frame_pose.second;
// store pose used
last_trajectory_poses_[kf_idx] = kf_pose;
// add all scans of the frame the map
for (auto cap_laser : trajectory_scans_.at(frame_pose.first))
addScanToMap(cap_laser, frame_pose.second);
prev_kf_idx = kf_idx;
updated = true;
}
updateOccupancyGridMsg(trajectory.header.stamp);
//std::cout << "PublisherOccupancyMap::recomputeOccupancyGrid: done" << std::endl;
//std::cout << "PublisherOccupancyMap::recomputeMap: done" << std::endl;
return updated;
}
void PublisherOccupancyMap::addScanToLogodds(const sensor_msgs::LaserScan& scan, const tf::Transform& T_map_base)
void PublisherOccupancyMap::addScanToMap(const CaptureLaser2dPtr& capture_laser, const Eigen::Vector3d& pose)
{
unsigned int laser_id = laser_frame_2_idx_[scan.header.frame_id];
// laser pose
geometry_msgs::Pose laser_pose;
tf::Transform T_map_laser;
T_map_laser = T_map_base * laser_extrinsics_[laser_id];
tf::poseTFToMsg(T_map_laser, laser_pose);
// Compute the log odds update
double theta = scan.angle_min;
for (unsigned int i=0; i < scan.ranges.size(); i++, theta+=scan.angle_increment)
// scan pose (considering extrinsics)
Eigen::Vector3s laser_pose = pose;
laser_pose.head<2>() += Eigen::Rotation2Dd(pose(2)) * capture_laser->getSensor()->getP()->getState();
laser_pose(2) += capture_laser->getSensor()->getO()->getState()(0);
// Apply log odds for each beam
const std::vector<float>& ranges = capture_laser->getScan().ranges_;
auto scan_params = std::static_pointer_cast<SensorLaser2d>(capture_laser->getSensor())->getScanParams();
double theta = scan_params.angle_min_;
for (unsigned int i=0; i < ranges.size(); i++, theta+=scan_params.angle_step_)
{
assert(theta <= scan.angle_max + 1e-3 && "TR 2 OCCGRID: Ray orientation higher than angle_max ");
if (scan.ranges[i] > scan.range_min)
assert(theta <= scan_params.angle_max_ + 1e-3 && "TR 2 OCCGRID: Ray orientation higher than angle_max ");
if (ranges[i] > scan_params.range_min_)
// Add the logodds ray
addRayToLogodds(theta, scan.ranges[i], laser_pose, scan.ranges[i] < scan.range_max-0.2);
addRayToMap(theta, ranges[i], laser_pose, ranges[i] < scan_params.range_max_-0.1);
}
}
void PublisherOccupancyMap::addRayToLogodds(const double& theta, const double& range, const geometry_msgs::Pose& laser_pose, bool _obstacle)
void PublisherOccupancyMap::addRayToMap(const double& theta, const double& range, const Eigen::Vector3d& laser_pose, bool _obstacle)
{
double d = 0;
Eigen::Vector2f origin, point, displ;
Eigen::Vector2d origin, point, displ;
double orientation = theta + tf::getYaw(laser_pose.orientation);
origin(0) = laser_pose.position.x;
origin(1) = laser_pose.position.y;
double orientation = theta + laser_pose(2);
origin = laser_pose.head<2>();
displ(0) = cos(orientation) * laser_ray_incr_;
displ(1) = sin(orientation) * laser_ray_incr_;
......@@ -185,7 +172,7 @@ void PublisherOccupancyMap::addRayToLogodds(const double& theta, const double& r
Eigen::Vector2i cell = vectorToCell(point);
if (cell != cell_prev)
logodds_grid_(cell(0), cell(1)) += Lfree_;
logodds_array_(cell(0), cell(1)) += Lfree_;
cell_prev = cell;
......@@ -198,18 +185,18 @@ void PublisherOccupancyMap::addRayToLogodds(const double& theta, const double& r
if (_obstacle)
{
double range_boundary = range;
for (auto j = 0; range_boundary-range < obstacle_ray_incr_; j++, range_boundary += laser_ray_incr_)
for (auto j = 0; j < n_obstacle_samples_; j++, range_boundary += laser_ray_incr_)
{
point(0) = origin(0) + cos(orientation) * range_boundary;
point(1) = origin(1) + sin(orientation) * range_boundary;
Eigen::Vector2i cell = vectorToCell(point);
logodds_grid_(cell(0), cell(1)) += Lobst_;
logodds_array_(cell(0), cell(1)) += Lobst_;
}
}
}
Eigen::Vector2i PublisherOccupancyMap::vectorToCell(const Eigen::Vector2f& p)
Eigen::Vector2i PublisherOccupancyMap::vectorToCell(const Eigen::Vector2d& p)
{
Eigen::Vector2i cell;
cell(0) = int( std::floor((p(0) - map_origin_(0)) / grid_size_ ));
......@@ -221,11 +208,11 @@ Eigen::Vector2i PublisherOccupancyMap::vectorToCell(const Eigen::Vector2f& p)
while ( oversize > 0 && n_cells_(i) < max_n_cells_ )
{
bool back = !(cell(i) < 0);
ROS_DEBUG("TR 2 OCCGRID: Point out of the map in dimension %i: \n\tpoint = %f, %f\n\tlower = %f, %f\n\tupper = %f, %f\n\tcell = %i, %i\n\tn cells = %i, %i", i,
ROS_DEBUG("PublisherOccupancyMap: Point out of the map in dimension %i: \n\tpoint = %f, %f\n\tlower = %f, %f\n\tupper = %f, %f\n\tcell = %i, %i\n\tn cells = %i, %i", i,
p(0), p(1), map_origin_(0), map_origin_(1), map_origin_(0) + grid_size_ * n_cells_(1), map_origin_(1) + grid_size_ * n_cells_(0),
cell(1), cell(0), n_cells_(1), n_cells_(0));
resizeMap(i, oversize, back);
ROS_DEBUG("TR 2 OCCGRID: Map resized in dimension %i: new limits:\n\tlower = %f, %f\n\tupper = %f, %f\n\tn cells = %i, %i", i,
ROS_DEBUG("PublisherOccupancyMap: Map resized in dimension %i: new limits:\n\tlower = %f, %f\n\tupper = %f, %f\n\tn cells = %i, %i", i,
map_origin_(0), map_origin_(1), map_origin_(0) + grid_size_ * n_cells_(1), map_origin_(1) + grid_size_ * n_cells_(0), n_cells_(1), n_cells_(0));
// Recompute current cell and oversize
......@@ -238,47 +225,6 @@ Eigen::Vector2i PublisherOccupancyMap::vectorToCell(const Eigen::Vector2f& p)
return cell;
}
void PublisherOccupancyMap::updateOccupancyGridMsg(const ros::Time& _stamp)
{
Eigen::Array<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> occupancy_probability(n_cells_(0), n_cells_(1));
occupancy_probability = (logodds_grid_ >= Lobst_thres_).select(100,occupancy_probability);
occupancy_probability = (logodds_grid_ <= Lfree_thres_).select(0,occupancy_probability);
occupancy_probability = (logodds_grid_ < Lobst_thres_ && logodds_grid_ > Lfree_thres_).select(-1,occupancy_probability);
{
std::lock_guard<std::mutex> lock(mut_occupancy_grid_);
if(resized_)
resizeOccupancyGrid();
std::copy(occupancy_probability.data(), occupancy_probability.data() + occupancy_probability.size(), occupancy_grid_.data.begin());
//ROS_INFO("TR 2 OCCGRID: OccupancyGrid updated! occupancy_probability.size() = %i - occupancy_grid_.data.size() = %i",occupancy_probability.size(),occupancy_grid_.data.size());
occupancy_grid_.header.stamp = _stamp;
occupancy_grid_.info.map_load_time = _stamp;
}
}
void PublisherOccupancyMap::resizeOccupancyGrid()
{
// initial pose in the middle of the map
geometry_msgs::Pose origin_pose;
origin_pose.position.x = map_origin_(0);
origin_pose.position.y = map_origin_(1);
origin_pose.position.z = 0.0;
// Occupancy grid meta data Initializiation
occupancy_grid_.info.resolution = grid_size_;
occupancy_grid_.info.width = n_cells_(0);
occupancy_grid_.info.height = n_cells_(1);
occupancy_grid_.info.origin = origin_pose;
// resize data
occupancy_grid_.data.resize(logodds_grid_.size(),-1);
resized_= false;
}
void PublisherOccupancyMap::resizeMap(const int& dim, const unsigned int& oversize, const bool& back)
{
Eigen::Vector2i new_n_cells = n_cells_;
......@@ -290,8 +236,8 @@ void PublisherOccupancyMap::resizeMap(const int& dim, const unsigned int& oversi
// Origin doesn't change
// Array resize
Eigen::Array<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> new_logodds_grid = Eigen::Array<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>::Zero(new_n_cells(0), new_n_cells(1));
new_logodds_grid.block(0, 0, n_cells_(0), n_cells_(1)) = logodds_grid_;
logodds_grid_ = new_logodds_grid;
new_logodds_grid.block(0, 0, n_cells_(0), n_cells_(1)) = logodds_array_;
logodds_array_ = new_logodds_grid;
}
// at the beggining of the array
else
......@@ -303,12 +249,49 @@ void PublisherOccupancyMap::resizeMap(const int& dim, const unsigned int& oversi
Eigen::Array<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> new_logodds_grid = Eigen::Array<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>::Zero(new_n_cells(0), new_n_cells(1));
Eigen::Vector2i place = Eigen::Vector2i::Zero(2);
place(dim) = oversize;
new_logodds_grid.block(place(0), place(1), n_cells_(0), n_cells_(1)) = logodds_grid_;
logodds_grid_ = new_logodds_grid;
new_logodds_grid.block(place(0), place(1), n_cells_(0), n_cells_(1)) = logodds_array_;
logodds_array_ = new_logodds_grid;
}
// Change in the occupancy grid msg
n_cells_ = new_n_cells;
resized_= true;
}
void PublisherOccupancyMap::updateMsg()
{
Eigen::ArrayXXd occupancy_probability(n_cells_(0), n_cells_(1));
occupancy_probability = (logodds_array_ >= Lobst_thres_).select(100,occupancy_probability);
occupancy_probability = (logodds_array_ <= Lfree_thres_).select(0,occupancy_probability);
occupancy_probability = (logodds_array_ < Lobst_thres_ && logodds_array_ > Lfree_thres_).select(-1,occupancy_probability);
if(resized_)
resizeMsg();
std::copy(occupancy_probability.data(), occupancy_probability.data() + occupancy_probability.size(), occupancy_grid_.data.begin());
//ROS_INFO("PublisherOccupancyMap::updateMsg: OccupancyGrid updated! occupancy_probability.size() = %i - occupancy_grid_.data.size() = %i",occupancy_probability.size(),occupancy_grid_.data.size());
occupancy_grid_.header.stamp = ros::Time::now();
occupancy_grid_.info.map_load_time = ros::Time::now();
}
void PublisherOccupancyMap::resizeMsg()
{
// initial pose in the middle of the map
geometry_msgs::Pose origin_pose;
origin_pose.position.x = map_origin_(0);
origin_pose.position.y = map_origin_(1);
origin_pose.position.z = 0.0;
// Occupancy grid meta data Initializiation
occupancy_grid_.info.resolution = grid_size_;
occupancy_grid_.info.width = n_cells_(0);
occupancy_grid_.info.height = n_cells_(1);
occupancy_grid_.info.origin = origin_pose;
// resize data
occupancy_grid_.data.resize(logodds_array_.size(),-1);
resized_= false;
}
WOLF_REGISTER_PUBLISHER(PublisherOccupancyMap)
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment