Skip to content

Fix VisionComponent Image & Optical Frame Transforms #26

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open

Fix VisionComponent Image & Optical Frame Transforms #26

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
4e170b1
FIX use RelativeLocation & RelativeRotation for image frame transforms
Tim-Fronsee Mar 11, 2020
ad073ee
ADD restore negative X and Z quaternion
Tim-Fronsee Mar 12, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
56 changes: 23 additions & 33 deletions Source/ROSIntegrationVision/Private/VisionComponent.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -198,22 +198,23 @@ void UVisionComponent::TickComponent(float DeltaTime,
TimePassed -= FrameTime;
MEASURE_TIME("Tick");

auto owner = GetOwner();
auto owner = GetOwner();
owner->UpdateComponentTransforms();

FDateTime Now = FDateTime::UtcNow();
Priv->Buffer->HeaderWrite->TimestampCapture = Now.ToUnixTimestamp() * 1000000000 + Now.GetMillisecond() * 1000000;

FVector Translation = GetComponentLocation();
FQuat Rotation = GetComponentQuat();
// Convert to meters and ROS coordinate system
FVector Translation = GetRelativeLocation();
FQuat Quat = FQuat(GetRelativeRotation());

// Convert to meters and ROS coordinate system in relation to the owner's transform.
Priv->Buffer->HeaderWrite->Translation.X = Translation.X / 100.0f;
Priv->Buffer->HeaderWrite->Translation.Y = -Translation.Y / 100.0f;
Priv->Buffer->HeaderWrite->Translation.Z = Translation.Z / 100.0f;
Priv->Buffer->HeaderWrite->Rotation.X = -Rotation.X;
Priv->Buffer->HeaderWrite->Rotation.Y = Rotation.Y;
Priv->Buffer->HeaderWrite->Rotation.Z = -Rotation.Z;
Priv->Buffer->HeaderWrite->Rotation.W = Rotation.W;
Priv->Buffer->HeaderWrite->Rotation.X = -Quat.X;
Priv->Buffer->HeaderWrite->Rotation.Y = Quat.Y;
Priv->Buffer->HeaderWrite->Rotation.Z = -Quat.Z;
Priv->Buffer->HeaderWrite->Rotation.W = Quat.W;

// Start writing to buffer
Priv->Buffer->StartWriting(ObjectToColor, ObjectColors);
Expand Down Expand Up @@ -293,14 +294,6 @@ void UVisionComponent::TickComponent(float DeltaTime,

Priv->Buffer->DoneReading();

double x = Priv->Buffer->HeaderRead->Translation.X;
double y = Priv->Buffer->HeaderRead->Translation.Y;
double z = Priv->Buffer->HeaderRead->Translation.Z;
double rx = Priv->Buffer->HeaderRead->Rotation.X;
double ry = Priv->Buffer->HeaderRead->Rotation.Y;
double rz = Priv->Buffer->HeaderRead->Rotation.Z;
double rw = Priv->Buffer->HeaderRead->Rotation.W;

if (!DisableTFPublishing) {
// Start advertising TF only if it has yet to advertise.
if (!TFPublisher->IsAdvertising())
Expand All @@ -313,35 +306,32 @@ void UVisionComponent::TickComponent(float DeltaTime,
TransformImage.header.time = time;
TransformImage.header.frame_id = ParentLink;
TransformImage.child_frame_id = ImageFrame;
TransformImage.transform.translation.x = x;
TransformImage.transform.translation.y = y;
TransformImage.transform.translation.z = z;
TransformImage.transform.rotation.x = rx;
TransformImage.transform.rotation.y = ry;
TransformImage.transform.rotation.z = rz;
TransformImage.transform.rotation.w = rw;
TransformImage.transform.translation.x = Priv->Buffer->HeaderRead->Translation.X;
TransformImage.transform.translation.y = Priv->Buffer->HeaderRead->Translation.Y;
TransformImage.transform.translation.z = Priv->Buffer->HeaderRead->Translation.Z;
TransformImage.transform.rotation.x = Priv->Buffer->HeaderRead->Rotation.X;
TransformImage.transform.rotation.y = Priv->Buffer->HeaderRead->Rotation.Y;
TransformImage.transform.rotation.z = Priv->Buffer->HeaderRead->Rotation.Z;
TransformImage.transform.rotation.w = Priv->Buffer->HeaderRead->Rotation.W;

TFImageFrame->transforms.Add(TransformImage);

TFPublisher->Publish(TFImageFrame);

// Publish optical frame
FRotator CameraLinkRotator(0.0, -90.0, 90.0);
FQuat CameraLinkQuaternion(CameraLinkRotator);
// Publish optical frame with a fixed joint connecting to the Image frame.
FRotator OpticalRotator(0.0, -90.0, 90.0);
FQuat OpticalQuat(OpticalRotator);

TSharedPtr<ROSMessages::tf2_msgs::TFMessage> TFOpticalFrame(new ROSMessages::tf2_msgs::TFMessage());
ROSMessages::geometry_msgs::TransformStamped TransformOptical;
TransformOptical.header.seq = 0;
TransformOptical.header.time = time;
TransformOptical.header.frame_id = ImageFrame;
TransformOptical.child_frame_id = ImageOpticalFrame;
TransformOptical.transform.translation.x = 0;
TransformOptical.transform.translation.y = 0;
TransformOptical.transform.translation.z = 0;
TransformOptical.transform.rotation.x = CameraLinkQuaternion.X;
TransformOptical.transform.rotation.y = CameraLinkQuaternion.Y;
TransformOptical.transform.rotation.z = CameraLinkQuaternion.Z;
TransformOptical.transform.rotation.w = CameraLinkQuaternion.W;
TransformOptical.transform.rotation.x = OpticalQuat.X;
TransformOptical.transform.rotation.y = OpticalQuat.Y;
TransformOptical.transform.rotation.z = OpticalQuat.Z;
TransformOptical.transform.rotation.w = OpticalQuat.W;

TFOpticalFrame->transforms.Add(TransformOptical);

Expand Down