Skip to content

Commit

Permalink
Camera info topic published (#33) (o3de#154)
Browse files Browse the repository at this point in the history 
* camera info topic published

Signed-off-by: Piotr Zyskowski <[email protected]>
  • Loading branch information
@piotr-zyskowski-rai
piotr-zyskowski-rai committed Jan 4, 2023
1 parent ae9bb1d commit 717d4b6
Show file tree
Hide file tree
Showing 4 changed files with 93 additions and 30 deletions.
54 changes: 41 additions & 13 deletions Gems/ROS2/Code/Source/Camera/CameraSensor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,12 +24,13 @@
namespace ROS2
{
CameraSensorDescription::CameraSensorDescription(const AZStd::string& cameraName, float verticalFov, int width, int height)
: verticalFieldOfViewDeg(verticalFov)
, width(width)
, height(height)
, cameraName(cameraName)
, aspectRatio(static_cast<float>(width) / static_cast<float>(height))
, viewToClipMatrix(MakeViewToClipMatrix())
: m_verticalFieldOfViewDeg(verticalFov)
, m_width(width)
, m_height(height)
, m_cameraName(cameraName)
, m_aspectRatio(static_cast<float>(width) / static_cast<float>(height))
, m_viewToClipMatrix(MakeViewToClipMatrix())
, m_cameraIntrinsics(MakeCameraIntrinsics())
{
validateParameters();
}
Expand All @@ -38,27 +39,49 @@ namespace ROS2
{
const float nearDist = 0.1f, farDist = 100.0f;
AZ::Matrix4x4 localViewToClipMatrix;
AZ::MakePerspectiveFovMatrixRH(localViewToClipMatrix, AZ::DegToRad(verticalFieldOfViewDeg), aspectRatio, nearDist, farDist, true);
AZ::MakePerspectiveFovMatrixRH(
localViewToClipMatrix, AZ::DegToRad(m_verticalFieldOfViewDeg), m_aspectRatio, nearDist, farDist, true);
return localViewToClipMatrix;
}

void CameraSensorDescription::validateParameters() const
{
AZ_Assert(
verticalFieldOfViewDeg > 0.0f && verticalFieldOfViewDeg < 180.0f, "Vertical fov should be in range 0.0 < FoV < 180.0 degrees");
AZ_Assert(!cameraName.empty(), "Camera name cannot be empty");
m_verticalFieldOfViewDeg > 0.0f && m_verticalFieldOfViewDeg < 180.0f,
"Vertical fov should be in range 0.0 < FoV < 180.0 degrees");
AZ_Assert(!m_cameraName.empty(), "Camera name cannot be empty");
}

std::array<double, 9> CameraSensorDescription::MakeCameraIntrinsics() const
{
// Intrinsic camera matrix of the camera image is being created here
// It is based on other parameters available in the structure - they must be initialized before this function is called
// Matrix is row-major and has the following form:
// [fx 0 cx]
// [ 0 fy cy]
// [ 0 0 1]
// Projects 3D points in the camera coordinate frame to 2D pixel
// coordinates using the focal lengths (fx, fy) and principal point
// (cx, cy).
const auto w = static_cast<double>(m_width);
const auto h = static_cast<double>(m_height);
const double horizontalFoV = 2.0 * AZStd::atan(AZStd::tan(m_verticalFieldOfViewDeg / 2.0) / m_aspectRatio);
const double focalLengthX = w / (2.0 * AZStd::tan(horizontalFoV / 2.0));
const double focalLengthY = h / (2.0 * AZStd::tan(m_verticalFieldOfViewDeg / 2.0));
return { focalLengthX, 0.0, w / 2.0, 0.0, focalLengthY, h / 2.0, 0.0, 0.0, 1.0 };
}

CameraSensor::CameraSensor(const CameraSensorDescription& cameraSensorDescription)
: m_cameraSensorDescription(cameraSensorDescription)
{
AZ_TracePrintf("CameraSensor", "Initializing pipeline for %s", cameraSensorDescription.cameraName.c_str());
AZ_TracePrintf("CameraSensor", "Initializing pipeline for %s", cameraSensorDescription.m_cameraName.c_str());

AZ::Name viewName = AZ::Name("MainCamera");
m_view = AZ::RPI::View::CreateView(viewName, AZ::RPI::View::UsageCamera);
m_view->SetViewToClipMatrix(cameraSensorDescription.viewToClipMatrix);
m_view->SetViewToClipMatrix(cameraSensorDescription.m_viewToClipMatrix);
m_scene = AZ::RPI::RPISystemInterface::Get()->GetSceneByName(AZ::Name("Main"));

AZStd::string pipelineName = cameraSensorDescription.cameraName + "Pipeline";
AZStd::string pipelineName = cameraSensorDescription.m_cameraName + "Pipeline";

AZ::RPI::RenderPipelineDescriptor pipelineDesc;
pipelineDesc.m_mainViewTagName = "MainCamera";
Expand All @@ -71,7 +94,7 @@ namespace ROS2

if (auto renderToTexturePass = azrtti_cast<AZ::RPI::RenderToTexturePass*>(m_pipeline->GetRootPass().get()))
{
renderToTexturePass->ResizeOutput(cameraSensorDescription.width, cameraSensorDescription.height);
renderToTexturePass->ResizeOutput(cameraSensorDescription.m_width, cameraSensorDescription.m_height);
}

m_scene->AddRenderPipeline(m_pipeline);
Expand Down Expand Up @@ -120,4 +143,9 @@ namespace ROS2
callback,
AZ::RPI::PassAttachmentReadbackOption::Output);
}

const CameraSensorDescription& CameraSensor::GetCameraSensorDescription() const
{
return m_cameraSensorDescription;
}
} // namespace ROS2
20 changes: 14 additions & 6 deletions Gems/ROS2/Code/Source/Camera/CameraSensor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,16 +25,20 @@ namespace ROS2
//! @param height - camera image height in pixels
CameraSensorDescription(const AZStd::string& cameraName, float verticalFov, int width, int height);

const float verticalFieldOfViewDeg; //!< camera vertical field of view
const int width; //!< camera image width in pixels
const int height; //!< camera image height in pixels
const AZStd::string cameraName; //!< camera name to differentiate cameras in a multi-camera setup
const float m_verticalFieldOfViewDeg; //!< camera vertical field of view
const int m_width; //!< camera image width in pixels
const int m_height; //!< camera image height in pixels
const AZStd::string m_cameraName; //!< camera name to differentiate cameras in a multi-camera setup

const float aspectRatio; //!< camera image aspect ratio; equal to (width / height)
const AZ::Matrix4x4 viewToClipMatrix; //!< camera view to clip space transform matrix; derived from other parameters
const float m_aspectRatio; //!< camera image aspect ratio; equal to (width / height)
const AZ::Matrix4x4 m_viewToClipMatrix; //!< camera view to clip space transform matrix; derived from other parameters
const std::array<double, 9> m_cameraIntrinsics; //!< camera intrinsics; derived from other parameters

private:
AZ::Matrix4x4 MakeViewToClipMatrix() const;

std::array<double, 9> MakeCameraIntrinsics() const;

void validateParameters() const;
};

Expand All @@ -57,7 +61,11 @@ namespace ROS2
void RequestFrame(
const AZ::Transform& cameraPose, std::function<void(const AZ::RPI::AttachmentReadback::ReadbackResult& result)> callback);

//! Function to get camera sensor description
[[nodiscard]] const CameraSensorDescription& GetCameraSensorDescription() const;

private:
CameraSensorDescription m_cameraSensorDescription;
AZStd::vector<AZStd::string> m_passHierarchy;
AZ::RPI::RenderPipelinePtr m_pipeline;
AZ::RPI::ViewPtr m_view;
Expand Down
47 changes: 36 additions & 11 deletions Gems/ROS2/Code/Source/Camera/ROS2CameraSensorComponent.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,23 +16,33 @@
#include <AzCore/Serialization/SerializeContext.h>

#include <Utilities/ROS2Names.h>
#include <sensor_msgs/distortion_models.hpp>
#include <sensor_msgs/image_encodings.hpp>

namespace ROS2
{
namespace Internal
{
const char* kImageMessageType = "sensor_msgs::msg::Image";
}
const char* kCameraInfoMessageType = "sensor_msgs::msg::CameraInfo";

AZStd::pair<AZStd::string, PublisherConfiguration> MakePublisherConfigurationPair(
const AZStd::string& topic, const AZStd::string& messageType)
{
PublisherConfiguration config;
config.m_topic = topic;
config.m_type = messageType;
return AZStd::make_pair(messageType, config);
}
} // namespace Internal

ROS2CameraSensorComponent::ROS2CameraSensorComponent()
{
PublisherConfiguration pc;
auto type = Internal::kImageMessageType;
pc.m_type = type;
pc.m_topic = "camera_image";
m_sensorConfiguration.m_frequency = 10;
m_sensorConfiguration.m_publishersConfigurations.insert(AZStd::make_pair(type, pc));
m_sensorConfiguration.m_publishersConfigurations.insert(
Internal::MakePublisherConfigurationPair("camera_image", Internal::kImageMessageType));
m_sensorConfiguration.m_publishersConfigurations.insert(
Internal::MakePublisherConfigurationPair("camera_info", Internal::kCameraInfoMessageType));
}

void ROS2CameraSensorComponent::Reflect(AZ::ReflectContext* context)
Expand Down Expand Up @@ -71,11 +81,17 @@ namespace ROS2
ROS2SensorComponent::Activate();

auto ros2Node = ROS2Interface::Get()->GetNode();
AZ_Assert(m_sensorConfiguration.m_publishersConfigurations.size() == 1, "Invalid configuration of publishers for camera sensor");
const auto publisherConfig = m_sensorConfiguration.m_publishersConfigurations[Internal::kImageMessageType];
AZStd::string fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic);

m_imagePublisher = ros2Node->create_publisher<sensor_msgs::msg::Image>(fullTopic.data(), publisherConfig.GetQoS());
const auto cameraImagePublisherConfig = m_sensorConfiguration.m_publishersConfigurations[Internal::kImageMessageType];
AZStd::string cameraImageFullTopic = ROS2Names::GetNamespacedName(GetNamespace(), cameraImagePublisherConfig.m_topic);
m_imagePublisher =
ros2Node->create_publisher<sensor_msgs::msg::Image>(cameraImageFullTopic.data(), cameraImagePublisherConfig.GetQoS());

const auto cameraInfoPublisherConfig = m_sensorConfiguration.m_publishersConfigurations[Internal::kCameraInfoMessageType];
AZStd::string cameraInfoFullTopic = ROS2Names::GetNamespacedName(GetNamespace(), cameraInfoPublisherConfig.m_topic);
AZ_TracePrintf("ROS2", "Creating publisher for camera info on topic %s", cameraInfoFullTopic.data());
m_cameraInfoPublisher =
ros2Node->create_publisher<sensor_msgs::msg::CameraInfo>(cameraInfoFullTopic.data(), cameraInfoPublisherConfig.GetQoS());

m_cameraSensor.emplace(CameraSensorDescription{ m_cameraName, m_VerticalFieldOfViewDeg, m_width, m_height });
}
Expand All @@ -100,15 +116,24 @@ namespace ROS2
{
const AZ::RHI::ImageDescriptor& descriptor = result.m_imageDescriptor;

AZStd::string frameName = GetEntity()->FindComponent<ROS2FrameComponent>()->GetFrameID();
sensor_msgs::msg::Image message;
message.encoding = sensor_msgs::image_encodings::RGBA8;

message.width = descriptor.m_size.m_width;
message.height = descriptor.m_size.m_height;
message.data = std::vector<uint8_t>(result.m_dataBuffer->data(), result.m_dataBuffer->data() + result.m_dataBuffer->size());
message.header.frame_id = GetEntity()->FindComponent<ROS2FrameComponent>()->GetFrameID().c_str();
message.header.frame_id = frameName.c_str();

m_imagePublisher->publish(message);

sensor_msgs::msg::CameraInfo cameraInfo;
cameraInfo.header.frame_id = frameName.c_str();
cameraInfo.width = descriptor.m_size.m_width;
cameraInfo.height = descriptor.m_size.m_height;
cameraInfo.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
cameraInfo.k = m_cameraSensor->GetCameraSensorDescription().m_cameraIntrinsics;
m_cameraInfoPublisher->publish(cameraInfo);
});
}
} // namespace ROS2
2 changes: 2 additions & 0 deletions Gems/ROS2/Code/Source/Camera/ROS2CameraSensorComponent.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
#pragma once

#include <rclcpp/publisher.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/msg/image.hpp>

#include "Sensor/ROS2SensorComponent.h"
Expand Down Expand Up @@ -45,6 +46,7 @@ namespace ROS2
void FrequencyTick() override;

std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::Image>> m_imagePublisher;
std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::CameraInfo>> m_cameraInfoPublisher;
std::optional<CameraSensor> m_cameraSensor;
};
} // namespace ROS2

0 comments on commit 717d4b6

Please sign in to comment.