Skip to content

Commit

Permalink
RobotImporter features: topic names, namespaces, handling plugins, jo…
Browse files Browse the repository at this point in the history 
…ints (#734)

RobotImporter features: topic names, namespaces, handling plugins, joints.

---------

Signed-off-by: Paulina Kubera <[email protected]>
Co-authored-by: Jan Hanca <[email protected]>
Co-authored-by: Adam Dabrowski <[email protected]>
  • Loading branch information
@c00kiepreferences @jhanca-robotecai @adamdbrw
3 people committed Aug 13, 2024
1 parent 4d23b6e commit fcce637
Show file tree
Hide file tree
Showing 20 changed files with 565 additions and 83 deletions.
5 changes: 5 additions & 0 deletions Gems/ROS2/Code/Source/Manipulation/JointsManipulationEditorComponent.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<JointsManipulationComponent>(
Expand Down
1 change: 1 addition & 0 deletions Gems/ROS2/Code/Source/Manipulation/JointsManipulationEditorComponent.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@ namespace ROS2
{
public:
JointsManipulationEditorComponent();
JointsManipulationEditorComponent(const PublisherConfiguration& publisherConfiguration);
~JointsManipulationEditorComponent() = default;
AZ_EDITOR_COMPONENT(JointsManipulationEditorComponent, "{BF2F77FD-92FB-4730-92C7-DDEE54F508BF}");

Expand Down
5 changes: 5 additions & 0 deletions Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,11 @@

namespace ROS2
{
JointsTrajectoryComponent::JointsTrajectoryComponent(const AZStd::string& followTrajectoryActionName)
: m_followTrajectoryActionName(followTrajectoryActionName)
{
}

void JointsTrajectoryComponent::Activate()
{
auto* ros2Frame = GetEntity()->FindComponent<ROS2FrameComponent>();
Expand Down
1 change: 1 addition & 0 deletions Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down
4 changes: 3 additions & 1 deletion Gems/ROS2/Code/Source/RobotImporter/ROS2RobotImporterEditorSystemComponent.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<AZ::ComponentApplicationRequests>::Get()->GetSerializeContext();
Expand Down
14 changes: 7 additions & 7 deletions Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2AckermannModelHook.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -158,12 +158,12 @@ namespace ROS2::SDFormat
VehicleDynamics::VehicleConfiguration GetConfiguration(
const sdf::ElementPtr element, const sdf::Model& sdfModel, const CreatedEntitiesMap& createdEntities)
{
const AZStd::array<std::string, Wheels::Total> jointNamesSDFormat{ { "front_left_joint",
"front_right_joint",
"rear_left_joint",
"rear_right_joint",
"left_steering_joint",
"right_steering_joint" } };
const static AZStd::array<std::string, Wheels::Total> 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
{
Expand Down Expand Up @@ -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"));
}
};

Expand Down
52 changes: 40 additions & 12 deletions Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2CameraSensorHook.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,14 +22,22 @@ namespace ROS2::SDFormat
SensorImporterHook importerHook;
importerHook.m_sensorTypes =
AZStd::unordered_set<sdf::SensorType>{ sdf::SensorType::CAMERA, sdf::SensorType::DEPTH_CAMERA, sdf::SensorType::RGBD_CAMERA };
importerHook.m_supportedSensorParams = AZStd::unordered_set<AZStd::string>{
">pose", ">update_rate", ">camera>horizontal_fov", ">camera>image>width", ">camera>image>height", ">camera>clip>near",
">camera>clip>far"
};
importerHook.m_supportedSensorParams = AZStd::unordered_set<AZStd::string>{ ">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<AZStd::string>{ "libgazebo_ros_camera.so",
"libgazebo_ros_depth_camera.so",
"libgazebo_ros_openni_kinect.so" };
importerHook.m_supportedPluginParams = AZStd::unordered_set<AZStd::string>{};
importerHook.m_supportedPluginParams = AZStd::unordered_set<AZStd::string>{
">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
{
Expand Down Expand Up @@ -61,25 +69,45 @@ namespace ROS2::SDFormat
cameraConfiguration.m_farClipDistance = static_cast<float>(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<AZStd::string> rawImageParamNames = { "image_raw", "imageTopicName" };
const static AZStd::vector<AZStd::string> 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<AZStd::string> depthImageParamNames = { "image_depth", "depthImageTopicName" };
const static AZStd::vector<AZStd::string> 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<bool>("visualize", sensorConfiguration.m_visualize, false);

// Get frame configuration
const auto frameConfiguration = HooksUtils::GetFrameConfiguration(cameraPluginParams);

// Create required components
HooksUtils::CreateComponent<ROS2FrameEditorComponent>(entity);
HooksUtils::CreateComponent<ROS2FrameEditorComponent>(entity, frameConfiguration);

// Create Camera component
if (HooksUtils::CreateComponent<ROS2CameraSensorEditorComponent>(entity, sensorConfiguration, cameraConfiguration))
Expand All @@ -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"));
}
};

Expand Down
27 changes: 22 additions & 5 deletions Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2GNSSSensorHook.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@
*/

#include <GNSS/ROS2GNSSSensorComponent.h>
#include <ROS2/Frame/ROS2FrameEditorComponent.h>
#include <RobotImporter/SDFormat/ROS2SDFormatHooksUtils.h>
#include <RobotImporter/SDFormat/ROS2SensorHooks.h>

Expand All @@ -19,9 +20,11 @@ namespace ROS2::SDFormat
{
SensorImporterHook importerHook;
importerHook.m_sensorTypes = AZStd::unordered_set<sdf::SensorType>{ sdf::SensorType::NAVSAT };
importerHook.m_supportedSensorParams = AZStd::unordered_set<AZStd::string>{ ">pose", ">update_rate" };
importerHook.m_supportedSensorParams = AZStd::unordered_set<AZStd::string>{ ">pose", ">update_rate", ">topic", ">visualize" };
importerHook.m_pluginNames = AZStd::unordered_set<AZStd::string>{ "libgazebo_ros_gps_sensor.so" };
importerHook.m_supportedPluginParams = AZStd::unordered_set<AZStd::string>{};
importerHook.m_supportedPluginParams = AZStd::unordered_set<AZStd::string>{ ">ros>remapping", ">ros>argument", ">ros>frame_name",
">ros>namespace", ">frameName", ">robotNamespace",
">updateRate" };
importerHook.m_sdfSensorToComponentCallback = [](AZ::Entity& entity,
const sdf::Sensor& sdfSensor) -> SensorImporterHook::ConvertSensorOutcome
{
Expand All @@ -30,18 +33,32 @@ 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<bool>("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<ROS2FrameEditorComponent>(entity, frameConfiguration);

if (HooksUtils::CreateComponent<ROS2GNSSSensorComponent>(entity, sensorConfiguration))
{
return AZ::Success();
}
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"));
}
};

Expand Down
33 changes: 24 additions & 9 deletions Gems/ROS2/Code/Source/RobotImporter/SDFormat/Hooks/ROS2ImuSensorHook.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@
#include <ROS2/Frame/ROS2FrameEditorComponent.h>
#include <RobotImporter/SDFormat/ROS2SDFormatHooksUtils.h>
#include <RobotImporter/SDFormat/ROS2SensorHooks.h>
#include <Source/EditorStaticRigidBodyComponent.h>
#include <Source/EditorArticulationLinkComponent.h>

#include <sdf/Imu.hh>
#include <sdf/Sensor.hh>
Expand All @@ -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<AZStd::string>{ "libgazebo_ros_imu_sensor.so" };
importerHook.m_supportedPluginParams = AZStd::unordered_set<AZStd::string>{};
importerHook.m_supportedPluginParams =
AZStd::unordered_set<AZStd::string>{ ">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
{
Expand Down Expand Up @@ -77,23 +81,34 @@ 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<ROS2FrameEditorComponent>(entity);
HooksUtils::CreateComponent<PhysX::EditorStaticRigidBodyComponent>(entity);
// setting imu topic
const AZStd::string messageTopic = HooksUtils::GetTopicName(imuPluginParams, element, "imu");
element->Get<bool>("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<ROS2FrameEditorComponent>(entity, frameConfiguration);
HooksUtils::CreateComponent<PhysX::EditorArticulationLinkComponent>(entity);

// Create Imu component
if (HooksUtils::CreateComponent<ROS2ImuSensorComponent>(entity, sensorConfiguration, imuConfiguration))
{
return AZ::Success();
}
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"));
}
};

Expand Down
Loading

0 comments on commit fcce637

Please sign in to comment.