|
@@ -7,13 +7,8 @@
|
|
|
*/
|
|
|
|
|
|
#include <Atom/RPI.Public/AuxGeom/AuxGeomFeatureProcessorInterface.h>
|
|
|
-#include <Atom/RPI.Public/RPISystemInterface.h>
|
|
|
#include <Atom/RPI.Public/Scene.h>
|
|
|
-#include <AzCore/Component/Entity.h>
|
|
|
-#include <AzCore/Serialization/EditContext.h>
|
|
|
-#include <AzCore/Serialization/EditContextConstants.inl>
|
|
|
#include <AzFramework/Physics/PhysicsSystem.h>
|
|
|
-#include <Lidar/LidarTemplateUtils.h>
|
|
|
#include <Lidar/ROS2LidarSensorComponent.h>
|
|
|
#include <ROS2/Frame/ROS2FrameComponent.h>
|
|
|
#include <ROS2/ROS2Bus.h>
|
|
@@ -34,9 +29,12 @@ namespace ROS2
|
|
|
serialize->Class<ROS2LidarSensorComponent, ROS2SensorComponent>()
|
|
|
->Version(1)
|
|
|
->Field("lidarModel", &ROS2LidarSensorComponent::m_lidarModel)
|
|
|
+ ->Field("lidarImplementation", &ROS2LidarSensorComponent::m_lidarSystem)
|
|
|
->Field("LidarParameters", &ROS2LidarSensorComponent::m_lidarParameters)
|
|
|
->Field("IgnoreLayer", &ROS2LidarSensorComponent::m_ignoreLayer)
|
|
|
- ->Field("IgnoredLayerIndex", &ROS2LidarSensorComponent::m_ignoredLayerIndex);
|
|
|
+ ->Field("IgnoredLayerIndex", &ROS2LidarSensorComponent::m_ignoredLayerIndex)
|
|
|
+ ->Field("ExcludedEntities", &ROS2LidarSensorComponent::m_excludedEntities)
|
|
|
+ ->Field("PointsAtMax", &ROS2LidarSensorComponent::m_addPointsAtMax);
|
|
|
|
|
|
if (AZ::EditContext* ec = serialize->GetEditContext())
|
|
|
{
|
|
@@ -46,30 +44,100 @@ namespace ROS2
|
|
|
->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game"))
|
|
|
->DataElement(AZ::Edit::UIHandlers::ComboBox, &ROS2LidarSensorComponent::m_lidarModel, "Lidar Model", "Lidar model")
|
|
|
->Attribute(AZ::Edit::Attributes::ChangeNotify, &ROS2LidarSensorComponent::OnLidarModelSelected)
|
|
|
- ->EnumAttribute(LidarTemplate::LidarModel::Generic3DLidar, "Generic Lidar")
|
|
|
+ ->EnumAttribute(LidarTemplate::LidarModel::Custom3DLidar, "Custom Lidar")
|
|
|
+ ->EnumAttribute(LidarTemplate::LidarModel::Ouster_OS0_64, "Ouster OS0-64")
|
|
|
+ ->EnumAttribute(LidarTemplate::LidarModel::Ouster_OS1_64, "Ouster OS1-64")
|
|
|
+ ->EnumAttribute(LidarTemplate::LidarModel::Ouster_OS2_64, "Ouster OS2-64")
|
|
|
+ ->EnumAttribute(LidarTemplate::LidarModel::Velodyne_Puck, "Velodyne Puck (VLP-16)")
|
|
|
+ ->EnumAttribute(LidarTemplate::LidarModel::Velodyne_HDL_32E, "Velodyne HDL-32E")
|
|
|
+ ->DataElement(
|
|
|
+ AZ::Edit::UIHandlers::ComboBox,
|
|
|
+ &ROS2LidarSensorComponent::m_lidarSystem,
|
|
|
+ "Lidar Implementation",
|
|
|
+ "Select a lidar implementation out of registered ones.")
|
|
|
+ ->Attribute(AZ::Edit::Attributes::ChangeNotify, &ROS2LidarSensorComponent::OnLidarImplementationSelected)
|
|
|
+ ->Attribute(AZ::Edit::Attributes::StringList, &ROS2LidarSensorComponent::FetchLidarSystemList)
|
|
|
->DataElement(
|
|
|
AZ::Edit::UIHandlers::EntityId,
|
|
|
&ROS2LidarSensorComponent::m_lidarParameters,
|
|
|
"Lidar parameters",
|
|
|
- "Configuration of Generic lidar")
|
|
|
+ "Configuration of Custom lidar")
|
|
|
->Attribute(AZ::Edit::Attributes::Visibility, &ROS2LidarSensorComponent::IsConfigurationVisible)
|
|
|
->DataElement(
|
|
|
AZ::Edit::UIHandlers::ComboBox,
|
|
|
&ROS2LidarSensorComponent::m_ignoreLayer,
|
|
|
"Ignore layer",
|
|
|
"Should we ignore selected layer index")
|
|
|
+ ->Attribute(AZ::Edit::Attributes::Visibility, &ROS2LidarSensorComponent::IsIgnoredLayerConfigurationVisible)
|
|
|
->DataElement(
|
|
|
AZ::Edit::UIHandlers::Default,
|
|
|
&ROS2LidarSensorComponent::m_ignoredLayerIndex,
|
|
|
"Ignored layer index",
|
|
|
- "Layer index to ignore");
|
|
|
+ "Layer index to ignore")
|
|
|
+ ->Attribute(AZ::Edit::Attributes::Visibility, &ROS2LidarSensorComponent::IsIgnoredLayerConfigurationVisible)
|
|
|
+ ->DataElement(
|
|
|
+ 0, &ROS2LidarSensorComponent::m_excludedEntities, "Excluded Entities", "List of entities excluded from raycasting.")
|
|
|
+ ->Attribute(AZ::Edit::Attributes::AutoExpand, true)
|
|
|
+ ->Attribute(AZ::Edit::Attributes::ContainerCanBeModified, true)
|
|
|
+ ->Attribute(AZ::Edit::Attributes::Visibility, &ROS2LidarSensorComponent::IsEntityExclusionVisible)
|
|
|
+ ->DataElement(
|
|
|
+ AZ::Edit::UIHandlers::Default,
|
|
|
+ &ROS2LidarSensorComponent::m_addPointsAtMax,
|
|
|
+ "Points at Max",
|
|
|
+ "If set true LiDAR will produce points at max range for free space")
|
|
|
+ ->Attribute(AZ::Edit::Attributes::Visibility, &ROS2LidarSensorComponent::IsMaxPointsConfigurationVisible);
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
+ AZStd::string GetDefaultLidarSystem()
|
|
|
+ {
|
|
|
+ const auto& lidarSystemList = LidarRegistrarInterface::Get()->GetRegisteredLidarSystems();
|
|
|
+ if (lidarSystemList.empty())
|
|
|
+ {
|
|
|
+ AZ_Warning("ROS2LidarSensorComponent", false, "No LIDAR system for the sensor to use.");
|
|
|
+ return AZStd::string();
|
|
|
+ }
|
|
|
+ return lidarSystemList.front();
|
|
|
+ }
|
|
|
+
|
|
|
+ void ROS2LidarSensorComponent::FetchLidarImplementationFeatures()
|
|
|
+ {
|
|
|
+ if (m_lidarSystem.empty())
|
|
|
+ {
|
|
|
+ m_lidarSystem = GetDefaultLidarSystem();
|
|
|
+ }
|
|
|
+ const auto* lidarMetaData = LidarRegistrarInterface::Get()->GetLidarSystemMetaData(m_lidarSystem);
|
|
|
+ AZ_Warning("ROS2LidarSensorComponent", lidarMetaData, "No metadata for \"%s\"", m_lidarSystem.c_str());
|
|
|
+ if (lidarMetaData)
|
|
|
+ {
|
|
|
+ m_lidarSystemFeatures = LidarRegistrarInterface::Get()->GetLidarSystemMetaData(m_lidarSystem)->m_features;
|
|
|
+ }
|
|
|
+ }
|
|
|
|
|
|
bool ROS2LidarSensorComponent::IsConfigurationVisible() const
|
|
|
{
|
|
|
- return m_lidarModel == LidarTemplate::LidarModel::Generic3DLidar;
|
|
|
+ return m_lidarModel == LidarTemplate::LidarModel::Custom3DLidar;
|
|
|
+ }
|
|
|
+
|
|
|
+ bool ROS2LidarSensorComponent::IsIgnoredLayerConfigurationVisible() const
|
|
|
+ {
|
|
|
+ return m_lidarSystemFeatures & LidarSystemFeatures::CollisionLayers;
|
|
|
+ }
|
|
|
+
|
|
|
+ bool ROS2LidarSensorComponent::IsEntityExclusionVisible() const
|
|
|
+ {
|
|
|
+ return m_lidarSystemFeatures & LidarSystemFeatures::EntityExclusion;
|
|
|
+ }
|
|
|
+
|
|
|
+ bool ROS2LidarSensorComponent::IsMaxPointsConfigurationVisible() const
|
|
|
+ {
|
|
|
+ return m_lidarSystemFeatures & LidarSystemFeatures::MaxRangePoints;
|
|
|
+ }
|
|
|
+
|
|
|
+ AZStd::vector<AZStd::string> ROS2LidarSensorComponent::FetchLidarSystemList()
|
|
|
+ {
|
|
|
+ FetchLidarImplementationFeatures();
|
|
|
+ return LidarRegistrarInterface::Get()->GetRegisteredLidarSystems();
|
|
|
}
|
|
|
|
|
|
AZ::Crc32 ROS2LidarSensorComponent::OnLidarModelSelected()
|
|
@@ -78,6 +146,63 @@ namespace ROS2
|
|
|
return AZ::Edit::PropertyRefreshLevels::EntireTree;
|
|
|
}
|
|
|
|
|
|
+ AZ::Crc32 ROS2LidarSensorComponent::OnLidarImplementationSelected()
|
|
|
+ {
|
|
|
+ FetchLidarImplementationFeatures();
|
|
|
+ return AZ::Edit::PropertyRefreshLevels::EntireTree;
|
|
|
+ }
|
|
|
+
|
|
|
+ void ROS2LidarSensorComponent::ConnectToLidarRaycaster()
|
|
|
+ {
|
|
|
+ if (auto raycasterId = m_implementationToRaycasterMap.find(m_lidarSystem); raycasterId != m_implementationToRaycasterMap.end())
|
|
|
+ {
|
|
|
+ m_lidarRaycasterId = raycasterId->second;
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ m_lidarRaycasterId = LidarId::CreateNull();
|
|
|
+ LidarSystemRequestBus::EventResult(
|
|
|
+ m_lidarRaycasterId, AZ_CRC(m_lidarSystem), &LidarSystemRequestBus::Events::CreateLidar, GetEntityId());
|
|
|
+ AZ_Assert(!m_lidarRaycasterId.IsNull(), "Could not access selected Lidar System.");
|
|
|
+
|
|
|
+ m_implementationToRaycasterMap.emplace(m_lidarSystem, m_lidarRaycasterId);
|
|
|
+ }
|
|
|
+
|
|
|
+ void ROS2LidarSensorComponent::ConfigureLidarRaycaster()
|
|
|
+ {
|
|
|
+ FetchLidarImplementationFeatures();
|
|
|
+ LidarRaycasterRequestBus::Event(m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRayOrientations, m_lastRotations);
|
|
|
+ LidarRaycasterRequestBus::Event(
|
|
|
+ m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRayRange, m_lidarParameters.m_maxRange);
|
|
|
+
|
|
|
+ if (m_lidarSystemFeatures & LidarSystemFeatures::Noise)
|
|
|
+ {
|
|
|
+ LidarRaycasterRequestBus::Event(
|
|
|
+ m_lidarRaycasterId,
|
|
|
+ &LidarRaycasterRequestBus::Events::ConfigureNoiseParameters,
|
|
|
+ m_lidarParameters.m_noiseParameters.m_angularNoiseStdDev,
|
|
|
+ m_lidarParameters.m_noiseParameters.m_distanceNoiseStdDevBase,
|
|
|
+ m_lidarParameters.m_noiseParameters.m_distanceNoiseStdDevRisePerMeter);
|
|
|
+ }
|
|
|
+
|
|
|
+ if (m_lidarSystemFeatures & LidarSystemFeatures::CollisionLayers)
|
|
|
+ {
|
|
|
+ LidarRaycasterRequestBus::Event(
|
|
|
+ m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureLayerIgnoring, m_ignoreLayer, m_ignoredLayerIndex);
|
|
|
+ }
|
|
|
+
|
|
|
+ if (m_lidarSystemFeatures & LidarSystemFeatures::EntityExclusion)
|
|
|
+ {
|
|
|
+ LidarRaycasterRequestBus::Event(m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ExcludeEntities, m_excludedEntities);
|
|
|
+ }
|
|
|
+
|
|
|
+ if (m_lidarSystemFeatures & LidarSystemFeatures::MaxRangePoints)
|
|
|
+ {
|
|
|
+ LidarRaycasterRequestBus::Event(
|
|
|
+ m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureMaxRangePointAddition, m_addPointsAtMax);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
ROS2LidarSensorComponent::ROS2LidarSensorComponent()
|
|
|
{
|
|
|
TopicConfiguration pc;
|
|
@@ -109,21 +234,6 @@ namespace ROS2
|
|
|
}
|
|
|
}
|
|
|
|
|
|
- void ROS2LidarSensorComponent::SetPhysicsScene()
|
|
|
- {
|
|
|
- auto* physicsSystem = AZ::Interface<AzPhysics::SystemInterface>::Get();
|
|
|
- auto foundBody = physicsSystem->FindAttachedBodyHandleFromEntityId(GetEntityId());
|
|
|
- auto lidarPhysicsSceneHandle = foundBody.first;
|
|
|
- if (foundBody.first == AzPhysics::InvalidSceneHandle)
|
|
|
- {
|
|
|
- auto* sceneInterface = AZ::Interface<AzPhysics::SceneInterface>::Get();
|
|
|
- lidarPhysicsSceneHandle = sceneInterface->GetSceneHandle(AzPhysics::DefaultPhysicsSceneName);
|
|
|
- }
|
|
|
-
|
|
|
- AZ_Assert(lidarPhysicsSceneHandle != AzPhysics::InvalidSceneHandle, "Invalid physics scene handle for entity");
|
|
|
- m_lidarRaycaster.SetRaycasterScene(lidarPhysicsSceneHandle);
|
|
|
- }
|
|
|
-
|
|
|
void ROS2LidarSensorComponent::Activate()
|
|
|
{
|
|
|
auto ros2Node = ROS2Interface::Get()->GetNode();
|
|
@@ -133,13 +243,18 @@ namespace ROS2
|
|
|
AZStd::string fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic);
|
|
|
m_pointCloudPublisher = ros2Node->create_publisher<sensor_msgs::msg::PointCloud2>(fullTopic.data(), publisherConfig.GetQoS());
|
|
|
|
|
|
- SetPhysicsScene();
|
|
|
if (m_sensorConfiguration.m_visualise)
|
|
|
{
|
|
|
auto* entityScene = AZ::RPI::Scene::GetSceneForEntityId(GetEntityId());
|
|
|
m_drawQueue = AZ::RPI::AuxGeomFeatureProcessorInterface::GetDrawQueueForScene(entityScene);
|
|
|
}
|
|
|
- m_lidarRaycaster.SetAddPointsMaxRange(m_lidarParameters.m_addPointsAtMax);
|
|
|
+
|
|
|
+ m_lastRotations = LidarTemplateUtils::PopulateRayRotations(m_lidarParameters);
|
|
|
+
|
|
|
+ FetchLidarImplementationFeatures();
|
|
|
+ ConnectToLidarRaycaster();
|
|
|
+ ConfigureLidarRaycaster();
|
|
|
+
|
|
|
ROS2SensorComponent::Activate();
|
|
|
}
|
|
|
|
|
@@ -151,18 +266,10 @@ namespace ROS2
|
|
|
|
|
|
void ROS2LidarSensorComponent::FrequencyTick()
|
|
|
{
|
|
|
- float distance = m_lidarParameters.m_maxRange;
|
|
|
auto entityTransform = GetEntity()->FindComponent<AzFramework::TransformComponent>();
|
|
|
- const auto directions =
|
|
|
- LidarTemplateUtils::PopulateRayDirections(m_lidarParameters, entityTransform->GetWorldTM().GetEulerRadians());
|
|
|
- AZ::Vector3 start = entityTransform->GetWorldTM().GetTranslation();
|
|
|
- start.SetZ(start.GetZ());
|
|
|
-
|
|
|
- auto worldToLidarTransform = entityTransform->GetWorldTM();
|
|
|
- worldToLidarTransform.Invert();
|
|
|
|
|
|
- m_lastScanResults =
|
|
|
- m_lidarRaycaster.PerformRaycast(start, directions, worldToLidarTransform, distance, m_ignoreLayer, m_ignoredLayerIndex);
|
|
|
+ LidarRaycasterRequestBus::EventResult(
|
|
|
+ m_lastScanResults, m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::PerformRaycast, entityTransform->GetWorldTM());
|
|
|
if (m_lastScanResults.empty())
|
|
|
{
|
|
|
AZ_TracePrintf("Lidar Sensor Component", "No results from raycast\n");
|
|
@@ -171,13 +278,13 @@ namespace ROS2
|
|
|
|
|
|
if (m_sensorConfiguration.m_visualise)
|
|
|
{ // Store points for visualisation purposes, in global frame
|
|
|
- auto localToWorldTM = entityTransform->GetWorldTM();
|
|
|
-
|
|
|
m_visualisationPoints = m_lastScanResults;
|
|
|
- for (AZ::Vector3& point : m_visualisationPoints)
|
|
|
- {
|
|
|
- point = localToWorldTM.TransformPoint(point);
|
|
|
- }
|
|
|
+ }
|
|
|
+
|
|
|
+ const auto inverseLidarTM = entityTransform->GetWorldTM().GetInverse();
|
|
|
+ for (auto& point : m_lastScanResults)
|
|
|
+ {
|
|
|
+ point = inverseLidarTM.TransformPoint(point);
|
|
|
}
|
|
|
|
|
|
auto* ros2Frame = Utils::GetGameOrEditorComponent<ROS2FrameComponent>(GetEntity());
|