bool ChooseAccessibleBalls::canMove(float x, float y){
return true;
// ROS_INFO("enter canMove");
// Get a copy of the current costmap to test. (threadsafe)
costmap_2d::Costmap2D costmap;
// ROS_INFO("aaa");
if(costmap_ros != NULL)
costmap_ros->getCostmapCopy( costmap );
// ROS_INFO("bbb");
// Coordinate transform.
unsigned int cell_x, cell_y;
if( !costmap.worldToMap( x, y, cell_x, cell_y )){
return false;
}
double cost = double( costmap.getCost( cell_x, cell_y ));
if(cost <= 127){
return true;
}
else{
return false;
}
}
void CostmapLayer::updateWithAddition(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
if (!enabled_)
return;
unsigned char* master_array = master_grid.getCharMap();
unsigned int span = master_grid.getSizeInCellsX();
for (int j = min_j; j < max_j; j++)
{
unsigned int it = j * span + min_i;
for (int i = min_i; i < max_i; i++)
{
if (costmap_[it] == NO_INFORMATION){
it++;
continue;
}
unsigned char old_cost = master_array[it];
if (old_cost == NO_INFORMATION)
master_array[it] = costmap_[it];
else
{
int sum = old_cost + costmap_[it];
if (sum >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
master_array[it] = costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1;
else
master_array[it] = sum;
}
it++;
}
}
}
void CostmapLayer::updateWithMax(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
if (!enabled_)
return;
unsigned char* master_array = master_grid.getCharMap();
unsigned int span = master_grid.getSizeInCellsX();
for (int j = min_j; j < max_j; j++)
{
unsigned int it = j * span + min_i;
for (int i = min_i; i < max_i; i++)
{
if (costmap_[it] == NO_INFORMATION){
it++;
continue;
}
unsigned char old_cost = master_array[it];
if (old_cost == NO_INFORMATION || old_cost < costmap_[it])
master_array[it] = costmap_[it];
it++;
}
}
}
void upTo(costmap_2d::Costmap2D &gradient_){
for (int varX = 0; varX < gradient_.getSizeInCellsX(); varX++) {
for (int varY = 0; varY < gradient_.getSizeInCellsY(); varY++) {
//if(gradient_.getCost(varX, varY) == 0)
gradient_.setCost(varX, varY, DEF);
}
}
}
bool CSUROGlobalPlanner::isCoordInMap(const costmap_2d::Costmap2D &costmap, const float& x, const float& y)
{
int cellI, cellJ;
original_costmap_.worldToMapEnforceBounds(x, y, cellI, cellJ);
if(cellI < costmap.getSizeInCellsX() && cellI >= 0 && cellJ < costmap.getSizeInCellsY() && cellJ >= 0)
return true;
return false;
}
void SimpleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i,
int max_j)
{
if (!enabled_)
return;
unsigned int mx;
unsigned int my;
if(master_grid.worldToMap(mark_x_, mark_y_, mx, my)){
master_grid.setCost(mx, my, LETHAL_OBSTACLE);
}
}
bool inMap(costmap_2d::Costmap2D &gradient_, int i, int j)
{
if(i < gradient_.getSizeInCellsX() && i >= 0 && j < gradient_.getSizeInCellsY() && j >= 0)
{
return true;
}
else
{
return false;
}
}
void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
if (!map_received_)
return;
if (!layered_costmap_->isRolling())
{
// if not rolling, the layered costmap (master_grid) has same coordinates as this layer
if (!use_maximum_)
updateWithTrueOverwrite(master_grid, min_i, min_j, max_i, max_j);
else
updateWithMax(master_grid, min_i, min_j, max_i, max_j);
}
else
{
// If rolling window, the master_grid is unlikely to have same coordinates as this layer
unsigned int mx, my;
double wx, wy;
// Might even be in a different frame
tf::StampedTransform transform;
try
{
tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0), transform);
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s", ex.what());
return;
}
// Copy map data given proper transformations
for (unsigned int i = min_i; i < max_i; ++i)
{
for (unsigned int j = min_j; j < max_j; ++j)
{
// Convert master_grid coordinates (i,j) into global_frame_(wx,wy) coordinates
layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy);
// Transform from global_frame_ to map_frame_
tf::Point p(wx, wy, 0);
p = transform(p);
// Set master_grid with cell from map
if (worldToMap(p.x(), p.y(), mx, my))
{
if (!use_maximum_)
master_grid.setCost(i, j, getCost(mx, my));
else
master_grid.setCost(i, j, std::max(getCost(mx, my), master_grid.getCost(i, j)));
}
}
}
}
}
//update what map cells are considered path based on the global_plan
void MapGrid::setTargetCells (const costmap_2d::Costmap2D& costmap,
const std::vector<geometry_msgs::PoseStamped>& global_plan)
{
sizeCheck (costmap.getSizeInCellsX(), costmap.getSizeInCellsY());
bool started_path = false;
queue<MapCell*> path_dist_queue;
std::vector<geometry_msgs::PoseStamped> adjusted_global_plan;
adjustPlanResolution (global_plan, adjusted_global_plan, costmap.getResolution());
if (adjusted_global_plan.size() != global_plan.size())
{
ROS_DEBUG ("Adjusted global plan resolution, added %zu points", adjusted_global_plan.size() - global_plan.size());
}
unsigned int i;
// put global path points into local map until we reach the border of the local map
for (i = 0; i < adjusted_global_plan.size(); ++i)
{
double g_x = adjusted_global_plan[i].pose.position.x;
double g_y = adjusted_global_plan[i].pose.position.y;
unsigned int map_x, map_y;
if (costmap.worldToMap (g_x, g_y, map_x, map_y) && costmap.getCost (map_x, map_y) != costmap_2d::NO_INFORMATION)
{
MapCell& current = getCell (map_x, map_y);
current.target_dist = 0.0;
current.target_mark = true;
path_dist_queue.push (¤t);
started_path = true;
}
else if (started_path)
{
break;
}
}
if (!started_path)
{
ROS_ERROR ("None of the %d first of %zu (%zu) points of the global plan were in the local costmap and free",
i, adjusted_global_plan.size(), global_plan.size());
return;
}
computeTargetDistance (path_dist_queue, costmap);
}
double getCost(float x, float y){
// Get a copy of the current costmap to test. (threadsafe)
costmap_2d::Costmap2D costmap;
costmap_ros->getCostmapCopy( costmap );
// Coordinate transform.
unsigned int cell_x, cell_y;
if( !costmap.worldToMap( x, y, cell_x, cell_y )){
return -1.0;
}
double cost = double( costmap.getCost( cell_x, cell_y ));
// ROS_INFO(" world pose = (%f, %f) map pose = (%d, %d) cost =%f", x, y, cell_x, cell_y, cost);
return cost;
}
inline bool MapGrid::updatePathCell (MapCell* current_cell, MapCell* check_cell,
const costmap_2d::Costmap2D& costmap)
{
//if the cell is an obstacle set the max path distance
unsigned char cost = costmap.getCost (check_cell->cx, check_cell->cy);
if (! getCell (check_cell->cx, check_cell->cy).within_robot &&
(cost == costmap_2d::LETHAL_OBSTACLE ||
cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE ||
cost == costmap_2d::NO_INFORMATION))
{
check_cell->target_dist = obstacleCosts();
return false;
}
double new_target_dist = current_cell->target_dist + 1;
if (new_target_dist < check_cell->target_dist)
{
check_cell->target_dist = new_target_dist;
}
return true;
}
void CostmapLayer::updateWithOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
if (!enabled_)
return;
unsigned char* master = master_grid.getCharMap();
unsigned int span = master_grid.getSizeInCellsX();
for (int j = min_j; j < max_j; j++)
{
unsigned int it = span*j+min_i;
for (int i = min_i; i < max_i; i++)
{
if (costmap_[it] != NO_INFORMATION)
master[it] = costmap_[it];
it++;
}
}
}
void SimpleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i,
int max_j)
{
if (!enabled_)
return;
unsigned int mx;
unsigned int my;
if(pode_marcar){
if(master_grid.worldToMap(mark_x_, mark_y_, mx, my)){
for(int i = -1; i < 1; i++)
master_grid.setCost(mx+5, my+i, LETHAL_OBSTACLE);
}
pode_marcar = false;
}
// ROS_INFO("Hi!");
}
void BallPickerLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
if (!enabled_)
return;
const unsigned char* master_array = master_grid.getCharMap();
for (int j = min_j; j < max_j; j++)
{
for (int i = min_i; i < max_i; i++)
{
if (clear_flag)
{
master_grid.setCost(i, j, FREE_SPACE);
}
else
{
int index = getIndex(i, j);
if (costmap_[index] == NO_INFORMATION)
continue;
unsigned char old_cost = master_array[index];
if (old_cost == NO_INFORMATION || old_cost < costmap_[index])
master_grid.setCost(i, j, costmap_[index]);
}
}
}
for (int i=0; i < getSizeInCellsX()*getSizeInCellsY(); i++)
costmap_[i] == NO_INFORMATION;
if (!obstacle_buffer.empty())
{
ROS_INFO("Costmap updated with ball obstacles.");
obstacle_buffer.clear();
std_srvs::Empty emptysrv;
confirm_update_client.call(emptysrv);
}
}
bool go(costmap_2d::Costmap2D &gradient_, costmap_2d::Costmap2D &original_, int i, int j, int cost)
{
if(inMap(gradient_, i, j) == false){
//ROS_INFO("salgo por no estar en mapa");
return false;
}
if(original_.getCost( i, j) != 0 ){
//ROS_INFO("salgo por ser obstaculo");
return false;
}
if(gradient_.getCost( i, j) <= cost){
//ROS_INFO("salgo por ser igual o menor");
return false;
}
return true;
}
bool canMove(float x, float y){
// Get a copy of the current costmap to test. (threadsafe)
costmap_2d::Costmap2D costmap;
costmap_ros->getCostmapCopy( costmap );
// Coordinate transform.
unsigned int cell_x, cell_y;
if( !costmap.worldToMap( x, y, cell_x, cell_y )){
// res.cost = -1.0;
return false;
}
double cost = double( costmap.getCost( cell_x, cell_y ));
ROS_INFO("cost = %f", cost);
if(cost <= 1){
return true;
}
else{
return false;
}
//ROS_INFO(" world pose = (%f, %f) map pose = (%d, %d) cost =%f", x, y, cell_x, cell_y, cost);
}
void VirtualWallLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
if (!enabled_)
return;
for (int j = min_j; j < max_j; j++)
{
for (int i = min_i; i < max_i; i++)
{
int index = getIndex(i, j);
if (costmap_[index] == NO_INFORMATION)
continue;
master_grid.setCost(i, j, costmap_[index]);
}
}
}
//mark the point of the costmap as local goal where global_plan first leaves the area (or its last point)
void MapGrid::setLocalGoal (const costmap_2d::Costmap2D& costmap,
const std::vector<geometry_msgs::PoseStamped>& global_plan)
{
sizeCheck (costmap.getSizeInCellsX(), costmap.getSizeInCellsY());
int local_goal_x = -1;
int local_goal_y = -1;
bool started_path = false;
std::vector<geometry_msgs::PoseStamped> adjusted_global_plan;
adjustPlanResolution (global_plan, adjusted_global_plan, costmap.getResolution());
// skip global path points until we reach the border of the local map
for (unsigned int i = 0; i < adjusted_global_plan.size(); ++i)
{
double g_x = adjusted_global_plan[i].pose.position.x;
double g_y = adjusted_global_plan[i].pose.position.y;
unsigned int map_x, map_y;
if (costmap.worldToMap (g_x, g_y, map_x, map_y) && costmap.getCost (map_x, map_y) != costmap_2d::NO_INFORMATION)
{
local_goal_x = map_x;
local_goal_y = map_y;
started_path = true;
}
else
{
if (started_path)
{
break;
}// else we might have a non pruned path, so we just continue
}
}
if (!started_path)
{
ROS_ERROR ("None of the points of the global plan were in the local costmap, global plan points too far from robot");
return;
}
queue<MapCell*> path_dist_queue;
if (local_goal_x >= 0 && local_goal_y >= 0)
{
MapCell& current = getCell (local_goal_x, local_goal_y);
costmap.mapToWorld (local_goal_x, local_goal_y, goal_x_, goal_y_);
current.target_dist = 0.0;
current.target_mark = true;
path_dist_queue.push (¤t);
}
computeTargetDistance (path_dist_queue, costmap);
}
void spread(costmap_2d::Costmap2D &gradient_, costmap_2d::Costmap2D &original_, int value, int i, int j, int startCellI, int startCellJ)
{
gradient_.setCost(i, j, value);
//ROS_INFO("(%d, %d) <- %d", i, j, value);
value = value + 5;
if(value>254)
value=254;
if(go(gradient_, original_, i-1,j, value))
spread(gradient_, original_, value, i-1, j, startCellI, startCellJ);
if(go(gradient_, original_, i,j-1, value))
spread(gradient_, original_, value, i, j-1, startCellI, startCellJ);
if(go(gradient_, original_, i+1,j, value))
spread(gradient_, original_, value, i+1, j, startCellI, startCellJ);
if(go(gradient_, original_, i,j+1, value))
spread(gradient_, original_, value, i, j+1, startCellI, startCellJ);
}
void fillPlan(costmap_2d::Costmap2D gradient_, std::vector<geometry_msgs::PoseStamped>& plan, const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, int startI, int startJ, int goalI, int goalJ)
{
int iPoint = startI;
int jPoint = startJ;
plan.push_back(start);
double x,y;
int counter = 0;
geometry_msgs::PoseStamped before = start;
geometry_msgs::PoseStamped point;
tf::Quaternion point_quat;
while(isEnd(gradient_, iPoint, jPoint, goalI, goalJ) == false)
{
counter++;
getMin(gradient_, iPoint, jPoint);
gradient_.mapToWorld (iPoint, jPoint, x, y);
point.pose.position.x = x;
point.pose.position.y = y;
point_quat = tf::createQuaternionFromYaw(atan2 (y, x));
point.pose.orientation.x = point_quat.x();
point.pose.orientation.y = point_quat.y();
point.pose.orientation.z = point_quat.z();
point.pose.orientation.w = point_quat.w();
if(counter%2 == 0)
plan.push_back(point);
before = point;
}
plan.push_back(goal);
}
//update what map cells are considered path based on the global_plan
void MapGrid::setPathCells(const costmap_2d::Costmap2D& costmap, const std::vector<geometry_msgs::PoseStamped>& global_plan){
sizeCheck(costmap.getSizeInCellsX(), costmap.getSizeInCellsY(), costmap.getOriginX(), costmap.getOriginY());
int local_goal_x = -1;
int local_goal_y = -1;
bool started_path = false;
queue<MapCell*> path_dist_queue;
queue<MapCell*> goal_dist_queue;
for(unsigned int i = 0; i < global_plan.size(); ++i){
double g_x = global_plan[i].pose.position.x;
double g_y = global_plan[i].pose.position.y;
unsigned int map_x, map_y;
if(costmap.worldToMap(g_x, g_y, map_x, map_y) && costmap.getCost(map_x, map_y) != costmap_2d::NO_INFORMATION){
MapCell& current = getCell(map_x, map_y);
current.path_dist = 0.0;
current.path_mark = true;
path_dist_queue.push(¤t);
local_goal_x = map_x;
local_goal_y = map_y;
started_path = true;
}
else{
if(started_path)
break;
}
}
if(local_goal_x >= 0 && local_goal_y >= 0){
MapCell& current = getCell(local_goal_x, local_goal_y);
costmap.mapToWorld(local_goal_x, local_goal_y, goal_x_, goal_y_);
current.goal_dist = 0.0;
current.goal_mark = true;
goal_dist_queue.push(¤t);
}
//compute our distances
computePathDistance(path_dist_queue, costmap);
computeGoalDistance(goal_dist_queue, costmap);
}
/**
* get the cellsof a footprint at a given position
*/
std::vector<upo_navigation::Position2DInt> FootprintHelper::getFootprintCells(
Eigen::Vector3f pos,
std::vector<geometry_msgs::Point> footprint_spec,
const costmap_2d::Costmap2D& costmap,
bool fill){
double x_i = pos[0];
double y_i = pos[1];
double theta_i = pos[2];
std::vector<upo_navigation::Position2DInt> footprint_cells;
//std::vector<geometry_msgs::Point> footprint_spec_ = costmap_ros_->getRobotFootprint();
//if we have no footprint... do nothing
if (footprint_spec.size() <= 1) {
unsigned int mx, my;
if (costmap.worldToMap(x_i, y_i, mx, my)) {
upo_navigation::Position2DInt center;
center.x = mx;
center.y = my;
footprint_cells.push_back(center);
}
return footprint_cells;
}
//pre-compute cos and sin values
double cos_th = cos(theta_i);
double sin_th = sin(theta_i);
double new_x, new_y;
unsigned int x0, y0, x1, y1;
unsigned int last_index = footprint_spec.size() - 1;
for (unsigned int i = 0; i < last_index; ++i) {
//find the cell coordinates of the first segment point
new_x = x_i + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
new_y = y_i + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
if(!costmap.worldToMap(new_x, new_y, x0, y0)) {
return footprint_cells;
}
//find the cell coordinates of the second segment point
new_x = x_i + (footprint_spec[i + 1].x * cos_th - footprint_spec[i + 1].y * sin_th);
new_y = y_i + (footprint_spec[i + 1].x * sin_th + footprint_spec[i + 1].y * cos_th);
if (!costmap.worldToMap(new_x, new_y, x1, y1)) {
return footprint_cells;
}
getLineCells(x0, x1, y0, y1, footprint_cells);
}
//we need to close the loop, so we also have to raytrace from the last pt to first pt
new_x = x_i + (footprint_spec[last_index].x * cos_th - footprint_spec[last_index].y * sin_th);
new_y = y_i + (footprint_spec[last_index].x * sin_th + footprint_spec[last_index].y * cos_th);
if (!costmap.worldToMap(new_x, new_y, x0, y0)) {
return footprint_cells;
}
new_x = x_i + (footprint_spec[0].x * cos_th - footprint_spec[0].y * sin_th);
new_y = y_i + (footprint_spec[0].x * sin_th + footprint_spec[0].y * cos_th);
if(!costmap.worldToMap(new_x, new_y, x1, y1)) {
return footprint_cells;
}
getLineCells(x0, x1, y0, y1, footprint_cells);
if(fill) {
getFillCells(footprint_cells);
}
return footprint_cells;
}
bool TableApproaches::tableApprCallback(hrl_table_detect::GetTableApproaches::Request& req,
hrl_table_detect::GetTableApproaches::Response& resp) {
double pose_step = 0.01, start_dist = 0.25, max_dist = 1.2, min_cost = 250;
double close_thresh = 0.10;
// TODO should this be transformed?
std::vector<geometry_msgs::Point> table_poly = req.table.points;
geometry_msgs::PointStamped approach_pt = req.approach_pt;
/*
double xsize = 1.0, ysize = 1.0, xoff = 2.5, yoff = 0.0;
geometry_msgs::Point pt;
pt.x = xoff-xsize/2; pt.y = yoff-ysize/2;
table_poly.push_back(pt);
pt.x = xoff+xsize/2; pt.y = yoff-ysize/2;
table_poly.push_back(pt);
pt.x = xoff+xsize/2; pt.y = yoff+ysize/2;
table_poly.push_back(pt);
pt.x = xoff-xsize/2; pt.y = yoff+ysize/2;
table_poly.push_back(pt);
geometry_msgs::PointStamped approach_pt;
approach_pt.header.frame_id = "/base_link";
approach_pt.header.stamp = ros::Time::now();
approach_pt.point.x = 2.2; approach_pt.point.y = 0.3;
*/
costmap_ros->getCostmapCopy(costmap);
world_model = new base_local_planner::CostmapModel(costmap);
geometry_msgs::PoseArray valid_locs;
valid_locs.header.frame_id = "/base_link";
valid_locs.header.stamp = ros::Time::now();
geometry_msgs::PoseArray invalid_locs;
invalid_locs.header.frame_id = "/base_link";
invalid_locs.header.stamp = ros::Time::now();
geometry_msgs::PoseArray close_locs;
close_locs.header.frame_id = "/base_link";
close_locs.header.stamp = ros::Time::now();
for(int i=0;i<4000;i++) {
geometry_msgs::PoseStamped cpose, odom_pose;
cpose.header.frame_id = "/base_link";
cpose.header.stamp = ros::Time(0);
cpose.pose.position.x = (rand()%8000) / 1000.0 -4 ;
cpose.pose.position.y = (rand()%8000) / 1000.0 - 4;
double rot = (rand()%10000) / 10000.0 * 2 * 3.14;
cpose.pose.orientation = tf::createQuaternionMsgFromYaw(rot);
cout << cpose.pose.orientation.z << " " << cpose.pose.orientation.w << " " << rot << endl;
tf_listener.transformPose("/odom_combined", cpose, odom_pose);
//uint32_t x, y;
//if(!costmap.worldToMap(odom_pose.pose.position.x, odom_pose.pose.position.y, x, y))
//continue;
Eigen::Vector3f pos(odom_pose.pose.position.x, odom_pose.pose.position.y, tf::getYaw(odom_pose.pose.orientation));
//cout << x << ", " << y << ":" << curpt.point.x << "," << curpt.point.y << "$";
//cout << double(costmap.getCost(x,y)) << endl;
double foot_cost = footprintCost(pos, 1);
if(foot_cost == 0)
valid_locs.poses.push_back(cpose.pose);
else if(foot_cost != -1)
close_locs.poses.push_back(cpose.pose);
else
invalid_locs.poses.push_back(cpose.pose);
cout << foot_cost << endl;
}
cout << costmap_ros->getRobotFootprint().size() << endl;
valid_pub.publish(valid_locs);
invalid_pub.publish(invalid_locs);
close_pub.publish(close_locs);
geometry_msgs::PoseArray dense_table_poses;
dense_table_poses.header.frame_id = "/base_link";
dense_table_poses.header.stamp = ros::Time::now();
uint32_t i2;
for(uint32_t i=0;i<table_poly.size();i++) {
i2 = i+1;
if(i2 == table_poly.size())
i2 = 0;
double diffx = table_poly[i2].x-table_poly[i].x;
double diffy = table_poly[i2].y-table_poly[i].y;
double len = std::sqrt(diffx*diffx + diffy*diffy);
double ang = std::atan2(diffy, diffx) - 3.14/2;
double incx = std::cos(ang)*0.01, incy = std::sin(ang)*0.01;
for(double t=0;t<len;t+=pose_step) {
double polyx = table_poly[i].x + t*diffx;
double polyy = table_poly[i].y + t*diffy;
geometry_msgs::PoseStamped test_pose, odom_test_pose;
bool found_pose = false;
for(int k=start_dist/0.01;k<max_dist/0.01;k++) {
test_pose.header.frame_id = "/base_link";
test_pose.header.stamp = ros::Time(0);
test_pose.pose.position.x = polyx + incx*k;
test_pose.pose.position.y = polyy + incy*k;
test_pose.pose.orientation = tf::createQuaternionMsgFromYaw(ang+3.14);
tf_listener.transformPose("/odom_combined", test_pose, odom_test_pose);
Eigen::Vector3f pos(odom_test_pose.pose.position.x,
odom_test_pose.pose.position.y,
tf::getYaw(odom_test_pose.pose.orientation));
double foot_cost = footprintCost(pos, 1.0);
// found a valid pose
if(foot_cost >= 0 && foot_cost <= min_cost) {
found_pose = true;
break;
}
uint32_t mapx, mapy;
// break if we go outside the grid
if(!costmap.worldToMap(odom_test_pose.pose.position.x,
odom_test_pose.pose.position.y, mapx, mapy))
break;
double occ_map = double(costmap.getCost(mapx, mapy));
// break if we come across and obstacle
if(occ_map == costmap_2d::LETHAL_OBSTACLE ||
occ_map == costmap_2d::NO_INFORMATION)
break;
}
if(found_pose)
dense_table_poses.poses.push_back(test_pose.pose);
}
}
ROS_INFO("POLY: %d, denseposes: %d", table_poly.size(), dense_table_poses.poses.size());
// downsample and sort dense pose possibilties by distance to
// approach point and thresholded distance to each other
geometry_msgs::PoseArray downsampled_table_poses;
boost::function<bool(geometry_msgs::Pose&, geometry_msgs::Pose&)> dist_comp
= boost::bind(&pose_dist_comp, _1, _2, approach_pt.point);
while(ros::ok() && !dense_table_poses.poses.empty()) {
// add the closest valid pose to the approach location on the table
geometry_msgs::Pose new_pose = *std::min_element(
dense_table_poses.poses.begin(), dense_table_poses.poses.end(),
dist_comp);
downsampled_table_poses.poses.push_back(new_pose);
// remove all poses in the dense sampling which are close to
// the newest added pose
boost::function<bool(geometry_msgs::Pose)> rem_thresh
= boost::bind(&pose_dist_thresh, _1, new_pose.position,
close_thresh);
dense_table_poses.poses.erase(std::remove_if(
dense_table_poses.poses.begin(),
dense_table_poses.poses.end(),
rem_thresh),
dense_table_poses.poses.end());
ROS_INFO("denseposes: %d", dense_table_poses.poses.size());
}
downsampled_table_poses.header.frame_id = "/base_link";
downsampled_table_poses.header.stamp = ros::Time::now();
approach_pub.publish(downsampled_table_poses);
resp.approach_poses = downsampled_table_poses;
ROS_INFO("POLY: %d, poses: %d", table_poly.size(), downsampled_table_poses.poses.size());
delete world_model;
return true;
}
void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
if (!enabled_ || (cell_inflation_radius_ == 0))
return;
// make sure the inflation list is empty at the beginning of the cycle (should always be true)
ROS_ASSERT_MSG(inflation_cells_.empty(), "The inflation list must be empty at the beginning of inflation");
unsigned char* master_array = master_grid.getCharMap();
unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();
if (seen_ == NULL) {
ROS_WARN("InflationLayer::updateCosts(): seen_ array is NULL");
seen_size_ = size_x * size_y;
seen_ = new bool[seen_size_];
}
else if (seen_size_ != size_x * size_y)
{
ROS_WARN("InflationLayer::updateCosts(): seen_ array size is wrong");
delete[] seen_;
seen_size_ = size_x * size_y;
seen_ = new bool[seen_size_];
}
memset(seen_, false, size_x * size_y * sizeof(bool));
// We need to include in the inflation cells outside the bounding
// box min_i...max_j, by the amount cell_inflation_radius_. Cells
// up to that distance outside the box can still influence the costs
// stored in cells inside the box.
min_i -= cell_inflation_radius_;
min_j -= cell_inflation_radius_;
max_i += cell_inflation_radius_;
max_j += cell_inflation_radius_;
min_i = std::max(0, min_i);
min_j = std::max(0, min_j);
max_i = std::min(int(size_x), max_i);
max_j = std::min(int(size_y), max_j);
// Inflation list; we append cells to visit in a list associated with its distance to the nearest obstacle
// We use a map<distance, list> to emulate the priority queue used before, with a notable performance boost
// Start with lethal obstacles: by definition distance is 0.0
std::vector<CellData>& obs_bin = inflation_cells_[0.0];
for (int j = min_j; j < max_j; j++)
{
for (int i = min_i; i < max_i; i++)
{
int index = master_grid.getIndex(i, j);
unsigned char cost = master_array[index];
if (cost == LETHAL_OBSTACLE)
{
obs_bin.push_back(CellData(index, i, j, i, j));
}
}
}
// Process cells by increasing distance; new cells are appended to the corresponding distance bin, so they
// can overtake previously inserted but farther away cells
std::map<double, std::vector<CellData> >::iterator bin;
for (bin = inflation_cells_.begin(); bin != inflation_cells_.end(); ++bin)
{
for (int i = 0; i < bin->second.size(); ++i)
{
// process all cells at distance dist_bin.first
const CellData& cell = bin->second[i];
unsigned int index = cell.index_;
// ignore if already visited
if (seen_[index])
{
continue;
}
seen_[index] = true;
unsigned int mx = cell.x_;
unsigned int my = cell.y_;
unsigned int sx = cell.src_x_;
unsigned int sy = cell.src_y_;
// assign the cost associated with the distance from an obstacle to the cell
unsigned char cost = costLookup(mx, my, sx, sy);
unsigned char old_cost = master_array[index];
if (old_cost == NO_INFORMATION && (inflate_unknown_ ? (cost > FREE_SPACE) : (cost >= INSCRIBED_INFLATED_OBSTACLE)))
master_array[index] = cost;
else
master_array[index] = std::max(old_cost, cost);
// attempt to put the neighbors of the current cell onto the inflation list
if (mx > 0)
enqueue(index - 1, mx - 1, my, sx, sy);
if (my > 0)
enqueue(index - size_x, mx, my - 1, sx, sy);
if (mx < size_x - 1)
enqueue(index + 1, mx + 1, my, sx, sy);
if (my < size_y - 1)
enqueue(index + size_x, mx, my + 1, sx, sy);
}
}
inflation_cells_.clear();
}
void FootprintLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
if(!enabled_) return;
std::vector<geometry_msgs::Point> footprint_points = costmap_2d::toPointVector(footprint_.polygon);
master_grid.setConvexPolygonCost(footprint_points, costmap_2d::FREE_SPACE);
}
// method for applyting changes in this layer to global costmap
void HANPLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j,
int max_i, int max_j)
{
boost::recursive_mutex::scoped_lock lock(configuration_mutex_);
if(!enabled_ || (lastTransformedHumans.size() == 0))
{
return;
}
// update map according to each human position
for(auto human : lastTransformedHumans)
{
// get position of human in map
unsigned int cell_x, cell_y;
if(!master_grid.worldToMap(human.pose.position.x, human.pose.position.y, cell_x, cell_y))
{
ROS_ERROR("hanp_layer: no world coordinates for human at x:%f y:%f",
human.pose.position.x, human.pose.position.y);
continue;
}
// general algorithm is to
// create satic grids according to human posture, standing, sitting, walking
// to calcuate cost use a sigmoid function
// however 'sitting' posture is not implemented yet
if(use_safety)
{
if(safety_grid == NULL)
{
ROS_WARN_THROTTLE(THROTTLE_TIME, "hanp_layer: not updating costmap as safety_grid is empty");
return;
}
// apply safety grid according to position of each human
auto size_x = (int)(safety_max / resolution), size_y = size_x;
// by convention x is number of columns, y is number of rows
for(int y = 0; y <= 2 * size_y; y++)
{
for(int x = 0; x <= 2 * size_x; x++)
{
// add to current cost
auto cost = (double)master_grid.getCost(x + cell_x - size_x, y + cell_y - size_y)
+ (double)(safety_grid[x + ((2 * size_x + 1) * y)] * safety_weight);
// detect overflow
if(cost > (int)costmap_2d::LETHAL_OBSTACLE)
{
cost = costmap_2d::LETHAL_OBSTACLE;
}
master_grid.setCost(x + cell_x - size_x, y + cell_y - size_y, (unsigned int)cost);
}
}
}
if(use_visibility)
{
// calculate visibility grid
double yaw = tf::getYaw(human.pose.orientation);
auto visibility_grid = createVisibilityGrid(visibility_max, resolution,
costmap_2d::LETHAL_OBSTACLE, cos(yaw), sin(yaw));
if(visibility_grid == NULL)
{
ROS_WARN_THROTTLE(THROTTLE_TIME, "hanp_layer: not updating costmap as visibility_grid is empty");
return;
}
// apply the visibility grid
auto size_x = (int)(visibility_max / resolution), size_y = size_x;
for(int y = 0; y <= 2 * size_y; y++)
{
for(int x = 0; x <= 2 * size_x; x++)
{
// add to current cost
auto cost = (double)master_grid.getCost(x + cell_x - size_x, y + cell_y - size_y)
+ (double)(visibility_grid[x + ((2 * size_x + 1) * y)] * visibility_weight);
// detect overflow
if(cost > (int)costmap_2d::LETHAL_OBSTACLE)
{
cost = costmap_2d::LETHAL_OBSTACLE;
}
master_grid.setCost(x + cell_x - size_x, y + cell_y - size_y, (unsigned int)cost);
}
}
}
}
// ROS_DEBUG("hanp_layer: updated costs");
}
void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i,
int max_j)
{
boost::unique_lock < boost::shared_mutex > lock(*access_);
if (!enabled_)
return;
//make sure the inflation queue is empty at the beginning of the cycle (should always be true)
ROS_ASSERT_MSG(inflation_queue_.empty(), "The inflation queue must be empty at the beginning of inflation");
unsigned char* master_array = master_grid.getCharMap();
unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();
memset(seen_, false, size_x * size_y * sizeof(bool));
// We need to include in the inflation cells outside the bounding
// box min_i...max_j, by the amount cell_inflation_radius_. Cells
// up to that distance outside the box can still influence the costs
// stored in cells inside the box.
min_i -= cell_inflation_radius_;
min_j -= cell_inflation_radius_;
max_i += cell_inflation_radius_;
max_j += cell_inflation_radius_;
min_i = std::max( 0, min_i );
min_j = std::max( 0, min_j );
max_i = std::min( int( size_x ), max_i );
max_j = std::min( int( size_y ), max_j );
for (int j = min_j; j < max_j; j++)
{
for (int i = min_i; i < max_i; i++)
{
int index = master_grid.getIndex(i, j);
unsigned char cost = master_array[index];
if (cost == LETHAL_OBSTACLE)
{
enqueue(master_array, index, i, j, i, j);
}
}
}
while (!inflation_queue_.empty())
{
//get the highest priority cell and pop it off the priority queue
const CellData& current_cell = inflation_queue_.top();
unsigned int index = current_cell.index_;
unsigned int mx = current_cell.x_;
unsigned int my = current_cell.y_;
unsigned int sx = current_cell.src_x_;
unsigned int sy = current_cell.src_y_;
//attempt to put the neighbors of the current cell onto the queue
if (mx > 0)
enqueue(master_array, index - 1, mx - 1, my, sx, sy);
if (my > 0)
enqueue(master_array, index - size_x, mx, my - 1, sx, sy);
if (mx < size_x - 1)
enqueue(master_array, index + 1, mx + 1, my, sx, sy);
if (my < size_y - 1)
enqueue(master_array, index + size_x, mx, my + 1, sx, sy);
//remove the current cell from the priority queue
inflation_queue_.pop();
}
}
bool transformGlobalPlan(
const tf::TransformListener& tf,
const std::vector<geometry_msgs::PoseStamped>& global_plan,
const tf::Stamped<tf::Pose>& global_pose,
const costmap_2d::Costmap2D& costmap,
const std::string& global_frame,
std::vector<geometry_msgs::PoseStamped>& transformed_plan){
const geometry_msgs::PoseStamped& plan_pose = global_plan[0];
transformed_plan.clear();
try {
if (!global_plan.size() > 0) {
ROS_ERROR("Received plan with zero length");
return false;
}
// get plan_to_global_transform from plan frame to global_frame
tf::StampedTransform plan_to_global_transform;
tf.waitForTransform(global_frame, ros::Time::now(),
plan_pose.header.frame_id, plan_pose.header.stamp,
plan_pose.header.frame_id, ros::Duration(0.5));
tf.lookupTransform(global_frame, ros::Time(),
plan_pose.header.frame_id, plan_pose.header.stamp,
plan_pose.header.frame_id, plan_to_global_transform);
//let's get the pose of the robot in the frame of the plan
tf::Stamped<tf::Pose> robot_pose;
tf.transformPose(plan_pose.header.frame_id, global_pose, robot_pose);
//we'll discard points on the plan that are outside the local costmap
double dist_threshold = std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0,
costmap.getSizeInCellsY() * costmap.getResolution() / 2.0);
unsigned int i = 0;
double sq_dist_threshold = dist_threshold * dist_threshold;
double sq_dist = 0;
//we need to loop to a point on the plan that is within a certain distance of the robot
while(i < (unsigned int)global_plan.size()) {
double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
sq_dist = x_diff * x_diff + y_diff * y_diff;
if (sq_dist <= sq_dist_threshold) {
break;
}
++i;
}
tf::Stamped<tf::Pose> tf_pose;
geometry_msgs::PoseStamped newer_pose;
//now we'll transform until points are outside of our distance threshold
while(i < (unsigned int)global_plan.size() && sq_dist <= sq_dist_threshold) {
const geometry_msgs::PoseStamped& pose = global_plan[i];
poseStampedMsgToTF(pose, tf_pose);
tf_pose.setData(plan_to_global_transform * tf_pose);
tf_pose.stamp_ = plan_to_global_transform.stamp_;
tf_pose.frame_id_ = global_frame;
poseStampedTFToMsg(tf_pose, newer_pose);
transformed_plan.push_back(newer_pose);
double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
sq_dist = x_diff * x_diff + y_diff * y_diff;
++i;
}
}
catch(tf::LookupException& ex) {
ROS_ERROR("No Transform available Error: %s\n", ex.what());
return false;
}
catch(tf::ConnectivityException& ex) {
ROS_ERROR("Connectivity Error: %s\n", ex.what());
return false;
}
catch(tf::ExtrapolationException& ex) {
ROS_ERROR("Extrapolation Error: %s\n", ex.what());
if (global_plan.size() > 0)
ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str());
return false;
}
return true;
}
void getMin(costmap_2d::Costmap2D gradient_, int &i, int &j)
{
int iOut = i;
int jOut = j;
if(gradient_.getCost( i-1, j-1) < gradient_.getCost( iOut, jOut))
{
iOut = i - 1;
jOut = j - 1;
}
if(gradient_.getCost( i-1, j) < gradient_.getCost( iOut, jOut))
{
iOut = i - 1;
jOut = j;
}
if(gradient_.getCost( i-1, j+1) < gradient_.getCost( iOut, jOut))
{
iOut = i - 1;
jOut = j + 1;
}
if(gradient_.getCost( i, j-1) < gradient_.getCost( iOut, jOut))
{
iOut = i;
jOut = j - 1;
}
if(gradient_.getCost( i, j+1) < gradient_.getCost( iOut, jOut))
{
iOut = i;
jOut = j + 1;
}
if(gradient_.getCost( i+1, j-1) < gradient_.getCost( iOut, jOut))
{
iOut = i + 1;
jOut = j - 1;
}
if(gradient_.getCost( i+1, j) < gradient_.getCost( iOut, jOut))
{
iOut = i + 1;
jOut = j;
}
if(gradient_.getCost( i+1, j+1) < gradient_.getCost( iOut, jOut))
{
iOut = i + 1;
jOut = j + 1;
}
i = iOut;
j = jOut;
}
double TableApproaches::footprintCost(const Eigen::Vector3f& pos, double scale){
double cos_th = cos(pos[2]);
double sin_th = sin(pos[2]);
std::vector<geometry_msgs::Point> scaled_oriented_footprint;
for(unsigned int i = 0; i < footprint_model.size(); ++i){
geometry_msgs::Point new_pt;
new_pt.x = pos[0] + (scale * footprint_model[i].x * cos_th - scale * footprint_model[i].y * sin_th);
new_pt.y = pos[1] + (scale * footprint_model[i].x * sin_th + scale * footprint_model[i].y * cos_th);
scaled_oriented_footprint.push_back(new_pt);
}
geometry_msgs::Point robot_position;
robot_position.x = pos[0];
robot_position.y = pos[1];
//check if the footprint is legal
double footprint_cost = world_model->footprintCost(robot_position, scaled_oriented_footprint, costmap.getInscribedRadius(), costmap.getCircumscribedRadius());
return footprint_cost;
}