ROS 2机器人操作系统与Gazebo机器人仿真（微课视频版）课件 第9章-其他类型机器人仿真简介 3-7节

ROS2机器人操作系统与Gazebo机器人仿真

第9章其它类型机器人仿真简介提纲9.1六足机器人仿真9.1.1建模9.1.2步态9.1.3仿真9.2四足机器人仿真 9.2.1建模 9.2.2仿真9.3双足机器人仿真9.3.1建模 9.3.2仿真9.4四旋翼无人机仿真9.4.1建模9.4.2仿真9.5海面船舶9.5.1建模 9.5.2仿真9.6水下潜艇

9.6.1建模9.6.2仿真9.7本章小结 9.3双足机器人仿真9.3双足机器人仿真双足机器人，亦称人形机器人，是指依靠两足站立和行走的机器人。双足机器人作为与人具有相同运动方式的机器人，对于未来的机器人在日常生活中的应用具有十分重要的意义。双足机器人不仅可以用于家庭服务、教育娱乐等领域，还可以在危险环境中代替人类执行任务，具有十分广阔的应用前景。双足机器人的研究难点和核心在于解决平衡控制和稳定行走的难题。相较于六足和四足机器人拥有静态平衡，双足机器人的静态平衡十分脆弱，仅维持自身平衡就已颇具挑战，行走控制更是复杂。由于双足结构复杂，运动控制难度较大，因此利用仿真技术进行大量的实验和优化十分关键。尽管当前人形机器人的步态研究与实践已取得一定成果，但仍面临诸多挑战。通过仿真技术深入学习双足机器人的平衡机制与步态控制，不仅是当前研究的热点，也是推动双足机器人技术迈向更高层次的重要途径。9.3双足机器人仿真9.3.1建模下图展示了Nao人形机器人模型。Nao人形机器人拥有头部、手部、腰部和腿部共计24个活动关节，十分灵活，但也给其控制增加了复杂性。通过对Nao人形机器人进行适配，添加关节位置控制器和关节状态发布器，完成人形机器人的建模。9.3双足机器人仿真9.3.1建模人形机器人Nao的SDF代码如下：<?xmlversion="1.0"?><sdfversion="1.11"><modelname="naoH25V40"><linkname="Torso"><pose>0000-00</pose><inertial><pose>-0.0041300.043420-00</pose><mass>1.04956</mass><inertia><ixx>0.00506234</ixx><ixy>1.43116e-05</ixy><ixz>0.000155191</ixz>9.3双足机器人仿真9.3.1建模<iyy>0.00488014</iyy><iyz>-2.70793e-05</iyz><izz>0.0016103</izz></inertia></inertial><collisionname="Torso_collision"><pose>000.010-00</pose><geometry><box><size>0.10.10.2115</size></box></geometry>9.3双足机器人仿真9.3.1建模</collision><visualname="Torso_visual"><pose>0000-00</pose><geometry><mesh><scale>0.10.10.1</scale><uri>meshes/V40/Torso.dae</uri></mesh></geometry></visual><gravity>1</gravity><velocity_decay/><self_collide>0</self_collide>9.3双足机器人仿真9.3.1建模</link><jointname="HeadYaw"type="revolute"><child>Neck</child><parent>Torso</parent><axis><xyz>001</xyz><limit><lower>-2.08567</lower><upper>2.08567</upper><effort>40.25</effort><velocity>8.26797</velocity></limit>9.3双足机器人仿真9.3.1建模<dynamics><damping>0.1</damping><friction>0</friction><spring_reference>0</spring_reference><spring_stiffness>0</spring_stiffness></dynamics><use_parent_model_frame>1</use_parent_model_frame></axis><physics><ode><implicit_spring_damper>1</implicit_spring_damper><cfm_damping>1</cfm_damping>9.3双足机器人仿真9.3.1建模<limit><cfm>0</cfm><erp>0.2</erp></limit></ode></physics></joint><pluginfilename="gz-sim-joint-position-controller-system"name="gz::sim::systems::JointPositionController"><joint_name>HeadYaw</joint_name><p_gain>10</p_gain><i_gain>0.1</i_gain>9.3双足机器人仿真9.3.1建模<d_gain>0.01</d_gain><i_max>1</i_max><i_min>-1</i_min><cmd_max>1000</cmd_max><cmd_min>-1000</cmd_min><use_actuator_msg>true</use_actuator_msg><actuator_number>0</actuator_number></plugin>（......此处省略）</model></sdf>9.3双足机器人仿真9.3.2仿真（1）话题桥接人形机器人在控制时与前述的六足和四足机人并没有区别，需要桥接控制话题和关节状态发布话题。人形机器人话题桥接的配置如下：-ros_topic_name:"actuators"gz_topic_name:"/actuators"ros_type_name:"actuator_msgs/msg/Actuators"gz_type_name:"gz.msgs.Actuators"lazy:falsedirection:ROS_TO_GZ9.3双足机器人仿真9.3.2仿真-ros_topic_name:"joint_states"gz_topic_name:"/world/tworobot/model/tworobot/joint_state"ros_type_name:"sensor_msgs/msg/JointState"gz_type_name:"gz.msgs.Model"lazy:falsedirection:GZ_TO_ROS9.3双足机器人仿真9.3.2仿真（2）关节控制人形机器人的关节控制与前述的六足和四足机器人控制方法相同，但保持人形机器人平衡和实现人形机器人的步态较为困难。为了方便展示人形机器人关节控制，以下实现了一个随机关节位置控制的节点，随机的设置关节的位置，机器人会摆出不同的姿态。控制人形机器人关节位置的节点代码如下：importrclpyfromrclpy.nodeimportNodefromstd_msgs.msgimportFloat64fromactuator_msgs.msgimportActuatorsimportrandom9.3双足机器人仿真9.3.2仿真classTwoController(Node):def__init__(self):super().__init__('twocontroller')self.actuator=Actuators()self.actuators_pub=self.create_publisher(Actuators,f'actuators',1)self.timer=self.create_timer(0.3,self.timer_callback)#将定时器周期设为1

deftimer_callback(self):self.actuator.position=[random.random()*2-1foriinrange(24)]self.actuators_pub.publish(self.actuator)9.3双足机器人仿真9.3.2仿真defmain():rclpy.init(args=None)twocontroller=TwoController()rclpy.spin(twocontroller)twocontroller.destroy_node()rclpy.shutdown()

if__name__=='__main__':main()9.3双足机器人仿真9.3.2仿真（3）Launch文件人形机器人仿真的Launch文件用于启动仿真环境、加载机器人、桥接话题、启动RViz2、发布机器人状态、运行关节控制节点。Launch文件代码如下：fromlaunchimportLaunchDescriptionfromlaunch.actionsimportIncludeLaunchDescriptionfromlaunch_ros.actionsimportNodefromlaunch.launch_description_sourcesimportPythonLaunchDescriptionSourcefromament_index_pythonimportget_package_share_directoryimportosimportre9.3双足机器人仿真9.3.2仿真print(__file__)packagepath=get_package_share_directory('tworobot')print(packagepath)sdfdir=packagepath+'/sdf/'print(packagepath+'/sdf/')SDF_WORLD_PATH=sdfdir+'world.sdf'SDF_MODEL_PATH=sdfdir+'nao.sdf'

defloadsdfmodel(sdfmodelpath):withopen(sdfmodelpath)asf:robot_desc=f.read()9.3双足机器人仿真9.3.2仿真

#使用正则表达式替换<uri>标签内的内容

robot_desc=re.sub(r'<uri>(.*?)</uri>',lambdamatch:f'<uri>file://{sdfdir}/{match.group(1)}</uri>',robot_desc)returnrobot_descdefgenerate_launch_description():#launchagazeboworldforsimulationgazebo_launch=IncludeLaunchDescription(PythonLaunchDescriptionSource(9.3双足机器人仿真9.3.2仿真

os.path.join(get_package_share_directory('ros_gz_sim'),'launch/gz_sim.launch.py')),launch_arguments=[('gz_args',SDF_WORLD_PATH+'-r')])

#addmodeltogazebosdfmodelstr=loadsdfmodel(SDF_MODEL_PATH)addrobottogazebo=Node(package='ros_gz_sim',executable='create',arguments=['-world','tworobot','-name','tworobot','-string',sdfmodelstr,'-x','0','-y','0','-z','0.35'])9.3双足机器人仿真9.3.2仿真#publishmodeltorosbyrobotstatepublisher,whichmakerobotcanbeseeninrviz2robot_state_node=Node(package='robot_state_publisher',executable='robot_state_publisher',name='robot_state_publisher',output='both',parameters=[{'use_sim_time':True},{'robot_description':sdfmodelstr},])

#startrviz2noderviz_node=Node(package='rviz2',executable='rviz2',parameters=[{'use_sim_time':True}],arguments=['-d',sdfdir+'tworobot.rviz'])

9.3双足机器人仿真9.3.2仿真#bridgegazebomsgtorosbridge_node=Node(package='ros_gz_bridge',executable='parameter_bridge',parameters=[{'config_file':sdfdir+'jointcontroltopicmap.yaml'},])

#robotcontrolnodecotroller_node=Node(package='tworobot',executable='random_controller')returnLaunchDescription([gazebo_launch,addrobottogazebo,robot_state_node,rviz_node,bridge_node,cotroller_node])9.3双足机器人仿真9.3.2仿真功能包经过编译和安装后，启动双足机器人关节控制仿真，运行Launch文件，执行命令：ros2launchtworobottwocontroller_launch.py上述命令运行后，ROS2会启动Gazebo和RViz2开始仿真，双足机器人就会在Gazebo中随机摆动四肢，如图所示。9.4四旋翼无人机仿真9.4四旋翼无人机仿真四旋翼无人机通过四个旋翼的转速控制来实现飞行速度和姿态的调整。每个旋翼由一个电动机驱动，通过改变转速可以调节升力的大小和方向。这种通过转速差异实现的控制方法使得四旋翼无人机具有极高的灵活性和操控性。近年来，四旋翼无人机因其结构简单、控制灵活、易于操作等特点，已经成为无人机领域的主流机型之一。四旋翼无人机在航拍、表演、物流配送、农业植保、环境监测等领域得到了广泛应用。四旋翼无人机的研究重点在于实现稳定的飞行控制和高效的路径规划。通过仿真技术，在虚拟环境中测试和优化各种算法和系统，从而提高无人机在复杂环境中的适应能力。尽管仿真技术存在一定的局限性，但其低成本、高安全性和可重复性的优势使其成为四旋翼无人机研究中不可或缺的工具。9.4四旋翼无人机仿真9.4.1建模四旋翼无人机建模的一方面在于外观，更重要的是其4个旋翼的控制，以及更高级的速度控制方法。Gazebo针对通过旋翼驱动提供升力的需求，提供了MulticopterMotorModel插件。MulticopterMotorModel插件可以通过旋转螺旋桨向模型施加推力，可以仿真各种螺旋浆飞行器的需求。在四旋翼无人机控制时，使用MulticopterMotorModel插件时需要调节每个螺旋浆的转速，虽然真实但较复杂，适用于通用的旋翼飞机的仿真。为了方便方便多旋翼无人控制，基于MulticopterMotorModel插件，Gazebo的MulticopterVelocityControl插件提供了多旋翼无人机的速度驱动接口，只需要简单配置就能够像控制小车一样通过Twist消息，实现多旋翼无人机的速度控制，简化了多旋翼无人机的仿真。以下介绍最简单的多旋翼无人机四旋翼无人机的建模。9.4四旋翼无人机仿真9.4.1建模下图为四旋翼无人机X3的仿真效果。相较前几节中的机器人，四旋翼无人机建模的关键要点有螺旋桨和机体关节的设置、对旋转螺旋桨配置MulticopterMotorModel控制器插件和为整个模型配置MulticopterVelocityControl插件三个方面。以下就四旋翼无人X3建模时编写SDF的要点进行介绍：9.4四旋翼无人机仿真9.4.1建模①关节设置。四旋翼无人机螺旋桨和机体的关节设置为转动类型关节。以下为其中一个关节的配置：<jointname="rotor_1_joint"type="revolute"><child>rotor_1</child><parent>base_link</parent><axis><xyz>001</xyz><limit><lower>-1e+16</lower><upper>1e+16</upper></limit><dynamics>9.4四旋翼无人机仿真9.4.1建模<spring_reference>0</spring_reference><spring_stiffness>0</spring_stiffness></dynamics></axis></joint>9.4四旋翼无人机仿真9.4.1建模②配置MulticopterMotorModel控制器插件。为每一个螺旋桨关节添加一个MulticopterMotorModel插件，其中一个关节的配置代码如下：<pluginfilename="gz-sim-multicopter-motor-model-system"name="gz::sim::systems::MulticopterMotorModel"><robotNamespace>X3</robotNamespace><jointName>rotor_0_joint</jointName><linkName>rotor_0</linkName><turningDirection>ccw</turningDirection><timeConstantUp>0.0125</timeConstantUp><timeConstantDown>0.025</timeConstantDown><maxRotVelocity>800.0</maxRotVelocity><motorConstant>8.54858e-06</motorConstant>9.4四旋翼无人机仿真9.4.1建模<momentConstant>0.016</momentConstant><commandSubTopic>gazebo/command/motor_speed</commandSubTopic><motorNumber>0</motorNumber><rotorDragCoefficient>8.06428e-05</rotorDragCoefficient><rollingMomentCoefficient>1e-06</rollingMomentCoefficient><motorSpeedPubTopic>motor_speed/0</motorSpeedPubTopic><rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim><motorType>velocity</motorType></plugin>9.4四旋翼无人机仿真9.4.1建模在上述MulticopterMotorModel插件的配置中设置了相关的参数，每个参数的含义可通过标签名识别。其中，参数<turningDirection>为螺旋桨转向的设置，任意两个相邻螺旋桨的转向要设置为相反。参数<robotNamespace>和<commandSubTopic>提供了关节速度（螺旋桨转速）的话题，并通过<motorNumber>将四个螺旋桨进行编号。在模型调试时，可通过话题驱动4个螺旋桨转动，命令如下：gztopic-t/X3/gazebo/command/motor_speed--msgtypegz.msgs.Actuators-p'velocity:[400,400,400,400]'对于旋翼无人机的低级仿真，使用上述MulticopterMotorModel插件，对每个螺旋桨进行速度控制的方法即可。9.4四旋翼无人机仿真9.4.1建模③配置MulticopterVelocityControl速度控制插件。基于MulticopterMotorModel插件，MulticopterVelocityControl速度控制插件提供了多旋翼无人机的高级控制接口，使用简单的Twist消息即可设置无人机的速度。为四旋翼无人机添加MulticopterVelocityControl插件的代码如下：<pluginfilename="gz-sim-multicopter-control-system"name="gz::sim::systems::MulticopterVelocityControl"><robotNamespace>X3</robotNamespace><enableSubTopic>enable</enableSubTopic><comLinkName>base_link</comLinkName><velocityGain>2.72.72.7</velocityGain><attitudeGain>230.15</attitudeGain><angularRateGain>0.40.520.18</angularRateGain>9.4四旋翼无人机仿真9.4.1建模<maximumLinearAcceleration>222</maximumLinearAcceleration><rotorConfiguration><rotor><jointName>rotor_0_joint</jointName><forceConstant>8.54858e-06</forceConstant><momentConstant>0.016</momentConstant><direction>1</direction></rotor><rotor><jointName>rotor_1_joint</jointName><forceConstant>8.54858e-06</forceConstant>9.4四旋翼无人机仿真9.4.1建模<momentConstant>0.016</momentConstant><direction>1</direction></rotor><rotor><jointName>rotor_2_joint</jointName><forceConstant>8.54858e-06</forceConstant><momentConstant>0.016</momentConstant><direction>-1</direction></rotor><rotor><jointName>rotor_3_joint</jointName><forceConstant>8.54858e-06</forceConstant><momentConstant>0.016</momentConstant>9.4四旋翼无人机仿真9.4.1建模<direction>-1</direction></rotor></rotorConfiguration></plugin>通过上述MulticopterVelocityControl插件为四旋翼无人机提供了一个名为/X3/cmd_vel的话题，用于控制无人机的速度。通过命令行工具将四旋翼无人的上升速度设置为0.3时，执行命令：gztopic-t/X3/cmd_vel--msgtypegz.msgs.Twist-p'linear:{z:0.3}'上述命令运行后，可看到四旋翼无人机的螺旋桨转动，并开始缓慢向上起飞升高。9.4四旋翼无人机仿真9.4.2仿真（1）话题桥接四旋翼无人机在使用ROS2进行仿真时，在简单的情况下，只需要桥接速度控制话题和关节状态两个话题即可。话题桥接配置代码如下：-ros_topic_name:"cmd_vel"gz_topic_name:"/X3/cmd_vel"ros_type_name:"geometry_msgs/msg/Twist"gz_type_name:"gz.msgs.Twist"lazy:falsedirection:ROS_TO_GZ-ros_topic_name:"joint_states"gz_topic_name:"/joint_states"ros_type_name:"sensor_msgs/msg/JointState"gz_type_name:"gz.msgs.Model"direction:GZ_TO_ROS9.4四旋翼无人机仿真9.4.2仿真（2）控制节点四旋翼无人机使用了MulticopterVelocityControl插件，使用Twist消息向四旋翼无人机发送速度指令即可。以下创建的四旋翼无人机的控制节点实现了四旋翼无人机起飞并绕圈飞行一周的功能，代码如下：importrclpyfromrclpy.nodeimportNodefromgeometry_msgs.msgimportTwistimportmathclassDroneController(Node):def__init__(self):super().__init__('drone_controller')9.4四旋翼无人机仿真9.4.2仿真self.publisher_=self.create_publisher(Twist,'cmd_vel',10)self.get_logger().info('DroneControllerhasbeenstarted.')self.msg=Twist()self.state='start'self.turnnum=0self.timer=self.create_timer(10.0,self.timer_callback)#每秒发布一次

deftimer_callback(self):ifself.state=='start':self.msg.linear.z=0.5self.msg.angular.z=0.0self.state='go'9.4四旋翼无人机仿真9.4.2仿真elifself.state=='go':self.msg.linear.x=0.5self.msg.linear.z=0.0self.msg.angular.z=0.0self.state='turn'elifself.state=='turn':self.msg.linear.x=0.0self.msg.linear.z=0.0self.msg.angular.z=math.pi/2/10self.turnnum+=1self.state='go'ifself.turnnum==4:self.state='down'9.4四旋翼无人机仿真9.4.2仿真elifself.state=='down':self.msg.linear.x=0.0self.msg.linear.z=-0.5self.msg.angular.z=0.0self.state='stop'else:self.msg.linear.x=0.0self.msg.linear.z=0.0self.msg.angular.z=0.0

self.publisher_.publish(self.msg)self.get_logger().info(self.state)9.4四旋翼无人机仿真9.4.2仿真self.get_logger().info('Publishing:LinearX:%.2f,LinearZ:%.2fAngularZ:%.2f'%(self.msg.linear.x,self.msg.linear.z,self.msg.angular.z))defmain(args=None):rclpy.init(args=args)drone_controller=DroneController()try:rclpy.spin(drone_controller)exceptKeyboardInterrupt:pass9.4四旋翼无人机仿真9.4.2仿真finally:drone_controller.destroy_node()rclpy.shutdown()

if__name__=='__main__':main()9.4四旋翼无人机仿真9.4.2仿真（3）Launch文件四旋翼无人机的仿真通过Launch文件运行，在Luanch文件中分为完成启动Gazebo仿真环境、加载四旋翼无人机模型、创建话题桥接和四旋翼无人机等四个节点。四旋翼无人机Launch文件代码如下：fromlaunchimportLaunchDescriptionfromlaunch.actionsimportIncludeLaunchDescriptionfromlaunch_ros.actionsimportNodefromlaunch.launch_description_sourcesimportPythonLaunchDescriptionSourcefromament_index_pythonimportget_package_share_directoryimportosprint(__file__)packagepath=get_package_share_directory('drone')9.4四旋翼无人机仿真9.4.2仿真print(packagepath)sdfdir=packagepath+'/sdf/'print(packagepath+'/sdf/')SDF_WORLD_PATH=sdfdir+'world.sdf'SDF_MODEL_PATH=sdfdir+'drone.sdf'

defloadsdfmodel(sdfmodelpath):withopen(sdfmodelpath)asf:robot_desc=f.read()#使用正则表达式替换<uri>标签内的内容robot_desc=robot_desc.replace('meshes',f'file://{sdfdir}materials')returnrobot_desc9.4四旋翼无人机仿真9.4.2仿真defgenerate_launch_description():#launchagazeboworldforsimulationgazebo_launch=IncludeLaunchDescription(PythonLaunchDescriptionSource(os.path.join(get_package_share_directory('ros_gz_sim'),'launch/gz_sim.launch.py')),launch_arguments=[('gz_args',SDF_WORLD_PATH+'-r')])

#addmodeltogazebosdfmodelstr=loadsdfmodel(SDF_MODEL_PATH)#print(sdfmodelstr)addrobottogazebo=Node(9.4四旋翼无人机仿真9.4.2仿真package='ros_gz_sim',executable='create',arguments=['-world','drone_world','-name','drone','-string',sdfmodelstr,'-x','0','-y','0','-z','0.15'])#bridgegazebomsgtorosbridge_node=Node(package='ros_gz_bridge',executable='parameter_bridge',parameters=[{'config_file':sdfdir+'drone_bridge.yaml'},])#robotcontrolnodecotroller_node=Node(package='drone',executable='controller')returnLaunchDescription([gazebo_launch,addrobottogazebo,bridge_node,cotroller_node])9.4四旋翼无人机仿真9.4.2仿真功能包经过编译和安装后，启动四旋翼无人仿真，运行Launch文件，执行命令：ros2launchdronedronecontroller_launch.py上述命令运行后，ROS2会启动Gazebo开始仿真，四旋翼无人机就会在Gazebo中按照控制节点的命令进行飞行，通过Plot3D插件可显示四旋翼无人的轨迹，如图所示。9.5海面船舶仿真9.5海面船舶仿真船舶是一类在水面航行机器。海面船舶仿真不仅需要进行船舶的建模，而且也需要对水面建模。对于船舶和水面的仿真Gazebo本身的支持有限，需要开发自定义的插件。目前Gazebo以第三方扩展的形式提供了水面的仿真，能够产生波浪的仿真效果。对于船舶的仿真，则可以通过Gazebo内置水动力仿真插件Hydrodynamics模拟船舶在水面中的受力，从而较好的进行船舶的仿真。海面船舶仿真对于船舶设计、海洋工程、航海训练等领域具有重要意义。目前通过船舶的仿真可以研究航行控制和高效的路径规划，从而对船舶的航行性能进行优化，提高其在复杂海洋环境中的适应能力。9.5海面船舶仿真9.5.1建模下图展示了一种海面船舶模型，该船舶有两个舱体构成，左右各有一个能够转向的发动机驱动螺旋桨在水中产生推力，从而使船舶能够完成左右转向和前进后退的运动。9.5海面船舶仿真9.5.1建模SDF建模船舶的主要要点有：①添加Thruster插件使得螺旋桨能够产生推力。以下为其中一个螺旋桨的Thrust插件配置：<pluginfilename="gz-sim-thruster-system"name="gz::sim::systems::Thruster"><joint_name>left_engine_propeller_joint</joint_name><thrust_coefficient>0.004422</thrust_coefficient><fluid_density>1000</fluid_density><propeller_diameter>0.2</propeller_diameter><velocity_control>true</velocity_control></plugin>9.5海面船舶仿真9.5.1建模②添加关节位置控制器，用于改螺旋桨推力的方向。以下为其中一个螺旋桨关节控制器的配置：<pluginfilename="gz-sim-joint-position-controller-system"name="gz::sim::systems::JointPositionController"><joint_name>left_chasis_engine_joint</joint_name><use_velocity_commands>true</use_velocity_commands><topic>/left_cmd_pos</topic></plugin>9.5海面船舶仿真9.5.1建模③使用第三方自定义的一个船体与水体作用的maritime::Surface插件，配置代码如下：<pluginfilename="libSurface.so"name="maritime::Surface"><link_name>base_link</link_name><hull_length>4.9</hull_length><hull_radius>0.213</hull_radius><fluid_level>0</fluid_level>9.5海面船舶仿真9.5.1建模<points><point>0.61.030</point><point>-1.41.030</point></points><wavefield><topic>/gazebo/wavefield/parameters</topic></wavefield></plugin>9.5海面船舶仿真9.5.1建模④为船体添加水动学插件Hydrodynamics，使得船体在仿真时能够遵循在液体中运动的物理规律。Hydrodynamics插件的配置代码如下：<pluginfilename="gz-sim-hydrodynamics-system"name="gz::sim::systems::Hydrodynamics"><link_name>base_link</link_name><xDotU>0.0</xDotU><yDotV>0.0</yDotV><nDotR>0.0</nDotR><xU>-51.3</xU><xAbsU>-72.4</xAbsU><yV>-40.0</yV><yAbsV>0.0</yAbsV>9.5海面船舶仿真9.5.1建模<zW>-500.0</zW><kP>-50.0</kP><mQ>-50.0</mQ><nR>-400.0</nR><nAbsR>0.0</nAbsR></plugin>9.5海面船舶仿真9.5.2仿真（1）话题桥接船舶的运动需要控制两个关节和两个螺旋桨，需要建立4个ROS2到Gazebo话题的桥接，以及1个Gazebo到ROS2的关节状态话题桥接。话题桥接配置代码如下：-ros_topic_name:"/cmd_left_angle"gz_topic_name:"/left_cmd_pos"ros_type_name:"std_msgs/msg/Float64"gz_type_name:"gz.msgs.Double"lazy:truedirection:ROS_TO_GZ9.5海面船舶仿真9.5.2仿真-ros_topic_name:"/cmd_right_angle"gz_topic_name:"/right_cmd_pos"ros_type_name:"std_msgs/msg/Float64"gz_type_name:"gz.msgs.Double"lazy:truedirection:ROS_TO_GZ

-ros_topic_name:"/cmd_left_thrust"gz_topic_name:"/model/wam-v/joint/left_engine_propeller_joint/cmd_thrust"ros_type_name:"std_msgs/msg/Float64"gz_type_name:"gz.msgs.Double"lazy:truedirection:ROS_TO_GZ

9.5海面船舶仿真9.5.2仿真-ros_topic_name:"/cmd_right_thrust"gz_topic_name:"/model/wam-v/joint/right_engine_propeller_joint/cmd_thrust"ros_type_name:"std_msgs/msg/Float64"gz_type_name:"gz.msgs.Double"lazy:truedirection:ROS_TO_GZ

-ros_topic_name:"joint_states"gz_topic_name:"/joint_states"ros_type_name:"sensor_msgs/msg/JointState"gz_type_name:"gz.msgs.Model"direction:GZ_TO_ROS9.5海面船舶仿真9.5.2仿真（2）船舶控制船舶的运动在控制上速度的调节需要通过左右两个螺旋桨的朝向和两个螺旋桨的速度来实现，需要共计4个话题。船舶速度的控制方法与差速小车控制器类似，借助有限状态机来实现船舶的直行和转弯，使得船舶在海面绕圈航行，控制节点代码如下：importrclpyfromrclpy.nodeimportNodefromstd_msgs.msgimportFloat64classBoatController(Node):def__init__(self):super().__init__('boat_publisher')self.la_pub=self.create_publisher(Float64,'/cmd_left_angle',10)self.ra_pub=self.create_publisher(Float64,'/cmd_right_angle',10)9.5海面船舶仿真9.5.2仿真self.lt_pub=self.create_publisher(Float64,'/cmd_left_thrust',10)self.rt_pub=self.create_publisher(Float64,'/cmd_right_thrust',10)self.msgs=[Float64()foriinrange(4)]self.action='turn'self.timer=self.create_timer(5.0,self.publish_angle)#每秒发布一次

defpublish_angle(self):self.get_logger().info(self.action)ifself.action=='turn':self.msgs[0].data=1.0self.msgs[1].data=1.09.5海面船舶仿真9.5.2仿真self.msgs[2].data=10.0self.msgs[3].data=10.0self.action='go'elifself.action=='go':self.msgs[0].data=0.0self.msgs[1].data=0.0self.msgs[2].data=100.0self.msgs[3].data=100.0self.action='turn'else:self.msgs[0].data=0.0self.msgs[1].data=0.0self.msgs[2].data=0.0self.msgs[3].data=0.09.5海面船舶仿真9.5.2仿真self.la_pub.publish(self.msgs[0])self.ra_pub.publish(self.msgs[1])self.lt_pub.publish(self.msgs[2])self.rt_pub.publish(self.msgs[3])defmain(args=None):rclpy.init(args=args)angle_publisher=BoatController()rclpy.spin(angle_publisher)angle_publisher.destroy_node()rclpy.shutdown()if__name__=='__main__':main()9.5海面船舶仿真9.5.2仿真（3）Launch文件船舶仿真的Launch文件用于启动仿真环境、控制节点、桥接话题等节点，代码如下：fromlaunchimportLaunchDescriptionfromlaunch.actionsimportIncludeLaunchDescriptionfromlaunch_ros.actionsimportNodefromlaunch.launch_description_sourcesimportPythonLaunchDescriptionSourcefromament_index_pythonimportget_package_share_directoryimportos9.5海面船舶仿真9.5.2仿真print(__file__)packagepath=get_package_share_directory('surfaceboat')print(packagepath)sdfdir=packagepath+'/sdf/'print(packagepath+'/sdf/')SDF_WORLD_PATH=sdfdir+'world.sdf'SDF_MODEL_PATH=sdfdir+'wam-v.sdf'#设置临时环境变量os.environ['GZ_SIM_SYSTEM_PLUGIN_PATH']=os.environ['GZ_SIM_SYSTEM_PLUGIN_PATH']+':'+packagepath+"/sdf"os.environ['LD_LIBRARY_PATH']=os.environ['LD_LIBRARY_PATH']+':'+packagepath+"/sdf"9.5海面船舶仿真9.5.2仿真defloadsdfmodel(sdfmodelpath):withopen(sdfmodelpath)asf:robot_desc=f.read()#使用正则表达式替换<uri>标签内的内容robot_desc=robot_desc.replace('meshes',f'file://{sdfdir}meshes')returnrobot_desc

defgenerate_launch_description():#launchagazeboworldforsimulationgazebo_launch=IncludeLaunchDescription(PythonLaunchDescriptionSource(9.5海面船舶仿真9.5.2仿真os.path.join(get_package_share_directory('ros_gz_sim'),'launch/gz_sim.launch.py')),launch_arguments=[('gz_args',SDF_WORLD_PATH+'-r')])

#addmodeltogazebosdfmodelstr=loadsdfmodel(SDF_MODEL_PATH)#print(sdfmodelstr)addrobottogazebo=Node(package='ros_gz_sim',executable='create',arguments=['-world','surfaceboat_world','-name','wam-v','-string',sdfmodelstr,'-x','0','-y','0','-z','0.15'])9.5海面船舶仿真9.5.2仿真#bridgegazebomsgtorosbridge_node=Node(package='ros_gz_bridge',executable='parameter_bridge',parameters=[{'config_file':sdfdir+'surfaceboat_bridge.yaml'},])

#publishmodeltorosbyrobotstatepublisher,whichmakerobotcanbeseeninrviz2robot_state_node=Node(package='robot_state_publisher',executable='robot_state_publisher',name='robot_state_publisher',parameters=[{'use_sim_time':True},{'robot_description':sdfmodelstr},])9.5海面船舶仿真9.5.2仿真#robotcontrolnodecotroller_node=Node(package='surfaceboat',executable='controller')returnLaunchDescription([gazebo_launch,addrobottogazebo,bridge_node,robot_state_node,cotroller_node])9.5海面船舶仿真9.5.2仿真功能包经过编译和安装后，在终端里运行Launch文件，执行命令：ros2launchsurfaceboatboat_launch.py以上命令运行后，启动海面船舶仿真，如图所示，在控制节点的命令下船舶会在海面上进行近似圆周绕圈运动，通过Plot3D插件可直观地显示船舶航行的轨迹。9.6水下潜艇仿真9.6水下潜艇仿真潜艇作为水下的航行器，具有十分重要的用途。相较于其他地面或空中的机器人仿真，潜艇的受力比较复杂除了重力、阻力外，还受到水的浮力。Gazebo通过浮力插件Buoyancy可以模拟出一个具有浮力的仿真环境，从而支持水下潜艇（机器人）的仿真。水下潜艇仿真在军事、海洋科学、水下工程等领域具有重要的应用价值。对于军事领域，水下潜艇仿真可以帮助军事人员进行战术训练和作战模拟，提高潜艇的作战效能。在海洋科学研究中，水下潜艇仿真可以用于模拟深海探测任务，研究海洋环境对潜艇的影响。水下潜艇的研究重点在于实现稳定的水下航行控制和高效的路径规划。通过仿真技术，可以对潜艇的航行性能进行优化，提高其在复杂水下环境中的适应能力。9.6水下潜艇仿真为了模拟水下环境，需要在仿真世界中添加浮力插件Buoyancy。在Gazebo中添加浮力插件，浮力插件Buoyancy的方法是向仿真世界的SDF中添加以下代码：<pluginfilename="gz-sim-buoyancy-system"name="gz::sim::systems::Buoyancy"><uniform_fluid_density>1000</uniform_fluid_density></plugin>9.6水下潜艇仿真9.6.1建模潜艇在运动上仿生鱼类，通过背鳍和腹鳍调整前进的方向，通过尾部的螺旋桨旋转产生前进的推力。下图为一款开源的水下潜艇Tethys的建模效果，潜艇在建模时的要点是配置影响其速度的各个关节、添加关节的控制插件，以及模拟水中阻力的物理插值。9.6水下潜艇仿真9.6.1建模以下为潜艇建模中SDF的关键配置代码：①将潜艇的水平鳍和垂直鳍设置为关节位置控制器，代码如下：<pluginfilename="gz-sim-joint-position-controller-system"name="gz::sim::systems::JointPositionController"><joint_name>horizontal_fins_joint</joint_name><use_velocity_commands>true</use_velocity_commands><topic>hfin_pos_cmd</topic></plugin><pluginfilename="gz-sim-joint-position-controller-system"name="gz::sim::systems::JointPositionController"><joint_name>vertical_fins_joint</joint_name><topic>vfin_pos_cmd</topic><use_velocity_commands>true</use_velocity_commands></plugin>9.6水下潜艇仿真9.6.1建模②向潜艇尾部的螺旋桨关节添加推力控制器，代码如下：<pluginfilename="gz-sim-thruster-system"name="gz::sim::systems::Thruster"><namespace>submarine</namespace><joint_name>propeller_joint</joint_name><thrust_coefficient>0.004422</thrust_coefficient><fluid_density>1000</fluid_density><propeller_diameter>0.2</propeller_diameter></plugin>ROS2