编写控制器，控制上述模型在gazebo中运动

为了在Gazebo中控制模型运动，需要编写一个控制器。以下是一个简单的Python控制器示例，可以使模型在x、y和z方向上来回移动：

#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Twist

class ModelController:
    def __init__(self):
        rospy.init_node('model_controller')
        self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
        self.rate = rospy.Rate(10)
        self.command = Twist()
        self.direction = 1.0
        self.distance = 0.0
        self.max_distance = 10.0

    def run(self):
        while not rospy.is_shutdown():
            self.command.linear.x = self.direction * 0.1
            self.command.linear.y = self.direction * 0.1
            self.command.linear.z = self.direction * 0.1
            self.pub.publish(self.command)
            self.distance += abs(self.command.linear.x)
            if self.distance >= self.max_distance:
                self.direction *= -1.0
                self.distance = 0.0
            self.rate.sleep()

if __name__ == '__main__':
    try:
        controller = ModelController()
        controller.run()
    except rospy.ROSInterruptException:
        pass

该控制器使用ROS Python API，通过发布Twist消息到/cmd_vel主题来控制模型运动。在run()方法中，控制器将模型向前移动，直到达到最大距离，然后将方向反转，继续移动。控制器以每秒10次的速度运行，可通过调整rate值来更改此速度。

要在Gazebo中使用该控制器，请将其保存为.py文件，并将其添加到模型的URDF文件中。在URDF文件中，添加一个controller节点，将该节点的type属性设置为"joint_state_controller/JointController"，并将该节点的name属性设置为您选择的名称。然后，将该节点的joint属性设置为您要控制的关节的名称。最后，将该节点的command属性设置为您保存的控制器文件的路径。以下是一个示例URDF文件，其中包含该控制器：

<?xml version="1.0"?>
<robot name="my_robot">

  <link name="base_link"/>

  <joint name="joint1" type="revolute">
    <parent link="base_link"/>
    <child link="link1"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="-1.57" upper="1.57" effort="100" velocity="3.14"/>
  </joint>

  <link name="link1"/>

  <gazebo>
    <plugin name="joint_state_controller" filename="libgazebo_ros_joint_state_controller.so">
      <robotNamespace>/my_robot</robotNamespace>
      <jointName>joint1</jointName>
    </plugin>
    <plugin name="model_controller" filename="path/to/your/controller.py">
      <robotNamespace>/my_robot</robotNamespace>
      <jointName>joint1</jointName>
    </plugin>
  </gazebo>

</robot>

在该示例中，控制器节点被添加到gazebo节点下的plugin节点中。控制器节点的名称为"model_controller"，它将控制关节"joint1"的运动。注意，控制器节点的robotNamespace属性必须与ROS命名空间中的模型名称匹配。

保存URDF文件并将其加载到Gazebo中后，您应该可以通过使用以下命令来启动控制器：

roslaunch my_robot my_robot.launch

在新的终端中，运行控制器节点：

rosrun my_robot model_controller.py

您应该会看到模型在Gazebo中移动。