diff --git a/Gems/ROS2/Code/Source/Manipulation/JointsManipulationEditorComponent.cpp b/Gems/ROS2/Code/Source/Manipulation/JointsManipulationEditorComponent.cpp index 23da69efc..7429856ad 100644 --- a/Gems/ROS2/Code/Source/Manipulation/JointsManipulationEditorComponent.cpp +++ b/Gems/ROS2/Code/Source/Manipulation/JointsManipulationEditorComponent.cpp @@ -29,6 +29,11 @@ namespace ROS2 m_jointStatePublisherConfiguration.m_frequency = 25.0f; } + JointsManipulationEditorComponent::JointsManipulationEditorComponent(const PublisherConfiguration& publisherConfiguration) + { + m_jointStatePublisherConfiguration = publisherConfiguration; + } + void JointsManipulationEditorComponent::BuildGameEntity(AZ::Entity* gameEntity) { gameEntity->CreateComponent( diff --git a/Gems/ROS2/Code/Source/Manipulation/JointsManipulationEditorComponent.h b/Gems/ROS2/Code/Source/Manipulation/JointsManipulationEditorComponent.h index 70164d242..089b6ea47 100644 --- a/Gems/ROS2/Code/Source/Manipulation/JointsManipulationEditorComponent.h +++ b/Gems/ROS2/Code/Source/Manipulation/JointsManipulationEditorComponent.h @@ -22,6 +22,7 @@ namespace ROS2 { public: JointsManipulationEditorComponent(); + JointsManipulationEditorComponent(const PublisherConfiguration& publisherConfiguration); ~JointsManipulationEditorComponent() = default; AZ_EDITOR_COMPONENT(JointsManipulationEditorComponent, "{BF2F77FD-92FB-4730-92C7-DDEE54F508BF}"); diff --git a/Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.cpp b/Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.cpp index bbbc79774..a4d8b46cd 100644 --- a/Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.cpp +++ b/Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.cpp @@ -17,6 +17,11 @@ namespace ROS2 { + JointsTrajectoryComponent::JointsTrajectoryComponent(const AZStd::string& followTrajectoryActionName) + : m_followTrajectoryActionName(followTrajectoryActionName) + { + } + void JointsTrajectoryComponent::Activate() { auto* ros2Frame = GetEntity()->FindComponent(); diff --git a/Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.h b/Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.h index a5dcb783e..5c6ac6c91 100644 --- a/Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.h +++ b/Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.h @@ -26,6 +26,7 @@ namespace ROS2 { public: JointsTrajectoryComponent() = default; + JointsTrajectoryComponent(const AZStd::string& followTrajectoryActionName); ~JointsTrajectoryComponent() = default; AZ_COMPONENT(JointsTrajectoryComponent, "{429DE04C-6B6D-4B2D-9D6C-3681F23CBF90}", AZ::Component); diff --git a/Gems/ROS2/Code/Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.cpp b/Gems/ROS2/Code/Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.cpp index 427d8a514..6a35b94b5 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.cpp @@ -71,8 +71,10 @@ namespace ROS2 m_sensorHooks.emplace_back(SDFormat::ROS2SensorHooks::ROS2GNSSSensor()); m_sensorHooks.emplace_back(SDFormat::ROS2SensorHooks::ROS2ImuSensor()); m_sensorHooks.emplace_back(SDFormat::ROS2SensorHooks::ROS2LidarSensor()); - m_modelPluginHooks.emplace_back(SDFormat::ROS2ModelPluginHooks::ROS2AckermannModel()); + m_modelPluginHooks.emplace_back(SDFormat::ROS2ModelPluginHooks::ROS2AckermannModel()); m_modelPluginHooks.emplace_back(SDFormat::ROS2ModelPluginHooks::ROS2SkidSteeringModel()); + m_modelPluginHooks.emplace_back(SDFormat::ROS2ModelPluginHooks::ROS2JointStatePublisherModel()); + m_modelPluginHooks.emplace_back(SDFormat::ROS2ModelPluginHooks::ROS2JointPoseTrajectoryModel()); // Query user-defined sensor and plugin hooks auto serializeContext = AZ::Interface::Get()->GetSerializeContext(); diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2AckermannModelHook.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2AckermannModelHook.cpp index 21b788ad3..46a7265c7 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2AckermannModelHook.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2AckermannModelHook.cpp @@ -158,12 +158,12 @@ namespace ROS2::SDFormat VehicleDynamics::VehicleConfiguration GetConfiguration( const sdf::ElementPtr element, const sdf::Model& sdfModel, const CreatedEntitiesMap& createdEntities) { - const AZStd::array jointNamesSDFormat{ { "front_left_joint", - "front_right_joint", - "rear_left_joint", - "rear_right_joint", - "left_steering_joint", - "right_steering_joint" } }; + const static AZStd::array jointNamesSDFormat{ { "front_left_joint", + "front_right_joint", + "rear_left_joint", + "rear_right_joint", + "left_steering_joint", + "right_steering_joint" } }; // Stores description of all joints in the SDF and a mapping to O3DE entities struct JointMapping { @@ -314,7 +314,7 @@ namespace ROS2::SDFormat } else { - return AZ::Failure(AZStd::string("Failed to create ROS2 Ackermann Control Component")); + return AZ::Failure(AZStd::string("Failed to create ROS 2 Ackermann Control Component")); } }; diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp index b3ea54967..55965278e 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp @@ -22,14 +22,22 @@ namespace ROS2::SDFormat SensorImporterHook importerHook; importerHook.m_sensorTypes = AZStd::unordered_set{ sdf::SensorType::CAMERA, sdf::SensorType::DEPTH_CAMERA, sdf::SensorType::RGBD_CAMERA }; - importerHook.m_supportedSensorParams = AZStd::unordered_set{ - ">pose", ">update_rate", ">camera>horizontal_fov", ">camera>image>width", ">camera>image>height", ">camera>clip>near", - ">camera>clip>far" - }; + importerHook.m_supportedSensorParams = AZStd::unordered_set{ ">pose", + ">update_rate", + ">camera>horizontal_fov", + ">camera>image>width", + ">camera>image>height", + ">camera>clip>near", + ">camera>clip>far", + ">visualize" }; importerHook.m_pluginNames = AZStd::unordered_set{ "libgazebo_ros_camera.so", "libgazebo_ros_depth_camera.so", "libgazebo_ros_openni_kinect.so" }; - importerHook.m_supportedPluginParams = AZStd::unordered_set{}; + importerHook.m_supportedPluginParams = AZStd::unordered_set{ + ">imageTopicName", ">cameraInfoTopicName", ">depthImageTopicName", ">depthImageCameraInfoTopicName", + ">ros>remapping", ">ros>argument", ">ros>frame_name", ">ros>namespace", ">updateRate", + ">frameName", ">robotNamespace" + }; importerHook.m_sdfSensorToComponentCallback = [](AZ::Entity& entity, const sdf::Sensor& sdfSensor) -> SensorImporterHook::ConvertSensorOutcome { @@ -61,25 +69,45 @@ namespace ROS2::SDFormat cameraConfiguration.m_farClipDistance = static_cast(cameraSensor->DepthFarClip()); } + const auto cameraPluginParams = HooksUtils::GetPluginParams(sdfSensor.Plugins()); + const auto element = sdfSensor.Element(); + SensorConfiguration sensorConfiguration; - sensorConfiguration.m_frequency = sdfSensor.UpdateRate(); + sensorConfiguration.m_frequency = HooksUtils::GetFrequency(cameraPluginParams); + if (sdfSensor.Type() != sdf::SensorType::DEPTH_CAMERA) { // COLOR_CAMERA and RGBD_CAMERA + const static AZStd::vector rawImageParamNames = { "image_raw", "imageTopicName" }; + const static AZStd::vector rawInfoParamNames = { "camera_info", "cameraInfoTopicName" }; + + const AZStd::string imageColor = HooksUtils::ValueOfAny(cameraPluginParams, rawImageParamNames, "camera_image_color"); + const AZStd::string colorInfo = HooksUtils::ValueOfAny(cameraPluginParams, rawInfoParamNames, "camera_color_info"); + HooksUtils::AddTopicConfiguration( - sensorConfiguration, "camera_image_color", CameraConstants::ImageMessageType, CameraConstants::ColorImageConfig); + sensorConfiguration, imageColor, CameraConstants::ImageMessageType, CameraConstants::ColorImageConfig); HooksUtils::AddTopicConfiguration( - sensorConfiguration, "color_camera_info", CameraConstants::CameraInfoMessageType, CameraConstants::ColorInfoConfig); + sensorConfiguration, colorInfo, CameraConstants::CameraInfoMessageType, CameraConstants::ColorInfoConfig); } if (sdfSensor.Type() != sdf::SensorType::CAMERA) { // DEPTH_CAMERA and RGBD_CAMERA + const static AZStd::vector depthImageParamNames = { "image_depth", "depthImageTopicName" }; + const static AZStd::vector depthInfoParamNames = { "camera_info_depth", "depthImageCameraInfoTopicName" }; + + const AZStd::string imageDepth = HooksUtils::ValueOfAny(cameraPluginParams, depthImageParamNames, "camera_image_depth"); + const AZStd::string depthInfo = HooksUtils::ValueOfAny(cameraPluginParams, depthInfoParamNames, "depth_camera_info"); + HooksUtils::AddTopicConfiguration( - sensorConfiguration, "camera_image_depth", CameraConstants::ImageMessageType, CameraConstants::DepthImageConfig); + sensorConfiguration, imageDepth, CameraConstants::ImageMessageType, CameraConstants::DepthImageConfig); HooksUtils::AddTopicConfiguration( - sensorConfiguration, "depth_camera_info", CameraConstants::CameraInfoMessageType, CameraConstants::DepthInfoConfig); + sensorConfiguration, depthInfo, CameraConstants::CameraInfoMessageType, CameraConstants::DepthInfoConfig); } + element->Get("visualize", sensorConfiguration.m_visualize, false); + + // Get frame configuration + const auto frameConfiguration = HooksUtils::GetFrameConfiguration(cameraPluginParams); // Create required components - HooksUtils::CreateComponent(entity); + HooksUtils::CreateComponent(entity, frameConfiguration); // Create Camera component if (HooksUtils::CreateComponent(entity, sensorConfiguration, cameraConfiguration)) @@ -88,7 +116,7 @@ namespace ROS2::SDFormat } else { - return AZ::Failure(AZStd::string("Failed to create ROS2 Camera Sensor component")); + return AZ::Failure(AZStd::string("Failed to create ROS 2 Camera Sensor component")); } }; diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2GNSSSensorHook.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2GNSSSensorHook.cpp index a9b57ae30..8aa726366 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2GNSSSensorHook.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2GNSSSensorHook.cpp @@ -7,6 +7,7 @@ */ #include +#include #include #include @@ -19,9 +20,11 @@ namespace ROS2::SDFormat { SensorImporterHook importerHook; importerHook.m_sensorTypes = AZStd::unordered_set{ sdf::SensorType::NAVSAT }; - importerHook.m_supportedSensorParams = AZStd::unordered_set{ ">pose", ">update_rate" }; + importerHook.m_supportedSensorParams = AZStd::unordered_set{ ">pose", ">update_rate", ">topic", ">visualize" }; importerHook.m_pluginNames = AZStd::unordered_set{ "libgazebo_ros_gps_sensor.so" }; - importerHook.m_supportedPluginParams = AZStd::unordered_set{}; + importerHook.m_supportedPluginParams = AZStd::unordered_set{ ">ros>remapping", ">ros>argument", ">ros>frame_name", + ">ros>namespace", ">frameName", ">robotNamespace", + ">updateRate" }; importerHook.m_sdfSensorToComponentCallback = [](AZ::Entity& entity, const sdf::Sensor& sdfSensor) -> SensorImporterHook::ConvertSensorOutcome { @@ -30,10 +33,24 @@ namespace ROS2::SDFormat return AZ::Failure(AZStd::string("Failed to read parsed SDFormat data of %s NavSat sensor", sdfSensor.Name().c_str())); } + const auto gnssPluginParams = HooksUtils::GetPluginParams(sdfSensor.Plugins()); + const auto element = sdfSensor.Element(); + SensorConfiguration sensorConfiguration; - sensorConfiguration.m_frequency = sdfSensor.UpdateRate(); + sensorConfiguration.m_frequency = HooksUtils::GetFrequency(gnssPluginParams); const AZStd::string messageType = "sensor_msgs::msg::NavSatFix"; - HooksUtils::AddTopicConfiguration(sensorConfiguration, "gnss", messageType, messageType); + + // setting gnss topic + const AZStd::string messageTopic = HooksUtils::GetTopicName(gnssPluginParams, element, "gnss"); + element->Get("visualize", sensorConfiguration.m_visualize, false); + + HooksUtils::AddTopicConfiguration(sensorConfiguration, messageTopic, messageType, messageType); + + // Get frame configuration + const auto frameConfiguration = HooksUtils::GetFrameConfiguration(gnssPluginParams); + + // Create required components + HooksUtils::CreateComponent(entity, frameConfiguration); if (HooksUtils::CreateComponent(entity, sensorConfiguration)) { @@ -41,7 +58,7 @@ namespace ROS2::SDFormat } else { - return AZ::Failure(AZStd::string("Failed to create ROS2 GNSS Sensor component")); + return AZ::Failure(AZStd::string("Failed to create ROS 2 GNSS Sensor component")); } }; diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2ImuSensorHook.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2ImuSensorHook.cpp index 74efb92fa..b3ce3b205 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2ImuSensorHook.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2ImuSensorHook.cpp @@ -10,7 +10,7 @@ #include #include #include -#include +#include #include #include @@ -34,9 +34,13 @@ namespace ROS2::SDFormat ">imu>linear_acceleration>y>noise>mean", ">imu>linear_acceleration>y>noise>stddev", ">imu>linear_acceleration>z>noise>mean", - ">imu>linear_acceleration>z>noise>stddev" }; + ">imu>linear_acceleration>z>noise>stddev", + ">topic", + ">visualize" }; importerHook.m_pluginNames = AZStd::unordered_set{ "libgazebo_ros_imu_sensor.so" }; - importerHook.m_supportedPluginParams = AZStd::unordered_set{}; + importerHook.m_supportedPluginParams = + AZStd::unordered_set{ ">topicName", ">ros>remapping", ">ros>argument", ">ros>frame_name", + ">ros>namespace", ">frameName", ">robotNamespace", ">updateRate" }; importerHook.m_sdfSensorToComponentCallback = [](AZ::Entity& entity, const sdf::Sensor& sdfSensor) -> SensorImporterHook::ConvertSensorOutcome { @@ -77,15 +81,26 @@ namespace ROS2::SDFormat } } + const auto imuPluginParams = HooksUtils::GetPluginParams(sdfSensor.Plugins()); + const auto element = sdfSensor.Element(); + SensorConfiguration sensorConfiguration; - sensorConfiguration.m_frequency = sdfSensor.UpdateRate(); + sensorConfiguration.m_frequency = HooksUtils::GetFrequency(imuPluginParams); const AZStd::string messageType = "sensor_msgs::msg::Imu"; - HooksUtils::AddTopicConfiguration(sensorConfiguration, "imu", messageType, messageType); - // Create required components - HooksUtils::CreateComponent(entity); - HooksUtils::CreateComponent(entity); + // setting imu topic + const AZStd::string messageTopic = HooksUtils::GetTopicName(imuPluginParams, element, "imu"); + element->Get("visualize", sensorConfiguration.m_visualize, false); + + HooksUtils::AddTopicConfiguration(sensorConfiguration, messageTopic, messageType, messageType); + // Get frame configuration + const auto frameConfiguration = HooksUtils::GetFrameConfiguration(imuPluginParams); + + // Create required components + HooksUtils::CreateComponent(entity, frameConfiguration); + HooksUtils::CreateComponent(entity); + // Create Imu component if (HooksUtils::CreateComponent(entity, sensorConfiguration, imuConfiguration)) { @@ -93,7 +108,7 @@ namespace ROS2::SDFormat } else { - return AZ::Failure(AZStd::string("Failed to create ROS2 Imu Sensor component")); + return AZ::Failure(AZStd::string("Failed to create ROS 2 Imu Sensor component")); } }; diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2JointPoseTrajectoryModelHook.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2JointPoseTrajectoryModelHook.cpp new file mode 100644 index 000000000..a538fdeb9 --- /dev/null +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2JointPoseTrajectoryModelHook.cpp @@ -0,0 +1,66 @@ +/* + * Copyright (c) Contributors to the Open 3D Engine Project. + * For complete copyright and license terms please see the LICENSE at the root of this distribution. + * + * SPDX-License-Identifier: Apache-2.0 OR MIT + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace ROS2::SDFormat +{ + ModelPluginImporterHook ROS2ModelPluginHooks::ROS2JointPoseTrajectoryModel() + { + ModelPluginImporterHook importerHook; + importerHook.m_pluginNames = AZStd::unordered_set{ "libgazebo_ros_joint_pose_trajectory.so" }; + importerHook.m_supportedPluginParams = + AZStd::unordered_set{ ">topicName", ">ros>remapping", ">ros>argument", ">updateRate", ">update_rate" }; + + importerHook.m_sdfPluginToComponentCallback = + [](AZ::Entity& entity, const sdf::Plugin& sdfPlugin, const sdf::Model& sdfModel, const CreatedEntitiesMap& createdEntities) + -> ModelPluginImporterHook::ConvertPluginOutcome + { + const auto poseTrajectoryParams = HooksUtils::GetPluginParams({ sdfPlugin }); + + // configure JointsManipulationEditorComponent publisher + PublisherConfiguration publisherConfiguration; + publisherConfiguration.m_frequency = HooksUtils::GetFrequency(poseTrajectoryParams); + publisherConfiguration.m_topicConfiguration.m_type = "sensor_msgs::msg::JointState"; + publisherConfiguration.m_topicConfiguration.m_topic = "joint_states"; + + // configure trajectory action name + const static AZStd::vector trajectoryTopicParamNames = { "set_joint_trajectory", "topicName" }; + const AZStd::string trajectoryActionName = + HooksUtils::ValueOfAny(poseTrajectoryParams, trajectoryTopicParamNames, "arm_controller/follow_joint_trajectory"); + + // add required components + HooksUtils::CreateComponent(entity); + + // create controllerComponent based on model joints/articulations + entity.FindComponent() + ? HooksUtils::CreateComponent(entity) + : HooksUtils::CreateComponent(entity); + + HooksUtils::CreateComponent(entity, publisherConfiguration); + + if (HooksUtils::CreateComponent(entity, trajectoryActionName)) + { + return AZ::Success(); + } + else + { + return AZ::Failure(AZStd::string("Failed to create ROS 2 Joint Pose Trajectory Component")); + } + }; + + return importerHook; + } +} // namespace ROS2::SDFormat diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2JointStatePublisherModelHook.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2JointStatePublisherModelHook.cpp new file mode 100644 index 000000000..bc50d6d1a --- /dev/null +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2JointStatePublisherModelHook.cpp @@ -0,0 +1,64 @@ +/* + * Copyright (c) Contributors to the Open 3D Engine Project. + * For complete copyright and license terms please see the LICENSE at the root of this distribution. + * + * SPDX-License-Identifier: Apache-2.0 OR MIT + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace ROS2::SDFormat +{ + ModelPluginImporterHook ROS2ModelPluginHooks::ROS2JointStatePublisherModel() + { + ModelPluginImporterHook importerHook; + importerHook.m_pluginNames = AZStd::unordered_set{ "libgazebo_ros_joint_state_publisher.so" }; + importerHook.m_supportedPluginParams = + AZStd::unordered_set{ ">updateRate", ">update_rate", ">ros>argument", ">ros>remapping", ">topicName" }; + + importerHook.m_sdfPluginToComponentCallback = + [](AZ::Entity& entity, const sdf::Plugin& sdfPlugin, const sdf::Model& sdfModel, const CreatedEntitiesMap& createdEntities) + -> ModelPluginImporterHook::ConvertPluginOutcome + { + const auto statePublisherParams = HooksUtils::GetPluginParams({ sdfPlugin }); + + // configure publisher + PublisherConfiguration publisherConfiguration; + publisherConfiguration.m_frequency = HooksUtils::GetFrequency(statePublisherParams); + publisherConfiguration.m_topicConfiguration.m_type = "sensor_msgs::msg::JointState"; + + const static AZStd::vector topicParamNames = { "topicName", "joint_states" }; + publisherConfiguration.m_topicConfiguration.m_topic = + HooksUtils::ValueOfAny(statePublisherParams, topicParamNames, "joint_states"); + + // add required components + HooksUtils::CreateComponent(entity); + + // create controllerComponent based on model joints/articulations + entity.FindComponent() + ? HooksUtils::CreateComponent(entity) + : HooksUtils::CreateComponent(entity); + + if (HooksUtils::CreateComponent(entity, publisherConfiguration)) + { + return AZ::Success(); + } + else + { + return AZ::Failure(AZStd::string("Failed to create ROS 2 Joint State Publisher Component")); + } + }; + + return importerHook; + } +} // namespace ROS2::SDFormat diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2LidarSensorHook.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2LidarSensorHook.cpp index 200f92f0e..d7ad7d976 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2LidarSensorHook.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2LidarSensorHook.cpp @@ -43,9 +43,12 @@ namespace ROS2::SDFormat ">ray>scan>vertical>resolution", ">ray>range>min", ">ray>range>max", - }; + ">topic", + ">visualize" }; importerHook.m_pluginNames = AZStd::unordered_set{ "libgazebo_ros_ray_sensor.so", "libgazebo_ros_laser.so" }; - importerHook.m_supportedPluginParams = AZStd::unordered_set{}; + importerHook.m_supportedPluginParams = + AZStd::unordered_set{ ">topicName", ">ros>remapping", ">ros>argument", ">updateRate", + ">ros>frame_name", ">ros>namespace", ">frameName", ">robotNamespace" }; importerHook.m_sdfSensorToComponentCallback = [](AZ::Entity& entity, const sdf::Sensor& sdfSensor) -> SensorImporterHook::ConvertSensorOutcome { @@ -55,12 +58,18 @@ namespace ROS2::SDFormat return AZ::Failure(AZStd::string("Failed to read parsed SDFormat data of %s Lidar sensor", sdfSensor.Name().c_str())); } - const bool is2DLidar = (lidarSensor->VerticalScanSamples() == 1); + const auto lidarPluginParams = HooksUtils::GetPluginParams(sdfSensor.Plugins()); + const auto element = sdfSensor.Element(); SensorConfiguration sensorConfiguration; - sensorConfiguration.m_frequency = sdfSensor.UpdateRate(); + sensorConfiguration.m_frequency = HooksUtils::GetFrequency(lidarPluginParams); + const bool is2DLidar = (lidarSensor->VerticalScanSamples() == 1); const AZStd::string messageType = is2DLidar ? "sensor_msgs::msg::LaserScan" : "sensor_msgs::msg::PointCloud2"; - const AZStd::string messageTopic = is2DLidar ? "scan" : "pc"; + + // setting lidar topic + const AZStd::string messageTopic = HooksUtils::GetTopicName(lidarPluginParams, element, (is2DLidar ? "scan" : "pc")); + element->Get("visualize", sensorConfiguration.m_visualize, false); + HooksUtils::AddTopicConfiguration(sensorConfiguration, messageTopic, messageType, messageType); LidarSensorConfiguration lidarConfiguration{ is2DLidar ? LidarTemplateUtils::Get2DModels() @@ -94,8 +103,11 @@ namespace ROS2::SDFormat "GPU Lidar requires RGL Gem, see https://github.com/RobotecAI/o3de-rgl-gem for more details.\n"); } + // Get frame configuration + const auto frameConfiguration = HooksUtils::GetFrameConfiguration(lidarPluginParams); + // Create required components - HooksUtils::CreateComponent(entity); + HooksUtils::CreateComponent(entity, frameConfiguration); // Create Lidar component const auto lidarComponent = is2DLidar @@ -107,7 +119,7 @@ namespace ROS2::SDFormat } else { - return AZ::Failure(AZStd::string("Failed to create ROS2 Lidar Sensor component")); + return AZ::Failure(AZStd::string("Failed to create ROS 2 Lidar Sensor component")); } }; diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2ModelPluginHooks.h b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2ModelPluginHooks.h index a89236881..f36aa42e7 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2ModelPluginHooks.h +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2ModelPluginHooks.h @@ -12,12 +12,16 @@ namespace ROS2::SDFormat::ROS2ModelPluginHooks { - //! Retrieve a model plugin importer hook which is used to map SDFormat model plugin of type skid steering drive into O3DE - //! components. + //! Get a mapping of SDFormat skid_steer_drive and diff_drive plugins into O3DE components. ModelPluginImporterHook ROS2SkidSteeringModel(); - //! Retrieve a model plugin importer hook which is used to map SDFormat model plugin of type Ackermann model drive into O3DE - //! components. + //! Get a mapping of SDFormat ackermann_drive plugin into O3DE components. ModelPluginImporterHook ROS2AckermannModel(); + //! Get a mapping of SDFormat joint_state_publisher plugin into O3DE components. + ModelPluginImporterHook ROS2JointStatePublisherModel(); + + //! Get a mapping of SDFormat joint_pose_trajectory plugin into O3DE components. + ModelPluginImporterHook ROS2JointPoseTrajectoryModel(); + } // namespace ROS2::SDFormat::ROS2ModelPluginHooks diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SDFormatHooksUtils.cpp b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SDFormatHooksUtils.cpp index e83f09e46..b9d0ee7dc 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SDFormatHooksUtils.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SDFormatHooksUtils.cpp @@ -9,10 +9,12 @@ #include "ROS2SDFormatHooksUtils.h" #include #include +#include #include #include #include #include +#include #include @@ -98,4 +100,166 @@ namespace ROS2::SDFormat "CreatePrefabFromUrdfOrSdf", "Setting transform failed: %s does not have transform interface\n", sdfSensor.Name().c_str()); } } + + namespace HooksUtils::PluginParser + { + // Inserts name (key) and value (val) of given parameter to map. + void ParseRegularContent(const sdf::Element& content, HooksUtils::PluginParams& remappings) + { + std::string contentName = content.GetName(); + std::string contentValue = content.GetValue()->GetAsString(); + if (contentName.empty() || contentValue.empty()) + { + AZ_Warning("PluginParser", false, "Encountered empty parameter value while parsing URDF/SDF plugin."); + return; + } + AZStd::string key(contentName.c_str(), contentName.size()); + AZStd::string val(contentValue.c_str(), contentValue.size()); + remappings[key] = val; + } + + // Parses parameters present in ros element, inserting them to the map. + void ParseRos2Remapping(const sdf::Element& rosContent, HooksUtils::PluginParams& remappings) + { + if (rosContent.GetName() != "remapping" && rosContent.GetName() != "argument") + { + // parameter other than remapping or argument can be handled as regular parameter + ParseRegularContent(rosContent, remappings); + return; + } + AZStd::string contentValue = rosContent.GetValue()->GetAsString().c_str(); + + if (contentValue.find_last_of('=') == std::string::npos || contentValue.find_last_of(':') == std::string::npos) + { + AZ_Warning("PluginParser", false, "Encountered invalid remapping while parsing URDF/SDF plugin."); + return; + } + + // get new name of the topic + int startVal = contentValue.find_last_of('='); + if (contentValue.find_last_of('/') != std::string::npos && contentValue.find_last_of('/') > startVal) + { + startVal = contentValue.find_last_of("/"); + } + startVal += 1; + + if (startVal >= contentValue.size()) + { + AZ_Warning("PluginParser", false, "Encountered invalid (empty) remapping while parsing URDF/SDF plugin."); + return; + } + AZStd::string newTopic = contentValue.substr(startVal, contentValue.size() - startVal); + + // get previous name of the topic + contentValue = contentValue.substr(0, contentValue.find_first_of(':')); + + int startKey = contentValue.find_last_of('/') != std::string::npos ? contentValue.find_last_of('/') + 1 : 0; + if (startKey >= contentValue.size()) + { + AZ_Warning("PluginParser", false, "Encountered invalid (empty) remapping while parsing URDF/SDF plugin."); + return; + } + AZStd::string prevTopic = contentValue.substr(startKey, contentValue.size() - startKey); + + if (ROS2Names::ValidateTopic(newTopic).IsSuccess() && ROS2Names::ValidateTopic(prevTopic).IsSuccess()) + { + remappings[prevTopic] = newTopic; + } + else + { + AZ_Warning("PluginParser", false, "Encountered invalid topic name while parsing URDF/SDF plugin."); + } + } + } // namespace HooksUtils::PluginParser + + ROS2FrameConfiguration HooksUtils::GetFrameConfiguration(const HooksUtils::PluginParams& pluginParams) + { + ROS2FrameConfiguration frameConfiguration; + + const static AZStd::vector namespaceRemapNames = { "robotNamespace", "namespace" }; + const AZStd::string remappedNamespace = HooksUtils::ValueOfAny(pluginParams, namespaceRemapNames); + if (!ROS2Names::ValidateNamespace(remappedNamespace).IsSuccess()) + { + AZ_Warning("PluginParser", false, "Encountered invalid namespace name while parsing URDF/SDF plugin."); + } + else if (!remappedNamespace.empty()) + { + frameConfiguration.m_namespaceConfiguration.SetNamespace(remappedNamespace, NamespaceConfiguration::NamespaceStrategy::Custom); + } + + if (pluginParams.contains("frameName")) + { + frameConfiguration.m_frameName = pluginParams.at("frameName"); + } + else if (pluginParams.contains("frame_name")) + { + frameConfiguration.m_frameName = pluginParams.at("frame_name"); + } + + return frameConfiguration; + } + + HooksUtils::PluginParams HooksUtils::GetPluginParams(const sdf::Plugins& plugins) + { + HooksUtils::PluginParams remappings; + if (plugins.empty()) + { + return remappings; + } + + const auto plugin = plugins[0]; + + for (const auto& content : plugin.Contents()) + { + std::string contentName = content->GetName(); + if (contentName == "ros") + { + // when ros tag is present, parse it's elements and insert them into the map + auto rosContent = content->GetFirstElement(); + while (rosContent != nullptr) + { + PluginParser::ParseRos2Remapping(*rosContent, remappings); + rosContent = rosContent->GetNextElement(); + } + } + else + { + PluginParser::ParseRegularContent(*content, remappings); + } + } + + return remappings; + } + + AZStd::string HooksUtils::ValueOfAny( + const HooksUtils::PluginParams& pluginParams, const AZStd::vector& paramNames, const AZStd::string& defaultVal) + { + for (const auto& paramName : paramNames) + { + if (pluginParams.contains(paramName)) + { + const AZStd::string paramValue = pluginParams.at(paramName); + return AZ::IO::PathView(paramValue).Filename().String(); + } + } + return defaultVal; + } + + AZStd::string HooksUtils::GetTopicName(const PluginParams& pluginParams, sdf::ElementPtr element, const AZStd::string& defaultVal) + { + if (element->HasElement("topic")) + { + return element->Get("topic").c_str(); + } + + const static AZStd::vector remapParamNames = { "topicName", "out" }; + return ValueOfAny(pluginParams, remapParamNames, defaultVal); + } + + float HooksUtils::GetFrequency(const PluginParams& pluginParams, const float defaultVal) + { + const static AZStd::vector frequencyParamNames = { "updateRate", "update_rate" }; + return AZStd::stof(ValueOfAny(pluginParams, frequencyParamNames, AZStd::to_string(defaultVal))); + } + } // namespace ROS2::SDFormat diff --git a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SDFormatHooksUtils.h b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SDFormatHooksUtils.h index 810064008..12c5c97a1 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SDFormatHooksUtils.h +++ b/Gems/ROS2/Code/Source/RobotImporter/SDFormat/ROS2SDFormatHooksUtils.h @@ -15,6 +15,7 @@ #include #include #include +#include #include #include #include @@ -111,5 +112,41 @@ namespace ROS2::SDFormat return nullptr; } + + using PluginParams = AZStd::unordered_map; + + //! Get frame configuration from given plugin params + //! @param pluginParams parameters of the plugin for which frame is created + //! @return configuration of the frame + ROS2FrameConfiguration GetFrameConfiguration(const HooksUtils::PluginParams& pluginParams); + + //! Find all parameters given in plugin element. + //! Given a ROS 2 remapping argument, extracts only names of + //! elements to be remapped, ignoring their namespaces. + //! @param plugin plugin to extract parameters from + //! @return a map of parameters present in plugin + PluginParams GetPluginParams(const sdf::Plugins& plugins); + + //! Find value of any of specified plugin parameters. + //! @param pluginParams map of plugin parameters defined in model description + //! @param paramNames vector of parameter names in query + //! @param defaultVal value to be returned when none of the parameters are present in the map + //! @return value on any of the query parameters or defaultVal when none were present + AZStd::string ValueOfAny( + const PluginParams& pluginParams, const AZStd::vector& paramNames, const AZStd::string& defaultVal = ""); + + //! Get the name of element's general topic after remapping. + //! @param pluginParams map of plugin parameters + //! @param element pointer to the sdf element + //! @param defaultVal value to be returned when no remaps of the topic are present in the map + //! @return remapped topic name or defaultVal when no remaps are present + AZStd::string GetTopicName(const PluginParams& pluginParams, sdf::ElementPtr element, const AZStd::string& defaultVal = ""); + + //! Get publisher frequency from plugin. + //! @param pluginParams map of plugin parameters + //! @param defaultVal value to be returned when frequency param does not appear in pluginParams + //! @return publisher frequency or defaultVal when frequency is not specified by element description + float GetFrequency(const PluginParams& pluginParams, const float defaultVal = 10.0); + } // namespace HooksUtils } // namespace ROS2::SDFormat diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/SensorsMaker.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/SensorsMaker.cpp index ec75dbf94..6c9bbef7a 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/SensorsMaker.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/URDF/SensorsMaker.cpp @@ -49,7 +49,8 @@ namespace ROS2 if (sensorResult.IsSuccess()) { const auto sensorElement = sensor->Element(); - const auto& unsupportedSensorParams = Utils::SDFormat::GetUnsupportedParams(sensorElement, hook->m_supportedSensorParams); + const auto& unsupportedSensorParams = Utils::SDFormat::GetUnsupportedParams( + sensorElement, hook->m_supportedSensorParams, hook->m_pluginNames, hook->m_supportedPluginParams); AZStd::string status; if (unsupportedSensorParams.empty()) { diff --git a/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp b/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp index 689bab852..1ee631c1c 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp +++ b/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.cpp @@ -954,18 +954,29 @@ namespace ROS2::Utils::SDFormat } AZStd::vector GetUnsupportedParams( - const sdf::ElementPtr& rootElement, const AZStd::unordered_set& supportedParams) + const sdf::ElementPtr& rootElement, + const AZStd::unordered_set& supportedParams, + const AZStd::unordered_set& pluginNames, + const AZStd::unordered_set& supportedPluginParams) { AZStd::vector unsupportedParams; - AZStd::function elementVisitor = - [&](const sdf::ElementPtr& elementPtr, const std::string& prefix) -> void + AZStd::function& supportedSet)> + elementVisitor = [&](const sdf::ElementPtr& elementPtr, + const std::string& info, + const std::string& prefix, + const AZStd::unordered_set& supportedSet) -> void { auto childPtr = elementPtr->GetFirstElement(); AZStd::string prefixAz(prefix.c_str(), prefix.size()); - if (!childPtr && !prefixAz.empty() && !supportedParams.contains(prefixAz)) + if (!childPtr && !prefixAz.empty() && !supportedSet.contains(prefixAz)) { + prefixAz.insert(0, AZStd::string(info.c_str(), info.size())); unsupportedParams.push_back(prefixAz); } @@ -980,12 +991,37 @@ namespace ROS2::Utils::SDFormat currentName.append(">"); currentName.append(childPtr->GetName()); - elementVisitor(childPtr, currentName); + elementVisitor(childPtr, info, currentName, supportedSet); childPtr = childPtr->GetNextElement(); } }; - elementVisitor(rootElement, ""); + elementVisitor(rootElement, "", "", supportedParams); + + auto pluginPtr = rootElement->GetFirstElement(); + while (pluginPtr) + { + if (pluginPtr->GetName() == "plugin") + { + if (pluginPtr->HasAttribute("filename")) + { + std::string fileName = pluginPtr->GetAttribute("filename")->GetAsString(); + std::string info = "plugin \"" + fileName + "\""; + + AZStd::string fileNameAz(fileName.c_str(), fileName.size()); + if (!pluginNames.contains(fileNameAz)) + { + unsupportedParams.push_back(AZStd::string(info.c_str(), info.size())); + } + else + { + info.append(": "); + elementVisitor(pluginPtr, info, "", supportedPluginParams); + } + } + } + pluginPtr = pluginPtr->GetNextElement(); + } return unsupportedParams; } @@ -1024,20 +1060,21 @@ namespace ROS2::Utils::SDFormat // If any files couldn't be found using our supplied prefix mappings, this callback will get called. // Attempt to use our full path resolution, and print a warning if it still couldn't be resolved. - sdfConfig.SetFindCallback([settings, baseFilePath](const std::string &fileName) -> std::string - { - auto amentPrefixPath = Utils::GetAmentPrefixPath(); - - auto resolved = Utils::ResolveAssetPath(AZ::IO::Path(fileName.c_str()), baseFilePath, amentPrefixPath, settings); - if (!resolved.empty()) + sdfConfig.SetFindCallback( + [settings, baseFilePath](const std::string& fileName) -> std::string { - AZ_Trace("SdfParserConfig", "SDF SetFindCallback resolved '%s' -> '%s'", fileName.c_str(), resolved.c_str()); - return resolved.c_str(); - } + auto amentPrefixPath = Utils::GetAmentPrefixPath(); + + auto resolved = Utils::ResolveAssetPath(AZ::IO::Path(fileName.c_str()), baseFilePath, amentPrefixPath, settings); + if (!resolved.empty()) + { + AZ_Trace("SdfParserConfig", "SDF SetFindCallback resolved '%s' -> '%s'", fileName.c_str(), resolved.c_str()); + return resolved.c_str(); + } - AZ_Warning("SdfParserConfig", false, "SDF SetFindCallback failed to resolve '%s'", fileName.c_str()); - return fileName; - }); + AZ_Warning("SdfParserConfig", false, "SDF SetFindCallback failed to resolve '%s'", fileName.c_str()); + return fileName; + }); return sdfConfig; } diff --git a/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.h b/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.h index 5716a086e..3cb5d37b0 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.h +++ b/Gems/ROS2/Code/Source/RobotImporter/Utils/RobotImporterUtils.h @@ -53,6 +53,7 @@ namespace ROS2::Utils //! always contain at least one element. The back() element of the stack returns the direct model the link is attached to //! @return Return true to continue visiting links or false to halt using LinkVisitorCallback = AZStd::function; + //! Visit links from URDF/SDF //! @param sdfModel Model object of SDF document corresponding to the tag. It used to query link //! @param visitNestedModelLinks When true recurses to any nested tags of the Model object and invoke visitor on their links as @@ -89,7 +90,7 @@ namespace ROS2::Utils AZStd::unordered_map GetAllJoints(const sdf::Model& sdfModel, bool gatherNestedModelJoints = false); //! Retrieve all joints from URDF/SDF in which the specified link is a child in a sdf::Joint. - //! @param sdfModel Model object of SDF document corresponding to the tag. It used to query joints + //! @param sdfModel Model object of SDF document corresponding to the tag. It is used to query joints //! @param linkName Name of link which to query in joint objects ChildName() //! @param gatherNestedModelJoints When true recurses to any nested tags of the Model object and also gathers their joints as //! well @@ -229,9 +230,14 @@ namespace ROS2::Utils::SDFormat //! Allows to store the list of unsupported parameters in metadata and logs. It is typically used with sensors and plugins. //! @param rootElement pointer to a root Element in parsed XML data that will be a subject to heuristics //! @param supportedParams set of predefined parameters that are supported + //! @param supportedPlugins set of predefined plugins that are supported + //! @param supportedPluginParams set of supported plugin params //! @returns list of unsupported parameters defined for given element AZStd::vector GetUnsupportedParams( - const sdf::ElementPtr& rootElement, const AZStd::unordered_set& supportedParams); + const sdf::ElementPtr& rootElement, + const AZStd::unordered_set& supportedParams, + const AZStd::unordered_set& supportedPlugins = AZStd::unordered_set(), + const AZStd::unordered_set& supportedPluginParams = AZStd::unordered_set()); //! Check if plugin is supported by using it's filename. The filepath is converted into the filename if necessary. //! @param plugin plugin in the parsed SDFormat data diff --git a/Gems/ROS2/Code/Tests/SdfParserTest.cpp b/Gems/ROS2/Code/Tests/SdfParserTest.cpp index d996fc319..c1107a4fa 100644 --- a/Gems/ROS2/Code/Tests/SdfParserTest.cpp +++ b/Gems/ROS2/Code/Tests/SdfParserTest.cpp @@ -556,25 +556,41 @@ namespace UnitTest { const auto& unsupportedCameraParams = ROS2::Utils::SDFormat::GetUnsupportedParams(cameraElement, cameraSupportedParams); - EXPECT_EQ(unsupportedCameraParams.size(), 3U); + EXPECT_EQ(unsupportedCameraParams.size(), 4U); EXPECT_EQ(unsupportedCameraParams[0U], ">pose"); EXPECT_EQ(unsupportedCameraParams[1U], ">camera>clip>near"); EXPECT_EQ(unsupportedCameraParams[2U], ">camera>clip>far"); + EXPECT_EQ(unsupportedCameraParams[3U], "plugin \"libgazebo_ros_camera.so\""); } cameraSupportedParams.emplace(">pose"); { const auto& unsupportedCameraParams = ROS2::Utils::SDFormat::GetUnsupportedParams(cameraElement, cameraSupportedParams); - EXPECT_EQ(unsupportedCameraParams.size(), 2U); + EXPECT_EQ(unsupportedCameraParams.size(), 3U); EXPECT_EQ(unsupportedCameraParams[0U], ">camera>clip>near"); EXPECT_EQ(unsupportedCameraParams[1U], ">camera>clip>far"); + EXPECT_EQ(unsupportedCameraParams[2U], "plugin \"libgazebo_ros_camera.so\""); } cameraSupportedParams.emplace(">camera>clip>near"); cameraSupportedParams.emplace(">camera>clip>far"); { const auto& unsupportedCameraParams = ROS2::Utils::SDFormat::GetUnsupportedParams(cameraElement, cameraSupportedParams); - EXPECT_EQ(unsupportedCameraParams.size(), 0U); + EXPECT_EQ(unsupportedCameraParams.size(), 1U); + EXPECT_EQ(unsupportedCameraParams[0U], "plugin \"libgazebo_ros_camera.so\""); + } + + { + const auto& unsupportedCameraParams = ROS2::Utils::SDFormat::GetUnsupportedParams(cameraElement, cameraSupportedParams, supportedPlugins); + EXPECT_EQ(unsupportedCameraParams.size(), 1U); + EXPECT_EQ(unsupportedCameraParams[0U], "plugin \"libgazebo_ros_camera.so\": >camera_name"); + } + + AZStd::unordered_set supportedCameraPluginParams = {">camera_name", ">ros>remapping"}; + { + const auto& unsupportedCameraParams = ROS2::Utils::SDFormat::GetUnsupportedParams( + cameraElement, cameraSupportedParams, supportedPlugins, supportedCameraPluginParams); + EXPECT_EQ(unsupportedCameraParams.size(), 0U); } const AZStd::unordered_set laserSupportedParams{ ">pose", @@ -588,8 +604,9 @@ namespace UnitTest ">ray>range>resolution", ">always_on", ">visualize" }; - const auto& unsupportedLaserParams = ROS2::Utils::SDFormat::GetUnsupportedParams(laserElement, laserSupportedParams); - EXPECT_EQ(unsupportedLaserParams.size(), 0U); + const auto& unsupportedLaserParams = ROS2::Utils::SDFormat::GetUnsupportedParams(laserElement, laserSupportedParams, supportedPlugins); + EXPECT_EQ(unsupportedLaserParams.size(), 1U); + EXPECT_EQ(unsupportedLaserParams[0U], "plugin \"librayplugin.so\""); } TEST_F(SdfParserTest, SensorPluginImporterHookCheck) @@ -604,10 +621,10 @@ namespace UnitTest const sdf::ElementPtr cameraElement = sdfModel->LinkByName("link1")->SensorByIndex(0U)->Element(); const auto& cameraImporterHook = ROS2::SDFormat::ROS2SensorHooks::ROS2CameraSensor(); - const auto& unsupportedCameraParams = - ROS2::Utils::SDFormat::GetUnsupportedParams(cameraElement, cameraImporterHook.m_supportedSensorParams); + const auto& unsupportedCameraParams = ROS2::Utils::SDFormat::GetUnsupportedParams( + cameraElement, cameraImporterHook.m_supportedSensorParams, cameraImporterHook.m_pluginNames, cameraImporterHook.m_supportedPluginParams); EXPECT_EQ(unsupportedCameraParams.size(), 1U); - EXPECT_EQ(unsupportedCameraParams[0U], ">pose"); + EXPECT_EQ(unsupportedCameraParams[0U], "plugin \"libgazebo_ros_camera.so\": >camera_name"); sdf::Plugin plug; plug.SetName("test_camera"); @@ -626,14 +643,12 @@ namespace UnitTest const sdf::ElementPtr lidarElement = sdfModel->LinkByName("link2")->SensorByIndex(0U)->Element(); const auto& lidarImporterHook = ROS2::SDFormat::ROS2SensorHooks::ROS2LidarSensor(); - const auto& unsupportedLidarParams = - ROS2::Utils::SDFormat::GetUnsupportedParams(lidarElement, lidarImporterHook.m_supportedSensorParams); - EXPECT_EQ(unsupportedLidarParams.size(), 5U); + const auto& unsupportedLidarParams = ROS2::Utils::SDFormat::GetUnsupportedParams( + lidarElement, lidarImporterHook.m_supportedSensorParams, lidarImporterHook.m_pluginNames, lidarImporterHook.m_supportedPluginParams); + EXPECT_EQ(unsupportedLidarParams.size(), 3U); EXPECT_EQ(unsupportedLidarParams[0U], ">always_on"); - EXPECT_EQ(unsupportedLidarParams[1U], ">visualize"); - EXPECT_EQ(unsupportedLidarParams[2U], ">pose"); - EXPECT_EQ(unsupportedLidarParams[3U], ">ray>scan>horizontal>resolution"); - EXPECT_EQ(unsupportedLidarParams[4U], ">ray>range>resolution"); + EXPECT_EQ(unsupportedLidarParams[1U], ">ray>range>resolution"); + EXPECT_EQ(unsupportedLidarParams[2U], "plugin \"librayplugin.so\""); } { const auto xmlStr = GetSdfWithImuSensor(); @@ -645,8 +660,8 @@ namespace UnitTest const sdf::ElementPtr imuElement = sdfModel->LinkByName("link1")->SensorByIndex(0U)->Element(); const auto& importerHook = ROS2::SDFormat::ROS2SensorHooks::ROS2ImuSensor(); - const auto& unsupportedImuParams = - ROS2::Utils::SDFormat::GetUnsupportedParams(imuElement, importerHook.m_supportedSensorParams); + const auto& unsupportedImuParams = ROS2::Utils::SDFormat::GetUnsupportedParams( + imuElement, importerHook.m_supportedSensorParams, importerHook.m_pluginNames, importerHook.m_supportedPluginParams); EXPECT_EQ(unsupportedImuParams.size(), 1U); EXPECT_EQ(unsupportedImuParams[0U], ">always_on"); } diff --git a/Gems/ROS2/Code/ros2_editor_files.cmake b/Gems/ROS2/Code/ros2_editor_files.cmake index 0bc954e24..f0bfe1474 100644 --- a/Gems/ROS2/Code/ros2_editor_files.cmake +++ b/Gems/ROS2/Code/ros2_editor_files.cmake @@ -44,6 +44,8 @@ set(FILES Source/RobotImporter/SDFormat/Hooks/ROS2ImuSensorHook.cpp Source/RobotImporter/SDFormat/Hooks/ROS2LidarSensorHook.cpp Source/RobotImporter/SDFormat/Hooks/ROS2SkidSteeringModelHook.cpp + Source/RobotImporter/SDFormat/Hooks/ROS2JointPoseTrajectoryModelHook.cpp + Source/RobotImporter/SDFormat/Hooks/ROS2JointStatePublisherModelHook.cpp Source/RobotImporter/SDFormat/ROS2ModelPluginHooks.h Source/RobotImporter/SDFormat/ROS2SDFormatHooksUtils.cpp Source/RobotImporter/SDFormat/ROS2SDFormatHooksUtils.h