Commit 481bcbde authored by Uwe Woessner's avatar Uwe Woessner
Browse files

fixed position on road


Signed-off-by: Uwe Woessner's avatarhpcwoess <woessner@hlrs.de>
parent 9fb6c93f
......@@ -353,21 +353,11 @@ void Sensor_Modular_Driver_Implementation::SetMovingObjects(const std::map<int,
double sightdistance = GetWorld()->GetVisibilityDistance();
std::list<std::pair<int,int>> AgentAndDist;
//!!!!!!!!!!!!!!!!!!!!TODO check if this there is no easier and better way to do this
const auto &roadIds = GetAgent()->GetRoads(MeasurementPoint::Front);
if (roadIds.empty())
{
return;
}
double roadPos_s = GetAgent()->GetObjectPosition().mainLocatePoint.at(roadIds.front()).roadPosition.s;
for (auto it=Agents->begin(); it!=Agents->end(); it++)
double roadPos_s = GetAgent()->GetEgoAgent().GetMainLocatePosition().roadPosition.s;
for (std::map<int, AgentInterface *>::const_iterator it = Agents->begin(); it != Agents->end(); it++)
{
const auto &roadIds = GetAgent()->GetRoads(MeasurementPoint::Front);
if (!roadIds.empty())
{
double absdist2ego = abs(roadPos_s - it->second->GetObjectPosition().mainLocatePoint.at(roadIds.front()).roadPosition.s);
AgentAndDist.push_back(std::make_pair(absdist2ego, it->first));
}
double absdist2ego = abs(roadPos_s - it->second->GetEgoAgent().GetMainLocatePosition().roadPosition.s);
AgentAndDist.push_back(std::make_pair(absdist2ego, it->first));
}
AgentAndDist.sort();
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment