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++;
    }
  }
}
Exemplo n.º 4
0
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);
		}
	}
}
Exemplo n.º 5
0
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;
}
Exemplo n.º 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(master_grid.worldToMap(mark_x_, mark_y_, mx, my)){
    master_grid.setCost(mx, my, LETHAL_OBSTACLE);
  }
}
Exemplo n.º 7
0
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)));
        }
      }
    }
  }
}
Exemplo n.º 9
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;
}
Exemplo n.º 11
0
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++;
    }
  }
}
Exemplo n.º 13
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!");
}
Exemplo n.º 14
0
  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);
     }

  }
Exemplo n.º 15
0
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);
}
Exemplo n.º 17
0
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]);
        }
    }
}
Exemplo n.º 18
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);
}
Exemplo n.º 19
0
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);


}
Exemplo n.º 20
0
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);



}
Exemplo n.º 21
0
  //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;
}
Exemplo n.º 23
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;
    }
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();
}
Exemplo n.º 25
0
 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);
 }
Exemplo n.º 26
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");
    }
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();
  }
}
Exemplo n.º 28
0
  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;
  }
Exemplo n.º 29
0
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;
}
Exemplo n.º 30
0
    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;
    }