Exemplo n.º 1
0
	void XVMatrix::setValues(RTT::types::carray<double> & inputs)
	{
		if (sizeCheck(inputs.count()))
		{
			memcpy(mat, inputs.address(), size() * sizeof(double));
		}
		else
		{
			std::__throw_out_of_range(__N("XVMatrix::_M_range_check"));
		}
	}
Exemplo n.º 2
0
	void XVMatrix::setValues(double & inputs, std::size_t size)
	{
		if (sizeCheck(size))
		{
			memcpy(mat, &inputs, this->size() * sizeof(double));
		}
		else
		{
			std::__throw_out_of_range(__N("XVMatrix::_M_range_check"));
		}
	}
Exemplo n.º 3
0
	void XVMatrix::setValues(std::vector<double> & inputs)
	{
		if (sizeCheck(inputs.size()))
		{
			memcpy(mat, inputs.data(), size() * sizeof(double));
		}
		else
		{
			std::__throw_out_of_range(__N("XVMatrix::_M_range_check"));
		}
	}
Exemplo n.º 4
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.º 5
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);
}
Exemplo n.º 6
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);
  }