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;
	  }
}
Exemple #2
0
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);
  }
}
Exemple #3
0
//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 (&current);
    }

    computeTargetDistance (path_dist_queue, costmap);
}
Exemple #4
0
//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 (&current);
            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;
}
Exemple #6
0
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(&current);
        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(&current);
    }
    //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;
}
Exemple #10
0
    // 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");
    }
Exemple #11
0
    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;
    }