diff --git a/seerep_ros/seerep_ros_conversions_fb/include/seerep_ros_conversions_fb/conversions.h b/seerep_ros/seerep_ros_conversions_fb/include/seerep_ros_conversions_fb/conversions.h index 3f419db83..78d84098e 100644 --- a/seerep_ros/seerep_ros_conversions_fb/include/seerep_ros_conversions_fb/conversions.h +++ b/seerep_ros/seerep_ros_conversions_fb/include/seerep_ros_conversions_fb/conversions.h @@ -323,6 +323,13 @@ toFlat(const sensor_msgs::CameraInfo& ci, std::string& projectuuid, std::string& msgUuid, double maxViewingDistance, flatbuffers::grpc::MessageBuilder& builder); +/** + * @brief Converts a Flatbuffer CameraIntrinsics message to the corresponding + * ROS sensor_msgs::CameraInfo message + * @param ci Flatbuffer CameraIntrinsics message + * @return ROS sensor_msgs::CameraInfo + */ +sensor_msgs::CameraInfo toROS(const seerep::fb::CameraIntrinsics& ci); /** * @brief Converts a ROS sensor_msgs::RegionOfInterest message to the * corresponding gRPC Flatbuffer RegionOfInterest message @@ -334,7 +341,13 @@ flatbuffers::Offset toFlat(const sensor_msgs::RegionOfInterest& roi, flatbuffers::grpc::MessageBuilder& builder); - +/** + * @brief Converts a Flatbuffer CameraIntrinsics message to the corresponding + * ROS sensor_msgs::CameraInfo message + * @param roi Flatbuffer RegionOfInterest message + * @return ROS sensor_msgs::RegionOfInterest + */ +sensor_msgs::RegionOfInterest toROS(const seerep::fb::RegionOfInterest& roi); } /* namespace seerep_ros_conversions_fb */ #endif /* SEEREP_ROS_CONVERSIONS_FB */ diff --git a/seerep_ros/seerep_ros_conversions_fb/src/seerep_ros_conversions_fb/conversions.cpp b/seerep_ros/seerep_ros_conversions_fb/src/seerep_ros_conversions_fb/conversions.cpp index c7f3c4a80..010a17c1e 100644 --- a/seerep_ros/seerep_ros_conversions_fb/src/seerep_ros_conversions_fb/conversions.cpp +++ b/seerep_ros/seerep_ros_conversions_fb/src/seerep_ros_conversions_fb/conversions.cpp @@ -427,6 +427,26 @@ toFlat(const sensor_msgs::CameraInfo& ci, std::string& projectuuid, cibuilder.add_height(ci.height); return cibuilder.Finish(); } + +sensor_msgs::CameraInfo toROS(const seerep::fb::CameraIntrinsics& ci) +{ + sensor_msgs::CameraInfo ret; + ret.header = toROS(*ci.header()); + ret.height = ci.height(); + ret.width = ci.width(); + + ret.distortion_model = ci.distortion_model()->str(); + ret.binning_x = ci.binning_x(); + ret.binning_y = ci.binning_y(); + std::copy_n(ci.distortion()->Data(), ci.distortion()->size(), + std::back_inserter(ret.D)); + std::memcpy(&ret.K, ci.intrinsic_matrix()->Data(), sizeof(ret.K)); + std::memcpy(&ret.R, ci.rectification_matrix()->Data(), sizeof(ret.R)); + std::memcpy(&ret.P, ci.projection_matrix()->Data(), sizeof(ret.P)); + ret.roi = toROS(*ci.region_of_interest()); + return ret; +} + flatbuffers::Offset toFlat(const sensor_msgs::RegionOfInterest& roi, @@ -440,4 +460,15 @@ toFlat(const sensor_msgs::RegionOfInterest& roi, roiBuilder.add_y_offset(roi.y_offset); return roiBuilder.Finish(); } + +sensor_msgs::RegionOfInterest toROS(const seerep::fb::RegionOfInterest& roi) +{ + sensor_msgs::RegionOfInterest ret; + ret.x_offset = roi.x_offset(); + ret.y_offset = roi.y_offset(); + ret.height = roi.height(); + ret.width = roi.width(); + ret.do_rectify = roi.do_rectify(); + return ret; +} } // namespace seerep_ros_conversions_fb