Skip to content

Commit 0ab6b9d

Browse files
committed
fixed camera tf name inconsistency and camera tf orientation
1 parent f3ffa07 commit 0ab6b9d

File tree

2 files changed

+14
-2
lines changed

2 files changed

+14
-2
lines changed

ros2/src/fsds_ros2_bridge/src/airsim_ros_wrapper.cpp

Lines changed: 13 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -764,8 +764,20 @@ void AirsimROSWrapper::append_static_camera_tf(const std::string& vehicle_name,
764764
static_cam_tf_body_msg.transform.translation.x = camera_setting.position.x();
765765
static_cam_tf_body_msg.transform.translation.y = camera_setting.position.y();
766766
static_cam_tf_body_msg.transform.translation.z = camera_setting.position.z();
767+
768+
// Existing quaternion from camera settings
767769
tf2::Quaternion quat;
768-
quat.setRPY(math_common::deg2rad(camera_setting.rotation.roll), math_common::deg2rad(camera_setting.rotation.pitch), math_common::deg2rad(camera_setting.rotation.yaw));
770+
quat.setRPY(math_common::deg2rad(camera_setting.rotation.roll),
771+
math_common::deg2rad(camera_setting.rotation.pitch),
772+
math_common::deg2rad(camera_setting.rotation.yaw));
773+
774+
// Rotation fix for ROS convention
775+
tf2::Quaternion additional_rotation(-0.5, 0.5, -0.5, 0.5);
776+
777+
// Multiply the quaternions to apply the additional rotation
778+
quat *= additional_rotation;
779+
780+
// Assign the result to the message
769781
static_cam_tf_body_msg.transform.rotation.x = quat.x();
770782
static_cam_tf_body_msg.transform.rotation.y = quat.y();
771783
static_cam_tf_body_msg.transform.rotation.z = quat.z();

ros2/src/fsds_ros2_bridge/src/fsds_ros2_bridge_camera.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -180,7 +180,7 @@ int main(int argc, char ** argv)
180180

181181
// load settings
182182
camera_name = nh->declare_parameter<std::string>("camera_name", "");
183-
camera_frame_prefix = nh->declare_parameter<std::string>("camera_frame_prefix", "/fsds/camera");
183+
camera_frame_prefix = nh->declare_parameter<std::string>("camera_frame_prefix", "/fsds/");
184184
camera_frame_id = camera_frame_prefix + camera_name;
185185

186186
framerate = nh->declare_parameter<double>("framerate", 0.0);

0 commit comments

Comments
 (0)