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