klemense1
commented
3 years ago why not use Polygon's function double Distance(const Polygon& poly, const Point2d& p) and compare the road polygon and the point of the agent's rear axis?
Closed tinmodeHuang closed 3 years ago
klemense1
commented
3 years ago why not use Polygon's function double Distance(const Polygon& poly, const Point2d& p) and compare the road polygon and the point of the agent's rear axis?
tinmodeHuang
commented
3 years ago the function you mentioned was in use, for answer to the latter, the purpose is to mapping the distance with whether driving out of the road or not. Moreover, build successfully after the modification as shown below:
double Agent::GetDistance2RoadEdge() const {
boost::geometry::model::polygon<boost::geometry::model::d2::point_xy<double> > green;
boost::geometry::read_wkt(
"POLYGON((2 1.3,2.4 1.7,2.8 1.8,3.4 1.2,3.7 1.6,3.4 2,4.1 3,5.3 2.6,5.4 1.2,4.9 0.8,2.9 0.7,2 1.3))", green);
double tmp;
double distance = std::numeric_limits<double>::max();
Polygon agent_poly = GetPolygonFromState(GetCurrentState());
for (auto const& point : boost::make_iterator_range(
boost::geometry::exterior_ring(green))) {
tmp = Distance(agent_poly, point);
distance = tmp < distance ? tmp : distance;
}
return distance;
}it turns out that the type of road_corridor_->GetPolygon() don't fit into the type bg::geometry::exterior_ring() required.
patrickhart
commented
3 years ago For checking whether you are on the road or not have a look here: https://github.com/bark-simulator/bark/blob/master/bark/world/evaluation/evaluator_drivable_area.hpp
Evaluators in BARK are called at every time-step to evaluate certain criteria, such as whether the agent is on the road or not.
patrickhart
commented
3 years ago additionally, the LaneCorridor returns you the line strings of the lane, see here: https://github.com/bark-simulator/bark/blob/7ba226af803127adcf6f4f3fc6e1c49cecde6a33/bark/world/map/lane_corridor.hpp#L36
then you can use the Distance function as mentioned by @klemense1
tinmodeHuang
commented
3 years ago first of all, thanks for the advice. However, if do it as you say, it seems that I need to identify which linestring is edges of the road. the last modification as shown below:
double Agent::GetDistance2RoadEdge() const {
double tmp;
double distance = std::numeric_limits<double>::max();
Polygon agent_poly = GetPolygonFromState(GetCurrentState());
for (auto const& point : boost::make_iterator_range(
boost::geometry::exterior_ring(road_corridor_->GetPolygon().obj_))) {
tmp = Distance(agent_poly, point);
distance = tmp < distance ? tmp : distance;
}
return distance;
}type of the member variable .obj_ of road_corridor_->GetPolygon()just meets the requirement of boost::geometry::exterior_ring()to parameter. it seems that the calculated distance can exactly represents the distance between the roadedge and the ego agent. could you check if the snippet above does it?
patrickhart
commented
3 years ago first of all, thanks for the advice. However, if do it as you say, it seems that I need to identify which linestring is edges of the road.
No, this is done automatically by the map_interace as it returns always the, e.g., left or right lane from the current LaneCorridor.
the last modification as shown below:
double Agent::GetDistance2RoadEdge() const { double tmp; double distance = std::numeric_limits<double>::max(); Polygon agent_poly = GetPolygonFromState(GetCurrentState()); for (auto const& point : boost::make_iterator_range( boost::geometry::exterior_ring(road_corridor_->GetPolygon().obj_))) { tmp = Distance(agent_poly, point); distance = tmp < distance ? tmp : distance; } return distance; }type of the member variable
.obj_ofroad_corridor_->GetPolygon()just meets the requirement ofboost::geometry::exterior_ring()to parameter. it seems that the calculated distance can exactly represents the distance between the roadedge and the ego agent. could you check if the snippet above does it?
This could potentially also return die distance to the front since it is a closed polygon. So it does not purely provide the distance to the road edges/lines.
tinmodeHuang
commented
3 years ago you're right, it have also been taken into consideration. though all vehicles spawn at those places far from the ends of roadcorridor most of the time, there is little risk so long as other agents, even the exterior controlled one, don't take the distance as input. Moreover, the ego agent always spawn among the others. by the way, if the left lanestring from the current LaneCorridor is always returned, how to identify if that is the left boundary, the right one or even the middle lane markings from the current road. whether or not there is the easy way to get the identifier of lanes if lanes are arranged in some kind of order.
patrickhart
commented
3 years ago There are interfaces available in the lanecorridor that determine this based on the a graph based map representation (the RoadGraph)
tinmodeHuang
commented
3 years ago I'll have a try in the future.
my idea is to calculate distance between every point in a road egde and the agent and take the minimum of distances as the distance between the Agent and the road edge.
the snippet in /bark/world/objects/agent.hpp:
the one in /bark/world/objects/agent.cpp:
the one in /bark/python_wrapper/world/agent.cpp:
but the error was thrown out as running
bazel test //....according to my understanding, it causes the error that roadcorridor->GetPolygon() is not of the boost::geometry::model::polygon<boost::geometry::model::d2::point_xy> type.
I'll appreciate any suggestions you give!