void BallPickerLayer::updateBounds(double origin_x, double origin_y, double origin_yaw, double* min_x, double* min_y, double* max_x, double* max_y)
{
if (!enabled_)
return;
if (layered_costmap_->isRolling())
updateOrigin(origin_x - getSizeInMetersX() / 2, origin_y - getSizeInMetersY() / 2);
double mark_x, mark_y;
unsigned int mx, my;
unsigned int index;
if (clear_flag)
{
*min_x = *min_x - getSizeInMetersX()/2;
*min_y = *min_y - getSizeInMetersY()/2;
*max_x = *max_x + getSizeInMetersX()/2;
*max_y = *max_y + getSizeInMetersY()/2;
}
for (std::vector<ball_picker::PointOfInterest>::iterator iter = obstacle_buffer.begin() ; iter != obstacle_buffer.end(); iter++)
{
mark_x = iter->x;
mark_y = iter->y;
if(worldToMap(mark_x, mark_y, mx, my))
{
index = getIndex(mx, my);
costmap_[index] = LETHAL_OBSTACLE;
}
*min_x = std::min(*min_x, mark_x);
*min_y = std::min(*min_y, mark_y);
*max_x = std::max(*max_x, mark_x);
*max_y = std::max(*max_y, mark_y);
}
}
void GrBulletObject::update()
{
updateOrigin();
updateRotation();
}
void VoxelLayer::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);
if (!enabled_)
return;
useExtraBounds(min_x, min_y, max_x, max_y);
bool current = true;
std::vector<ObservationSet> observations;
std::vector<Observation> clearing_observations;
if(clear_old_){
resetOldCosts(min_x, min_y, max_x, max_y);
}
//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<ObservationSet>::const_iterator it = observations.begin(); it != observations.end(); ++it)
{
const ObservationSet& obs_set = *it;
//check if this is in the max_timeout list
bool max_timeout = false;
if(observation_timeout.find(obs_set.topic)!= observation_timeout.end()){
max_timeout = true;
ROS_DEBUG("Observation set for topic %s - clearing\n", obs_set.topic.c_str());
}
for(std::vector<Observation>::const_iterator it_o = obs_set.marking_observations.begin();
it_o != obs_set.marking_observations.end(); it_o++){
const Observation& obs = *it_o;
//we should throw out stale observations also
CostMapList cm_list;
cm_list.topic = obs_set.topic;
cm_list.obs_timestamp = obs.cloud_->header.stamp;
double obs_ts = cm_list.obs_timestamp / 1.0e6;
const pcl::PointCloud<pcl::PointXYZ>& cloud = *(obs.cloud_);
double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;
int count = 0;
for (unsigned int i = 0; i < cloud.points.size(); ++i)
{
//if the obstacle is too high or too far away from the robot we won't add it
if (cloud.points[i].z > max_obstacle_height_)
continue;
//compute the squared distance from the hitpoint to the pointcloud's origin
double sq_dist = (cloud.points[i].x - obs.origin_.x) * (cloud.points[i].x - obs.origin_.x)
+ (cloud.points[i].y - obs.origin_.y) * (cloud.points[i].y - obs.origin_.y)
+ (cloud.points[i].z - obs.origin_.z) * (cloud.points[i].z - obs.origin_.z);
//if the point is far enough away... we won't consider it
if (sq_dist >= sq_obstacle_range)
continue;
//now we need to compute the map coordinates for the observation
unsigned int mx, my, mz;
if (cloud.points[i].z < origin_z_)
{
if (!worldToMap3D(cloud.points[i].x, cloud.points[i].y, origin_z_, mx, my, mz))
continue;
}
else if (!worldToMap3D(cloud.points[i].x, cloud.points[i].y, cloud.points[i].z, mx, my, mz))
{
continue;
}
//mark the cell in the voxel grid and check if we should also mark it in the costmap
if (voxel_grid_.markVoxelInMap(mx, my, mz, mark_threshold_))
{
unsigned int index = getIndex(mx, my);
//these are the ones set as occupied
costmap_[index] = LETHAL_OBSTACLE;
touch((double)cloud.points[i].x, (double)cloud.points[i].y, min_x, min_y, max_x, max_y);
//keep track of which indices we updated
if(max_timeout){
cm_list.indices.push_back(ObstaclePoint(index, (double)cloud.points[i].x, (double)cloud.points[i].y));
count++;
}
}
}
if(max_timeout && count > 0){
locations_utime.updateObstacleTime(cm_list);
new_obs_list.push_back(cm_list);
}
}
}
if(clear_old_){
//inflate the bounds - otherwise the inflation layer wont reset the inflated cost
*min_x -= inflation_radius_;
*min_y -= inflation_radius_;
*max_x += inflation_radius_;
*max_y += inflation_radius_;
}
if (publish_voxel_)
{
costmap_2d::VoxelGrid grid_msg;
unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY();
grid_msg.size_x = voxel_grid_.sizeX();
grid_msg.size_y = voxel_grid_.sizeY();
grid_msg.size_z = voxel_grid_.sizeZ();
grid_msg.data.resize(size);
memcpy(&grid_msg.data[0], voxel_grid_.getData(), size * sizeof(unsigned int));
grid_msg.origin.x = origin_x_;
grid_msg.origin.y = origin_y_;
grid_msg.origin.z = origin_z_;
grid_msg.resolutions.x = resolution_;
grid_msg.resolutions.y = resolution_;
grid_msg.resolutions.z = z_resolution_;
grid_msg.header.frame_id = global_frame_;
grid_msg.header.stamp = ros::Time::now();
voxel_pub_.publish(grid_msg);
}
footprint_layer_.updateBounds(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
}
void transformChanged ()
{
revertTransform();
m_evaluateTransform();
updateOrigin();
}
void originChanged ()
{
m_aabb_light.origin = m_originKey.m_origin;
updateOrigin();
}
