esmini
esmini copied to clipboard
esmini
V2X in esmini
Hi, I know that OSC is currently not including V2X, but we did it via mqtt messaging and user defined actions. I wanted to ask now some questions regarding this V2X topic:
- Do you have any V2X functionalities on your roadmap, if so how looks your time planning for this and how do you plan to integrate it?
- I wanted to use esmini in that case more as a library, not as a tool, this means where I can find your "main" function to have access to all triggers, actions, objects, ... (it is ScenarioEngine->step right?) to integrate such mqtt messaging (receive objects' position and overwrite esmini simulation data and also send positions of esmini objects)? So the question would be where in the code such changes should be done, I think you can answer it quite fast (seems ScenarioEngine->step is doing that all). Theoratically I could use osi_receiver_ip here and listen to OSI UDP messages to get all objects' position and so on, but this would work only in the direction getting data from esmini and would not mtach my triggers for such messaging, the opposite direction to overwrite esmini data would not work this way right? For the moment I see no other way for such extended functionality to customize the ScenarioEngine->step function.
- If I would integrate such V2X functionalities into esmini, would you be interested in? Should I then ask for pull request, even it is not officially part of OSC? But these functionalities would be then not complete, this means we have our own servers and tools sending and receiving these mqtt messages, that would not be shared, only the core functionality of V2X communication would be then part of official esmini.
- If I wanted to make such V2X communication visible, could you say me where in the code I could plot lines or antenna signs, that this and this object is communicating?
- Same question as 3 only for the visual part of 4.
Kind regards,
Thaddäus
First, this is an interesting use case! So far I haven't heard about involving esmini in such a simulation of systems of systems. We haven't yet done anything such.
Hi, I know that OSC is currently not including V2X, but we did it via mqtt messaging and user defined actions. I wanted to ask now some questions regarding this V2X topic:
- Do you have any V2X functionalities on your roadmap, if so how looks your time planning for this and how do you plan to integrate it?
No, there's currently no plans for V2X integration.
- I wanted to use esmini in that case more as a library, not as a tool, this means where I can find your "main" function to have access to all triggers, actions, objects, ... (it is ScenarioEngine->step right?) to integrate such mqtt messaging (receive objects' position and overwrite esmini simulation data and also send positions of esmini objects)? So the question would be where in the code such changes should be done, I think you can answer it quite fast (seems ScenarioEngine->step is doing that all). Theoratically I could use osi_receiver_ip here and listen to OSI UDP messages to get all objects' position and so on, but this would work only in the direction getting data from esmini and would not mtach my triggers for such messaging, the opposite direction to overwrite esmini data would not work this way right? For the moment I see no other way for such extended functionality to customize the ScenarioEngine->step function.
Very good. I think you're thinking is in the right direction. There are like three levels of integrating esmini.
- Listen to OSI output from esmini.
- Link and use the esmini lib (DLL, .so...). It provides a simplified (limited) API to control and read out data from ongoing simulation.
- Link esmini internal libraries statically to get full control and data access. Actually esmini application itself could serve as an example starting point. Although it doesn't showcase a lot of features it shows how to initialize esmini and get a handle to the player object. The player object is a good entry-point, including handles to e.g. scenarioEngine (including entities), roadmanager (OpenDrive object) viewer and scenarioGateway. For more examples, you can see esminiLib.cpp and how it interacts with the simulation via the player object. (the esmini lib itself links internal static libs package them together with the simple API into a shared lib for dynamic linking).
- If I would integrate such V2X functionalities into esmini, would you be interested in? Should I then ask for pull request, even it is not officially part of OSC? But these functionalities would be then not complete, this means we have our own servers and tools sending and receiving these mqtt messages, that would not be shared, only the core functionality of V2X communication would be then part of official esmini.
Maybe you can build what you need as a separate application, linking with selected esmini libs. If you want to add some functions (extend the API of some classes) I think that's perfect for pull requests. But my spontaneous opinion would be to keep actual V2X integration separate as for now. But if V2X protocol gets standardized, maybe by ASAM, then I think it could make sense to add support in esmini.
- If I wanted to make such V2X communication visible, could you say me where in the code I could plot lines or antenna signs, that this and this object is communicating?
Unfortunately we have no convenient API for adding visual features. The existing ones, e.g. road lines and sensor frustums, are coded directly on top of OpenSceneGraph API in the viewer class. Long term it would be great with a "VisualFeature" class that could be a starting point for such elements. But for now I have to refer to the ViewerBase module.
- Same question as 3 only for the visual part of 4.
For now, and assuming we keep actual V2X integration out of esmini core, I think visualization of specific V2X elements should not be part of the viewer class. But one idea could be to implement a base class for adding simple visualization items, like lines or simple shapes (boxes, triangles, cylinders...) and have a common API for creating and controlling. Such a base class can be used by esmini-like applications which then can add specific items to the visualization.
However, short term, considering our current backlog we will not be able to support fully. So any such initiative would require most efforts from you/the user.
Kind regards,
Thaddäus
Think about it and let us know what will be your approach.
Hi Emil,
many thanks for the answers. I think we will extend the esminiLib.cpp and some other classes (player base and scenario engine seem to be also such candidates) to get control and access to even more attributes of the simulation (here a pull request makes sense afterwards, these new functions will be used for the v2x functionality, but will be not directly linked to it and can be also used for other functionalities). The visual effects will come later, but also here some visual basic elements could be directly added to esmini like you mentioned.
So far I have a lot of work upcoming to me, will work on it the next weeks. In case of any questions I will come up to you here again later.
Kind regards,
Thaddäus
Object* obj = entities_.object_[i];
LOG("MQTT update esmini vehicle from mqtt t=%d, x=%.2f (old %.2f), y=%.2f (old %.2f), h=%.2f (old %.2f), v=%.2f (old %.2f)", obj->name_, mqtt_geometry_poses_received_[found].creation_time_ms, x, obj->pos_.GetX(), y, obj->pos_.GetY(), heading, obj->pos_.GetH(), mqtt_geometry_poses_received_[found].speed_m_s, obj->speed_);
scenarioGateway.reportObject(obj->id_, obj->name_, static_cast<int>(obj->type_), obj->category_, obj->model_id_,
obj->GetActivatedControllerType(), obj->boundingbox_, static_cast<int>(obj->scaleMode_), obj->visibilityMask_,
simulationTime_, mqtt_geometry_poses_received_[found].speed_m_s, obj->wheel_angle_, obj->wheel_rot_, x, y, heading);
LOG("x=%.2f, y=%.2f", obj->pos_.GetX(), obj->pos_.GetY());
// here access the pos from incoming mqtt message
//obj->pos_.SetAlignModeZ(roadmanager::Position::ALIGN_MODE::ALIGN_HARD);
//obj->pos_.SetAlignModeP(roadmanager::Position::ALIGN_MODE::ALIGN_HARD);
// Update object state via gateway
//scenarioGateway.updateObjectWorldPosXYH(obj->id_, 0.0, x, y, heading);
//scenarioGateway.updateObjectSpeed(obj->id_, 0.0, mqtt_geometry_poses_received_[found].speed_m_s);
output
3.610: MQTT update esmini vehicle from mqtt t=-164441120, x=-178.89 (old -127.57), y=-119.15 (old 361.07), h=0.07 (old 6.05), v=0.01 (old 0.00)
3.610: x=-127.57, y=361.07
already checked in SetInertiaPos, yes x_ and y_ are really overridden by my new values so the reportObject function is called correctly, but GetX and GetY return the old values and the more important thing is, that the position of the car does not change in visualization, he keeps the old values. I'm calling my reportObject function in scenario engine directly below your ones. Do you have any idea what is going wrong here? Where is the link to the visualization in your case? Is it correct that only the update-functions or reportObject function does really move the object and the visualization comes at a later step accessing these new positions?
The reported positions are registered in gateway, then picked up by scenarioEngine at start of next frame (in scenarioEngine step function). See this sequence diagram: https://media.githubusercontent.com/media/esmini/esmini/master/docs/esmini-frame-detailed.png Please note that the diagram is not perfectly correct, but should give a rough idea.
Can you check if the reported position is applied during the step() function, and subsequently returned by the object GetX()/GetY() methods?
But if objects are not affected by the reported positions at all, then there is another root cause and we should have a closer look.
Hi, it looks like, that you always are modifying directly the positions of objects during scenario engine step function and only giving the gateway the pointers to these positions, in the reportObject function as well as in the update-functions. You see in my code above (also in step function of scenario engine below your reportObject/update part, with if else conditions I've deactivated your report/update part for ego and used mine), that I tried to modify the objects' positions directly by gateway-functions. This seems to be the biggest difference.
Kind regards,
Thaddäus
Could you try in some version to set manually in your step-function code to set x,y positions and speed manually independent of the used xosc and see if it works? Could you replace this in scenario engine step function
// Report updated state to the gateway
if (scenarioGateway.isObjectReported(obj->id_))
{
if (obj->CheckDirtyBits(Object::DirtyBit::LONGITUDINAL | Object::DirtyBit::LATERAL))
{
scenarioGateway.updateObjectPos(obj->id_, simulationTime_, &obj->pos_);
}
if (obj->CheckDirtyBits(Object::DirtyBit::SPEED))
{
scenarioGateway.updateObjectSpeed(obj->id_, simulationTime_, obj->speed_);
}
if (obj->CheckDirtyBits(Object::DirtyBit::WHEEL_ANGLE))
{
scenarioGateway.updateObjectWheelAngle(obj->id_, simulationTime_, obj->wheel_angle_);
}
if (obj->CheckDirtyBits(Object::DirtyBit::WHEEL_ROTATION))
{
scenarioGateway.updateObjectWheelRotation(obj->id_, simulationTime_, obj->wheel_rot_);
}
if (obj->CheckDirtyBits(Object::DirtyBit::VISIBILITY))
{
scenarioGateway.updateObjectVisibilityMask(obj->id_, obj->visibilityMask_);
}
}
else
{
// Object not reported yet, do that
scenarioGateway.reportObject(obj->id_, obj->name_, static_cast<int>(obj->type_), obj->category_, obj->model_id_,
obj->GetActivatedControllerType(), obj->boundingbox_, static_cast<int>(obj->scaleMode_), obj->visibilityMask_,
simulationTime_, obj->speed_, obj->wheel_angle_, obj->wheel_rot_, &obj->pos_);
}
by:
if (obj->name_ != "Ego")
{
// Report updated state to the gateway
if (scenarioGateway.isObjectReported(obj->id_))
{
if (obj->CheckDirtyBits(Object::DirtyBit::LONGITUDINAL | Object::DirtyBit::LATERAL))
{
scenarioGateway.updateObjectPos(obj->id_, simulationTime_, &obj->pos_);
}
if (obj->CheckDirtyBits(Object::DirtyBit::SPEED))
{
scenarioGateway.updateObjectSpeed(obj->id_, simulationTime_, obj->speed_);
}
if (obj->CheckDirtyBits(Object::DirtyBit::WHEEL_ANGLE))
{
scenarioGateway.updateObjectWheelAngle(obj->id_, simulationTime_, obj->wheel_angle_);
}
if (obj->CheckDirtyBits(Object::DirtyBit::WHEEL_ROTATION))
{
scenarioGateway.updateObjectWheelRotation(obj->id_, simulationTime_, obj->wheel_rot_);
}
if (obj->CheckDirtyBits(Object::DirtyBit::VISIBILITY))
{
scenarioGateway.updateObjectVisibilityMask(obj->id_, obj->visibilityMask_);
}
}
else
{
// Object not reported yet, do that
scenarioGateway.reportObject(obj->id_, obj->name_, static_cast<int>(obj->type_), obj->category_, obj->model_id_,
obj->GetActivatedControllerType(), obj->boundingbox_, static_cast<int>(obj->scaleMode_), obj->visibilityMask_,
simulationTime_, obj->speed_, obj->wheel_angle_, obj->wheel_rot_, &obj->pos_);
}
}
else
{
double y = 200;
if (i < 500)
{
double x = 0.5 * i + 100;
double v = 0.5 / deltaSimTime;
}
else
{
double x = i + 100;
double v = 1.0 / deltaSimTime;
}
if (scenarioGateway.isObjectReported(obj->id_))
{
LOG("%s, x=%.2f (old %.2f), y=%.2f (old %.2f), v=%.2f (old %.2f)", obj->name_.c_str(), x, obj->pos_.GetX(), y, obj->pos_.GetY(), v, obj->speed_);
obj->pos_.SetAlignModeZ(roadmanager::Position::ALIGN_MODE::ALIGN_HARD);
obj->pos_.SetAlignModeP(roadmanager::Position::ALIGN_MODE::ALIGN_HARD);
scenarioGateway.updateObjectWorldPosXYH(obj->id_, 0.0, x, y, 0);
scenarioGateway.updateObjectSpeed(obj->id_, 0.0, v);
LOG("%s, x=%.2f, y=%.2f, v=%.2f", obj->name_.c_str(), obj->pos_.GetX(), obj->pos_.GetY(), obj->speed_);
}
else
{
LOG("%s, x=%.2f, y=%.2f, v=%.2f", obj->name_.c_str(), x, y, v);
scenarioGateway.reportObject(obj->id_, obj->name_.c_str(), static_cast<int>(obj->type_), obj->category_, obj->model_id_,
obj->GetActivatedControllerType(), obj->boundingbox_, static_cast<int>(obj->scaleMode_), obj->visibilityMask_,
simulationTime_, v, obj->wheel_angle_, obj->wheel_rot_, x, y, 0);
LOG("%s, x=%.2f, y=%.2f, v=%.2f", obj->name_.c_str(), obj->pos_.GetX(), obj->pos_.GetY(), obj->speed_);
}
}
The double x,y and v must maybe be moved outside of the i-for loop, but would be a good test for me to see, if I'm too stupid or if there is any problem with the gateway functions or the procedure at all, at moment I don't know how esmini would behave here, if the global positions do not match the road network, any error / exception / crash? or is simply the offset very large? car driving in the sky? ... My positions should match the road network of the xosc, but wanted to check it with esmini if the car is driving on the road.
Hi,
I've found the issue, in my xosc the interactive driver was used as controller and seems this controller is overwriting in each step the positions to its own old ones. This was the reson, why my overwritten positions were resetted after each step. This issue has me cost really a lot of time, but is maybe a feature of the interactive driver controller.
Ah, you're right. That's probably because the interactive controller keeps the state of its own vehicle model. That state is not updated by any values from scenario engine. It's like a one way communication of state from interactive controller to scenario engine.
This could probably be modified. Perhaps as an option to the controller, to read back state from scenario to catch any updates from other sources.
Would that solve your problem, if you still have one...
Hi Emil, in general an optional property of this controller does notcreally help here, instead of using this new option I can also omit, deactivate or remove this controller also by one liner.
I would more suggest to add the way back to this controller permanently or try to use the state of scenario engine by a pointer, because I expect you will be earlier or later at same point like me, that you try to overwrite the positions and it would not work in all cases due to this controller. Imagine for example you work with more realistic vehicle models and you have to combine the states with a kalman filter or you try to combine the interactive controller with a lane keeping or lane departure warning controller. Does it not make sense in general to have the backward direction for states here?