1. cpp 编写
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <ros/ros.h>
#include <std_msgs/Float64.h>namespace gazebo
{class PoleJointAnglePublisher : public ModelPlugin{public:void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf){// 确保ROS节点只初始化一次if (!ros::isInitialized()){int argc = 0;char **argv = nullptr;ros::init(argc, argv, "pole_joint_angle_publisher",ros::init_options::NoSigintHandler);}// 从SDF参数中获取关节名称if (_sdf->HasElement("jointName")){this->jointName = _sdf->Get<std::string>("jointName");}else{gzerr << "Please specify a jointName parameter." << std::endl;return;}// 获取关节this->joint = _model->GetJoint(this->jointName);if (!this->joint){gzerr << "Joint '" << this->jointName << "' not found!" << std::endl;return;}// 创建ROS节点this->rosNode.reset(new ros::NodeHandle("pole_joint_angle_publisher"));// 创建话题发布者this->jointAnglePub = this->rosNode->advertise<std_msgs::Float64>(this->jointName + "_angle", 1000);// 创建更新连接this->updateConnection = event::Events::ConnectWorldUpdateBegin(std::bind(&PoleJointAnglePublisher::OnUpdate, this));}public:void OnUpdate(){// 获取当前关节角度double angle = this->joint->Position(0);// 创建ROS消息并发布std_msgs::Float64 msg;msg.data = angle;this->jointAnglePub.publish(msg);}private:std::string jointName;physics::JointPtr joint;std::unique_ptr<ros::NodeHandle> rosNode;ros::Publisher jointAnglePub;event::ConnectionPtr updateConnection;};// 注册插件GZ_REGISTER_MODEL_PLUGIN(PoleJointAnglePublisher)
}
2. cmakelist.txt
cmake_minimum_required(VERSION 2.8.3)project(carcar)find_package(catkin REQUIRED COMPONENTSroscppstd_msgs
)find_package(gazebo REQUIRED)catkin_package()include_directories(${catkin_INCLUDE_DIRS}${GAZEBO_INCLUDE_DIRS}
)link_directories(${GAZEBO_LIBRARY_DIRS})find_package(roslaunch)foreach(dir config launch meshes urdf)install(DIRECTORY ${dir}/DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
endforeach(dir)add_library(print_joint_angle_plugin SHARED src/print_joint_angle.cpp)
target_link_libraries(print_joint_angle_plugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})3. 使用
在xacro文件中添加编译生成的插件：
<gazebo><plugin name="pole_joint_angle_publisher" filename="libprint_joint_angle_plugin.so"><jointName>pole_joint</jointName></plugin></gazebo>
pole_joint是传入的随动关节名称。
然后启动gazebo之后，关节pole_joint的角度值会发布在/pole_joint_angle_publisher/pole_joint_angle话题中。
)
