Skip to content

Commit

Permalink
Merge pull request #4466 from cyberbotics/fix4453
Browse files Browse the repository at this point in the history
fix4453
  • Loading branch information
zimmy87 authored Apr 29, 2022
2 parents 1261b07 + a9e8e36 commit 28b559d
Show file tree
Hide file tree
Showing 2 changed files with 20 additions and 24 deletions.
1 change: 1 addition & 0 deletions ros2/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h
100644 → 100755
Original file line number Diff line number Diff line change
Expand Up @@ -249,6 +249,7 @@ class AirsimROSWrapper
void set_nans_to_zeros_in_pose(VehicleSetting& vehicle_setting) const;
void set_nans_to_zeros_in_pose(const VehicleSetting& vehicle_setting, CameraSetting& camera_setting) const;
void set_nans_to_zeros_in_pose(const VehicleSetting& vehicle_setting, LidarSetting& lidar_setting) const;
geometry_msgs::msg::Transform get_camera_optical_tf_from_body_tf(const geometry_msgs::msg::Transform& body_tf) const;

/// utils. todo parse into an Airlib<->ROS conversion class
tf2::Quaternion get_tf2_quat(const msr::airlib::Quaternionr& airlib_quat) const;
Expand Down
43 changes: 19 additions & 24 deletions ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1154,6 +1154,21 @@ void AirsimROSWrapper::convert_tf_msg_to_enu(geometry_msgs::msg::TransformStampe
tf_msg.transform.rotation.z = -tf_msg.transform.rotation.z;
}

geometry_msgs::msg::Transform AirsimROSWrapper::get_camera_optical_tf_from_body_tf(const geometry_msgs::msg::Transform& body_tf) const
{
geometry_msgs::msg::Transform optical_tf = body_tf; //same translation
auto opticalQ = msr::airlib::Quaternionr(optical_tf.rotation.w, optical_tf.rotation.x, optical_tf.rotation.y, optical_tf.rotation.z);
if (isENU_)
opticalQ *= msr::airlib::Quaternionr(0.7071068, -0.7071068, 0, 0); //CamOptical in CamBodyENU is rmat[1,0,0;0,0,-1;0,1,0]==xyzw[-0.7071068,0,0,0.7071068]
else
opticalQ *= msr::airlib::Quaternionr(0.5, 0.5, 0.5, 0.5); //CamOptical in CamBodyNED is rmat[0,0,1;1,0,0;0,1,0]==xyzw[0.5,0.5,0.5,0.5]
optical_tf.rotation.w = opticalQ.w();
optical_tf.rotation.x = opticalQ.x();
optical_tf.rotation.y = opticalQ.y();
optical_tf.rotation.z = opticalQ.z();
return optical_tf;
}

void AirsimROSWrapper::append_static_vehicle_tf(VehicleROS* vehicle_ros, const VehicleSetting& vehicle_setting)
{
geometry_msgs::msg::TransformStamped vehicle_tf_msg;
Expand Down Expand Up @@ -1194,18 +1209,10 @@ void AirsimROSWrapper::append_static_camera_tf(VehicleROS* vehicle_ros, const st
convert_tf_msg_to_enu(static_cam_tf_body_msg);
}

geometry_msgs::msg::TransformStamped static_cam_tf_optical_msg = static_cam_tf_body_msg;
geometry_msgs::msg::TransformStamped static_cam_tf_optical_msg;
static_cam_tf_optical_msg.header.frame_id = static_cam_tf_body_msg.header.frame_id;
static_cam_tf_optical_msg.child_frame_id = camera_name + "_optical/static";

tf2::Quaternion quat_cam_body;
tf2::Quaternion quat_cam_optical;
tf2::convert(static_cam_tf_body_msg.transform.rotation, quat_cam_body);
tf2::Matrix3x3 mat_cam_body(quat_cam_body);
tf2::Matrix3x3 mat_cam_optical;
mat_cam_optical.setValue(mat_cam_body.getColumn(1).getX(), mat_cam_body.getColumn(2).getX(), mat_cam_body.getColumn(0).getX(), mat_cam_body.getColumn(1).getY(), mat_cam_body.getColumn(2).getY(), mat_cam_body.getColumn(0).getY(), mat_cam_body.getColumn(1).getZ(), mat_cam_body.getColumn(2).getZ(), mat_cam_body.getColumn(0).getZ());
mat_cam_optical.getRotation(quat_cam_optical);
quat_cam_optical.normalize();
tf2::convert(quat_cam_optical, static_cam_tf_optical_msg.transform.rotation);
static_cam_tf_optical_msg.transform = get_camera_optical_tf_from_body_tf(static_cam_tf_body_msg.transform);

vehicle_ros->static_tf_msg_vec_.emplace_back(static_cam_tf_body_msg);
vehicle_ros->static_tf_msg_vec_.emplace_back(static_cam_tf_optical_msg);
Expand Down Expand Up @@ -1367,19 +1374,7 @@ void AirsimROSWrapper::publish_camera_tf(const ImageResponse& img_response, cons
cam_tf_optical_msg.header.stamp = rclcpp::Time(img_response.time_stamp);
cam_tf_optical_msg.header.frame_id = frame_id;
cam_tf_optical_msg.child_frame_id = child_frame_id + "_optical";
cam_tf_optical_msg.transform.translation = cam_tf_body_msg.transform.translation;

tf2::Quaternion quat_cam_body;
tf2::Quaternion quat_cam_optical;
tf2::convert(cam_tf_body_msg.transform.rotation, quat_cam_body);
tf2::Matrix3x3 mat_cam_body(quat_cam_body);
// tf2::Matrix3x3 mat_cam_optical = matrix_cam_body_to_optical_ * mat_cam_body * matrix_cam_body_to_optical_inverse_;
// tf2::Matrix3x3 mat_cam_optical = matrix_cam_body_to_optical_ * mat_cam_body;
tf2::Matrix3x3 mat_cam_optical;
mat_cam_optical.setValue(mat_cam_body.getColumn(1).getX(), mat_cam_body.getColumn(2).getX(), mat_cam_body.getColumn(0).getX(), mat_cam_body.getColumn(1).getY(), mat_cam_body.getColumn(2).getY(), mat_cam_body.getColumn(0).getY(), mat_cam_body.getColumn(1).getZ(), mat_cam_body.getColumn(2).getZ(), mat_cam_body.getColumn(0).getZ());
mat_cam_optical.getRotation(quat_cam_optical);
quat_cam_optical.normalize();
tf2::convert(quat_cam_optical, cam_tf_optical_msg.transform.rotation);
cam_tf_optical_msg.transform = get_camera_optical_tf_from_body_tf(cam_tf_body_msg.transform);

tf_broadcaster_->sendTransform(cam_tf_body_msg);
tf_broadcaster_->sendTransform(cam_tf_optical_msg);
Expand Down

0 comments on commit 28b559d

Please sign in to comment.