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;
}
}
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;
}
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;
}
//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 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;
}
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);
}
//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);
}
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;
}
// 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");
}
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;
}