gazebo中通过编写插件发布随动关节的角度值到话题

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 COMPONENTS
  roscpp
  std_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话题中。