diff --git a/CMakeLists.txt b/CMakeLists.txt index 1cb7828..49ff17d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -101,6 +101,7 @@ set(LIBRARIES MrsGazeboCommonResources_FluidResistancePlugin MrsGazeboCommonResources_MotorSpeedRepublisherPlugin MrsGazeboCommonResources_MotorPropModelPlugin + MrsGazeboCommonResources_LinkStaticTFPublisher ) catkin_package( @@ -371,6 +372,18 @@ target_link_libraries(MrsGazeboCommonResources_MotorPropModelPlugin ${Boost_LIBRARIES} ) +# LinkStaticTFPublisher + +add_library(MrsGazeboCommonResources_LinkStaticTFPublisher SHARED + src/sensor_and_model_plugins/link_static_tf_publisher.cpp + ) + +target_link_libraries(MrsGazeboCommonResources_LinkStaticTFPublisher + ${GAZEBO_LIBRARIES} + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} + ) + ## -------------------------------------------------------------- ## | Install | ## -------------------------------------------------------------- diff --git a/src/sensor_and_model_plugins/link_static_tf_publisher.cpp b/src/sensor_and_model_plugins/link_static_tf_publisher.cpp new file mode 100644 index 0000000..981ac15 --- /dev/null +++ b/src/sensor_and_model_plugins/link_static_tf_publisher.cpp @@ -0,0 +1,82 @@ +/* Mostly generated using ChatGPT 3.5 */ + +#include +#include +#include +#include +#include +#include +#include + +namespace gazebo +{ + +class LinkStaticTFPublisher : public ModelPlugin { +private: + physics::ModelPtr model; + tf2_ros::StaticTransformBroadcaster tf_broadcaster; + +public: + LinkStaticTFPublisher() { + } + + virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) { + // Safety check + if (!_model) { + ROS_ERROR("[LinkStaticTFPublisher]: Invalid model pointer!"); + return; + } + + model = _model; + + std::string parentLinkName; + std::string childLinkName; + + if (_sdf->HasElement("parentLink")) { + parentLinkName = _sdf->Get("parentLink"); + }else{ + ROS_ERROR("[LinkStaticTFPublisher]: SDF is missing element \"parentLink\""); + return; + } + + if (_sdf->HasElement("childLink")) { + childLinkName = _sdf->Get("childLink"); + }else{ + ROS_ERROR("[LinkStaticTFPublisher]: SDF is missing element \"childLink\""); + return; + } + + // Get pointers to the specified parent and child links + physics::LinkPtr parentLink = model->GetLink(parentLinkName); + physics::LinkPtr childLink = model->GetLink(childLinkName); + + // Check if both links are valid + if (!parentLink || !childLink) { + ROS_ERROR_STREAM("[LinkStaticTFPublisher]: One or both of the links \"" << parentLinkName << "\", \"" << childLinkName << "\" do not exist!"); + return; + } + + // Get the transform between the parent and child links + ignition::math::Pose3d relativePose = childLink->WorldPose() - parentLink->WorldPose(); + + tf2::Quaternion quat(relativePose.Rot().X(), relativePose.Rot().Y(), relativePose.Rot().Z(), relativePose.Rot().W()); + + // Publish the transform + geometry_msgs::TransformStamped transformStamped; + transformStamped.header.stamp = ros::Time::now(); + transformStamped.header.frame_id = parentLinkName; + transformStamped.child_frame_id = childLinkName; + transformStamped.transform.translation.x = relativePose.Pos().X(); + transformStamped.transform.translation.y = relativePose.Pos().Y(); + transformStamped.transform.translation.z = relativePose.Pos().Z(); + transformStamped.transform.rotation.w = relativePose.Rot().W(); + transformStamped.transform.rotation.x = relativePose.Rot().X(); + transformStamped.transform.rotation.y = relativePose.Rot().Y(); + transformStamped.transform.rotation.z = relativePose.Rot().Z(); + tf_broadcaster.sendTransform(transformStamped); + ROS_INFO_STREAM("[LinkStaticTFPublisher]: Published static TF between frames \"" << parentLinkName << "\", \"" << childLinkName << "\""); + } +}; + +GZ_REGISTER_MODEL_PLUGIN(LinkStaticTFPublisher) +} // namespace gazebo