src/costmap_prohibition_layer.cpp
bool CostmapProhibitionLayer::parseProhibitionListFromYaml(ros::NodeHandle *nhandle, const std::string &param)
{
  std::lock_guard<std::mutex> l(_data_mutex);
  std::unordered_map<std::string, geometry_msgs::Pose> map_out;

  XmlRpc::XmlRpcValue param_yaml;

  bool ret_val = true;

  if (nhandle->getParam(param, param_yaml))
  {
    if (param_yaml.getType() == XmlRpc::XmlRpcValue::TypeArray) // list of goals
    {
      std::vector<std::vector<geometry_msgs::Point>> empty_zones;
      auto p = _prohibition_polygons.insert(_prohibition_polygons.begin(), std::pair<std::string, std::vector<std::vector<geometry_msgs::Point>>>("init", empty_zones));
      for (int i = 0; i < param_yaml.size(); ++i)
      {
        if (param_yaml[i].getType() == XmlRpc::XmlRpcValue::TypeArray)
        {
          std::vector<geometry_msgs::Point> vector_to_add;

          /* **************************************
           * differ between points and polygons
           * lines get to a polygon with the resolution
           * of the costmap
           **************************************** */

          // add a point
          if (param_yaml[i].size() == 1)
          {
            geometry_msgs::Point point;
            ret_val = getPoint(param_yaml[i][0], point);
            _prohibition_points.push_back(point);
          }
          // add a line
          else if (param_yaml[i].size() == 2)
          {
            if (param_yaml[i][0].getType() == XmlRpc::XmlRpcValue::TypeDouble ||
                param_yaml[i][0].getType() == XmlRpc::XmlRpcValue::TypeInt)
            {
              // add a lonely point
              geometry_msgs::Point point;
              ret_val = getPoint(param_yaml[i], point);
              _prohibition_points.push_back(point);
            }
            else
            {
              // add a line!
              geometry_msgs::Point point_A;
              ret_val = getPoint(param_yaml[i][0], point_A);
              vector_to_add.push_back(point_A);

              geometry_msgs::Point point_B;
              ret_val = getPoint(param_yaml[i][1], point_B);
              vector_to_add.push_back(point_B);

              // calculate the normal vector for AB
              geometry_msgs::Point point_N;
              point_N.x = point_B.y - point_A.y;
              point_N.y = point_A.x - point_B.x;

              // get the absolute value of N to normalize and get
              // it to the length of the costmap resolution
              double abs_N = sqrt(pow(point_N.x, 2) + pow(point_N.y, 2));
              point_N.x = point_N.x / abs_N * _costmap_resolution;
              point_N.y = point_N.y / abs_N * _costmap_resolution;

              // calculate the new points to get a polygon which can be filled
              geometry_msgs::Point point;
              point.x = point_A.x + point_N.x;
              point.y = point_A.y + point_N.y;
              vector_to_add.push_back(point);

              point.x = point_B.x + point_N.x;
              point.y = point_B.y + point_N.y;
              vector_to_add.push_back(point);

              p->second.push_back(vector_to_add);
            }
          }
          // add a point or add a polygon
          else if (param_yaml[i].size() >= 3)
          {
            // add a polygon with any number of points
            for (int j = 0; j < param_yaml[i].size(); ++j)
            {
              geometry_msgs::Point point;
              ret_val = getPoint(param_yaml[i][j], point);
              vector_to_add.push_back(point);
            }
            p->second.push_back(vector_to_add);
          }
        }
        else
        {
          ROS_ERROR_STREAM("Prohibition Layer:" << param << " with index " << i << " is not correct.");
          ret_val = false;
        }
      }
      if (p->second.size() == 0) {
        ROS_INFO("nothing added from yaml file");
        _prohibition_polygons.erase(p);
      }
    }
    else
    {
      ROS_ERROR_STREAM("Prohibition Layer: " << param << "struct is not correct.");
      ret_val = false;
    }
  }
  else
  {
    ROS_ERROR_STREAM("Prohibition Layer: Cannot read " << param << " from parameter server");
    ret_val = false;
  }
  return ret_val;
}
// load prohibition positions out of the rosparam server
load prohibition positions out of the rosparam server
load prohibition positions out of the rosparam server
bool CostmapProhibitionLayer::parseProhibitionListFromYaml(ros::NodeHandle *nhandle, const std::string &param)
{
  std::lock_guard<std::mutex> l(_data_mutex);
  std::unordered_map<std::string, geometry_msgs::Pose> map_out;
  XmlRpc::XmlRpcValue param_yaml;
  bool ret_val = true;
  if (nhandle->getParam(param, param_yaml))
  {
    if (param_yaml.getType() == XmlRpc::XmlRpcValue::TypeArray) 
    {
      std::vector<std::vector<geometry_msgs::Point>> empty_zones;
      auto p = _prohibition_polygons.insert(_prohibition_polygons.begin(), std::pair<std::string, std::vector<std::vector<geometry_msgs::Point>>>("init", empty_zones));
      for (int i = 0; i < param_yaml.size(); ++i)
      {
        if (param_yaml[i].getType() == XmlRpc::XmlRpcValue::TypeArray)
        {
          std::vector<geometry_msgs::Point> vector_to_add;
          if (param_yaml[i].size() == 1)
          {
            geometry_msgs::Point point;
            ret_val = getPoint(param_yaml[i][0], point);
            _prohibition_points.push_back(point);
          }
          else if (param_yaml[i].size() == 2)
          {
            if (param_yaml[i][0].getType() == XmlRpc::XmlRpcValue::TypeDouble ||
                param_yaml[i][0].getType() == XmlRpc::XmlRpcValue::TypeInt)
            {
              geometry_msgs::Point point;
              ret_val = getPoint(param_yaml[i], point);
              _prohibition_points.push_back(point);
            }
            else
            {
              !
              geometry_msgs::Point point_A;
              ret_val = getPoint(param_yaml[i][0], point_A);
              vector_to_add.push_back(point_A);
              geometry_msgs::Point point_B;
              ret_val = getPoint(param_yaml[i][1], point_B);
              vector_to_add.push_back(point_B);
              geometry_msgs::Point point_N;
              point_N.x = point_B.y - point_A.y;
              point_N.y = point_A.x - point_B.x;
              double abs_N = sqrt(pow(point_N.x, 2) + pow(point_N.y, 2));
              point_N.x = point_N.x / abs_N * _costmap_resolution;
              point_N.y = point_N.y / abs_N * _costmap_resolution;
              geometry_msgs::Point point;
              point.x = point_A.x + point_N.x;
              point.y = point_A.y + point_N.y;
              vector_to_add.push_back(point);
              point.x = point_B.x + point_N.x;
              point.y = point_B.y + point_N.y;
              vector_to_add.push_back(point);
              p->second.push_back(vector_to_add);
            }
          }
           or add a polygon
          else if (param_yaml[i].size() >= 3)
          {
            for (int j = 0; j < param_yaml[i].size(); ++j)
            {
              geometry_msgs::Point point;
              ret_val = getPoint(param_yaml[i][j], point);
              vector_to_add.push_back(point);
            }
            p->second.push_back(vector_to_add);
          }
        }
        else
        {
          ROS_ERROR_STREAM("Prohibition Layer:" << param << " with index " << i << " is not correct.");
          ret_val = false;
        }
      }
      if (p->second.size() == 0) {
        ROS_INFO("nothing added from yaml file");
        _prohibition_polygons.erase(p);
      }
    }
    else
    {
      ROS_ERROR_STREAM("Prohibition Layer: " << param << "struct is not correct.");
      ret_val = false;
    }
  }
  else
  {
    ROS_ERROR_STREAM("Prohibition Layer: Cannot read " << param << " from parameter server");
    ret_val = false;
  }
  return ret_val;
}
bool CostmapProhibitionLayer :: parseProhibitionListFromYaml ( ros :: NodeHandle * nhandle , const std :: string & param ) { std :: lock_guard < std :: mutex > l ( _data_mutex ) ; std :: unordered_map < std :: string , geometry_msgs :: Pose > map_out ; XmlRpc :: XmlRpcValue param_yaml ; bool ret_val = true ; if ( nhandle -> getParam ( param , param_yaml ) ) { if ( param_yaml . getType ( ) == XmlRpc :: XmlRpcValue :: TypeArray ) { std :: vector < std :: vector < geometry_msgs :: Point > > empty_zones ; auto p = _prohibition_polygons . insert ( _prohibition_polygons . begin ( ) , std :: pair < std :: string , std :: vector < std :: vector < geometry_msgs :: Point > > > ( " " , empty_zones ) ) ; for ( int i = 0 ; i < param_yaml . size ( ) ; ++ i ) { if ( param_yaml [ i ] . getType ( ) == XmlRpc :: XmlRpcValue :: TypeArray ) { std :: vector < geometry_msgs :: Point > vector_to_add ; if ( param_yaml [ i ] . size ( ) == 1 ) { geometry_msgs :: Point point ; ret_val = getPoint ( param_yaml [ i ] [ 0 ] , point ) ; _prohibition_points . push_back ( point ) ; } else if ( param_yaml [ i ] . size ( ) == 2 ) { if ( param_yaml [ i ] [ 0 ] . getType ( ) == XmlRpc :: XmlRpcValue :: TypeDouble || param_yaml [ i ] [ 0 ] . getType ( ) == XmlRpc :: XmlRpcValue :: TypeInt ) { geometry_msgs :: Point point ; ret_val = getPoint ( param_yaml [ i ] , point ) ; _prohibition_points . push_back ( point ) ; } else { geometry_msgs :: Point point_A ; ret_val = getPoint ( param_yaml [ i ] [ 0 ] , point_A ) ; vector_to_add . push_back ( point_A ) ; geometry_msgs :: Point point_B ; ret_val = getPoint ( param_yaml [ i ] [ 1 ] , point_B ) ; vector_to_add . push_back ( point_B ) ; geometry_msgs :: Point point_N ; point_N . x = point_B . y - point_A . y ; point_N . y = point_A . x - point_B . x ; double abs_N = sqrt ( pow ( point_N . x , 2 ) + pow ( point_N . y , 2 ) ) ; point_N . x = point_N . x / abs_N * _costmap_resolution ; point_N . y = point_N . y / abs_N * _costmap_resolution ; geometry_msgs :: Point point ; point . x = point_A . x + point_N . x ; point . y = point_A . y + point_N . y ; vector_to_add . push_back ( point ) ; point . x = point_B . x + point_N . x ; point . y = point_B . y + point_N . y ; vector_to_add . push_back ( point ) ; p -> second . push_back ( vector_to_add ) ; } } else if ( param_yaml [ i ] . size ( ) >= 3 ) { for ( int j = 0 ; j < param_yaml [ i ] . size ( ) ; ++ j ) { geometry_msgs :: Point point ; ret_val = getPoint ( param_yaml [ i ] [ j ] , point ) ; vector_to_add . push_back ( point ) ; } p -> second . push_back ( vector_to_add ) ; } } else { ROS_ERROR_STREAM ( " " << param << " " << i << " " ) ; ret_val = false ; } } if ( p -> second . size ( ) == 0 ) { ROS_INFO ( " " ) ; _prohibition_polygons . erase ( p ) ; } } else { ROS_ERROR_STREAM ( " " << param << " " ) ; ret_val = false ; } } else { ROS_ERROR_STREAM ( " " << param << " " ) ; ret_val = false ; } return ret_val ; }
load prohibition positions out of the rosparam server
load prohibition positions out of the rosparam server
// list of goals /* **************************************
           * differ between points and polygons
           * lines get to a polygon with the resolution
           * of the costmap
           **************************************** */ // add a point // add a line // add a lonely point // add a line! // calculate the normal vector for AB // get the absolute value of N to normalize and get // it to the length of the costmap resolution // calculate the new points to get a polygon which can be filled // add a point or add a polygon // add a polygon with any number of points