鱼香ROS社区
    • 版块
    • 最新
    • 未解决
    • 已解决
    • 群组
    • 注册
    • 登录
    紧急通知:禁止一切关于政治&VPN翻墙等话题,发现相关帖子会立马删除封号
    提问前必看的发帖注意事项: 社区问答规则(小鱼个人)更新 | 高质量帖子发布指南

    ignition中麦克纳姆轮仿真,小车无法左右移动

    已定时 已固定 已锁定 已移动 未解决
    综合问题
    ros2 ignition ros2control mecanum
    1
    1
    60
    正在加载更多帖子
    • 从旧到新
    • 从新到旧
    • 最多赞同
    回复
    • 在新帖中回复
    登录后回复
    此主题已被删除。只有拥有主题管理权限的用户可以查看。
    • W
      wahaha啊
      最后由 wahaha啊 编辑

      ros2 humble+ros2 control+gazebo fortress
      小车可以旋转和前后移动,但无法左右平移
      一下是相关代码:
      包括小车底盘模型,ros2 control控制以及配置文件,和启动文件
      我有点担心启动文件是否正确

      <?xml version="1.0" encoding="utf-8"?>
      <robot name="jetauto" xmlns:xacro="http://ros.org/wiki/xacro" >
        <xacro:property name="M_PI"               value="3.1415926535897931"/>
        <xacro:property name="base_link_mass"     value="1.3" /> 
        <xacro:property name="base_link_w"        value="0.297"/>
        <xacro:property name="base_link_h"        value="0.145"/>
        <xacro:property name="base_link_d"        value="0.11255"/>
      
        <xacro:property name="wheel_link_mass"    value="0.2" />
        <xacro:property name="wheel_link_radius"  value="0.049"/>
        <xacro:property name="wheel_link_length"  value="0.04167"/>
      
        <link name="base_footprint"/>
      
        <joint name="base_joint" type="fixed">
          <parent link="base_footprint"/>
          <child link="base_link"/>
          <origin xyz="0.0 0.0 0.005" rpy="0 0 0"/>
        </joint>
      
        <link
          name="base_link">
          <xacro:box_inertial m="${base_link_mass}" w="${base_link_w}" h="${base_link_h}" d="${base_link_d}"/>
          <visual>
            <origin
              xyz="0 0 0"
              rpy="0 0 0" />
            <geometry>
              <mesh
                filename="package://gazebo_simulation/meshes/base_link.stl" />
            </geometry>
            <material name="green"/>
          </visual>
          <collision>
            <origin
              xyz="${base_link_w/2.0 - 0.14810} 0 ${0.126437/2 + 0.02362364}"
              rpy="0 0 0" />
            <geometry>
              <box size="${base_link_w} ${base_link_h} ${base_link_d}" />
            </geometry>
          </collision>
        </link>
        <link
          name="back_shell_link">
          <inertial>
            <origin
              xyz="-1.22838595456587E-05 0.00218574826309681 -0.0500522861933898"
              rpy="0 0 0" />
            <mass
              value="0.0663478534899862" />
            <inertia
              ixx="5.65277934912267E-05"
              ixy="-5.13394387877366E-11"
              ixz="-4.07561372273553E-11"
              iyy="4.33740893441632E-05"
              iyz="-5.43059341238134E-06"
              izz="6.86642544694324E-05" />
          </inertial>
          <visual>
            <origin
              xyz="0 0 0"
              rpy="0 0 0" />
            <geometry>
              <mesh
                filename="package://gazebo_simulation/meshes/back_shell_link.stl" />
            </geometry>
            <material name="black">
            </material>
          </visual>
          <!-- <collision>-->
            <!--<origin-->
              <!--xyz="0 0 0"-->
              <!--rpy="0 0 0" />-->
            <!--<geometry>-->
              <!--<mesh-->
                <!--filename="package://jetauto_description/meshes/back_shell_link.stl" />-->
            <!--</geometry>-->
          <!--</collision> -->
        </link>
        <joint
          name="back_shell_joint"
          type="fixed">
          <origin
            xyz="-0.076481 0 0.082796"
            rpy="-3.1416 0 1.5708" />
          <parent
            link="base_link" />
          <child
            link="back_shell_link" />
        </joint>
        
                               <!-- wheel_right_front -->
        <link
          name="wheel_right_front_link">
          <!-- <xacro:cylinder_inertial m="${wheel_link_mass}" r="${wheel_link_radius}" h="${wheel_link_length}" /> -->
          <xacro:sphere_inertial m="${wheel_link_mass}" r="${wheel_link_radius}"/>
          <visual>
            <origin
              xyz="0 0 0"
              rpy="0 0 0" />
            <geometry>
              <mesh
                filename="package://gazebo_simulation/meshes/wheel_right_front_link.stl" />
            </geometry>
            <material name="black"/>
          </visual>
          <collision>
            <origin
              xyz="0 0 ${wheel_link_length/2.0}"
              rpy="0 0 0" />
            <geometry>
              <!-- <cylinder length="${wheel_link_length}" radius="${wheel_link_radius}"/> -->
              <sphere radius="${wheel_link_radius}"/>
            </geometry>
          </collision>
        </link>
      
        <joint
          name="wheel_right_front_joint"
          type="continuous">
          <origin
            xyz="0.097397 -0.086125 0.04508"
            rpy="1.5708 1.5708 0" />
          <parent
            link="base_link" />
          <child
            link="wheel_right_front_link" />
          <axis xyz="0 0 -1" />
        </joint>
        
                              <!-- wheel_left_front -->
      
        <link name="wheel_left_front_link">
          <!-- <xacro:cylinder_inertial m="${wheel_link_mass}" r="${wheel_link_radius}" h="${wheel_link_length}" /> -->
          <xacro:sphere_inertial m="${wheel_link_mass}" r="${wheel_link_radius}"/>
          <visual>
            <origin
              xyz="0 0 0"
              rpy="0 0 0" />
            <geometry>
              <mesh
                filename="package://gazebo_simulation/meshes/wheel_left_front_link.stl" />
            </geometry>
            <material name="black"/>
          </visual>
          <collision>
            <origin
              xyz="0 0 -${wheel_link_length/2.0}"
              rpy="0 0 0" />
            <geometry>
              <!-- <cylinder length="${wheel_link_length}" radius="${wheel_link_radius}"/> -->
              <sphere radius="${wheel_link_radius}"/>
            </geometry>
          </collision>
        </link>
      
        <joint
          name="wheel_left_front_joint"
          type="continuous">
          <origin
            xyz="0.097397 0.086125 0.04508"
            rpy="1.5708 1.5708 0" />
          <parent
            link="base_link" />
          <child
            link="wheel_left_front_link" />
          <axis xyz="0 0 -1" />
        </joint>
                               <!-- wheel_right_back -->
        <link
          name="wheel_right_back_link">
          <!-- <xacro:cylinder_inertial m="${wheel_link_mass}" r="${wheel_link_radius}" h="${wheel_link_length}" /> -->
          <xacro:sphere_inertial m="${wheel_link_mass}" r="${wheel_link_radius}"/>
          <visual>
            <origin
              xyz="0 0 0"
              rpy="0 0 0" />
            <geometry>
              <mesh
                filename="package://gazebo_simulation/meshes/wheel_right_back_link.stl" />
            </geometry>
            <material name="black"/>
          </visual>
          <collision>
            <origin
              xyz="0 0 ${wheel_link_length/2.0}"
              rpy="0 0 0" />
            <geometry>
              <!-- <cylinder length="${wheel_link_length}" radius="${wheel_link_radius}"/> -->
              <sphere radius="${wheel_link_radius}"/>
            </geometry>
          </collision>
        </link>
      
        <joint
          name="wheel_right_back_joint"
          type="continuous">
          <origin
            xyz="-0.097397 -0.086125 0.04508"
            rpy="1.5708 1.5708 0" />
          <parent
            link="base_link" />
          <child
            link="wheel_right_back_link" />
          <axis xyz="0 0 -1" />
        </joint>
      
                               <!-- wheel_left_back -->
        <link
          name="wheel_left_back_link">
          <!-- <xacro:cylinder_inertial m="${wheel_link_mass}" r="${wheel_link_radius}" h="${wheel_link_length}" /> -->
          <xacro:sphere_inertial m="${wheel_link_mass}" r="${wheel_link_radius}"/>
          <visual>
            <origin
              xyz="0 0 0"
              rpy="0 0 0" />
            <geometry>
              <mesh
                filename="package://gazebo_simulation/meshes/wheel_left_back_link.stl" />
            </geometry>
            <material name="black"/>
          </visual>
          <collision>
            <origin
              xyz="0 0 -${wheel_link_length/2.0}"
              rpy="0 0 0" />
            <geometry>
              <!-- <cylinder length="${wheel_link_length}" radius="${wheel_link_radius}"/> -->
              <sphere radius="${wheel_link_radius}"/>
            </geometry>
          </collision>
        </link>
      
        <joint
          name="wheel_left_back_joint"
          type="continuous">
          <origin
            xyz="-0.097397 0.086125 0.04508"
            rpy="1.5708 1.5708 0" />
          <parent
            link="base_link" />
          <child
            link="wheel_left_back_link" />
          <axis xyz="0 0 -1" />
        </joint>
      
      
        <link
          name="mic_link">
          <inertial>
            <origin
              xyz="7.37972827624667E-07 0.000380767498086923 -0.0010743560446495"
              rpy="0 0 0" />
            <mass
              value="0.0234232570822793" />
            <inertia
              ixx="1.59705800233789E-05"
              ixy="-5.64325349754072E-11"
              ixz="8.96027229707658E-11"
              iyy="2.23652963143563E-05"
              iyz="6.01204669252721E-08"
              izz="3.74624298483869E-05" />
          </inertial>
          <visual>
            <origin
              xyz="0 0 0"
              rpy="0 0 0" />
            <geometry>
              <mesh
                filename="package://gazebo_simulation/meshes/mic_link.stl" />
            </geometry>
            <material name="black"/>
          </visual>
          <!--<collision>-->
            <!--<origin-->
              <!--xyz="0 0 0"-->
              <!--rpy="0 0 0" />-->
            <!--<geometry>-->
              <!--<mesh-->
                <!--filename="package://jetauto_description/meshes/mic_link.stl" />-->
            <!--</geometry>-->
          <!--</collision>-->
        </link>
        <joint
          name="mic_joint"
          type="fixed">
          <origin
            xyz="-0.079228 -1.5532E-05 0.15861"
            rpy="0 0 1.5708" />
          <parent
            link="base_link" />
          <child
            link="mic_link" />
        </joint>
        <link
          name="speaker_link">
          <inertial>
            <origin
              xyz="0 0 0"
              rpy="0 0 0" />
            <mass
              value="0" />
            <inertia
              ixx="0"
              ixy="0"
              ixz="0"
              iyy="0"
              iyz="0"
              izz="0" />
          </inertial>
          <visual>
            <origin
              xyz="0 0 0"
              rpy="0 0 0" />
            <geometry>
              <mesh
                filename="package://gazebo_simulation/meshes/speaker_link.stl" />
            </geometry>
            <material name="black"/>
          </visual>
          <!--<collision>-->
            <!--<origin-->
              <!--xyz="0 0 0"-->
              <!--rpy="0 0 0" />-->
            <!--<geometry>-->
              <!--<mesh-->
                <!--filename="package://jetauto_description/meshes/speaker_link.stl" />-->
            <!--</geometry>-->
          <!--</collision>-->
        </link>
        <joint
          name="speaker_joint"
          type="fixed">
          <origin
            xyz="-0.130292759618243 0 0.0549589364206024"
            rpy="0 0 0" />
          <parent
            link="base_link" />
          <child
            link="speaker_link" />
        </joint>
      </robot>
      
      <?xml version="1.0" encoding="utf-8"?>  <!-- XML文件声明 -->
      <!-- <robot name="jetauto" xmlns:xacro="http://ros.org/wiki/xacro">  定义机器人模型,使用xacro命名空间 -->
      <robot name="jetauto" xmlns:xacro="http://ros.org/wiki/xacro" xmlns:ignition="http://gazebosim.org/schema/"> 
      <!-- <robot name="jetauto" xmlns:xacro="http://ros.org/wiki/xacro" xmlns:gz="http://gazebosim.org/schema/"> -->
          
          <!-- 定义机器人名称参数 -->
          <xacro:arg name="robot_name" default="jetauto"/>
      
          <!-- 包含基础配置文件 - 仅包含一次材质定义 -->
          <xacro:include filename="$(find gazebo_simulation)/urdf/materials.xacro"/>     <!-- 包含材质定义文件 -->
          <xacro:include filename="$(find gazebo_simulation)/urdf/inertial_matrix.xacro"/> <!-- 包含惯性矩阵定义文件 -->
      
          <!-- 定义M_PI变量,确保所有引入的文件可用 -->
          <xacro:property name="M_PI" value="3.1415926535897931" />
          <xacro:property name="camera_x" value="0.120" />
      
          <!-- 包含IMU配置 -->
          <xacro:include filename="$(find gazebo_simulation)/urdf/imu.urdf.xacro"/>     <!-- 包含IMU传感器配置文件 -->
      
          <!-- 激光雷达配置部分 -->
          <xacro:include filename="$(find gazebo_simulation)/urdf/lidar_a1.urdf.xacro"/> <!-- 包含A1激光雷达配置 -->
      
          <!-- JetAuto标准版配置 -->
          <xacro:include filename="$(find gazebo_simulation)/urdf/depth_camera.urdf.xacro"/> <!-- 包含深度相机配置 -->
          <xacro:include filename="$(find gazebo_simulation)/urdf/jetauto_car.urdf.xacro"/> <!-- 包含标准版底盘配置 -->
          
      
          <!-- 加载参数 -->
          <xacro:arg name="use_sim_time" default="true"/>
          <xacro:arg name="mecanum_control" default="true"/>
          <xacro:arg name="use_gazebo" default="true"/>
          <!-- Include and instantiate ros2_control -->
          <xacro:include filename="$(find gazebo_simulation)/urdf/ros2_control.xacro" />
          <xacro:jetauto_ros2_control /> 
          
          <!-- 应用Gazebo材质映射 -->
          <!-- <xacro:gazebo_material_blue reference="base_link"/>
          <xacro:gazebo_material_black reference="wheel_right_front_link"/>
          <xacro:gazebo_material_black reference="wheel_left_front_link"/>
          <xacro:gazebo_material_black reference="wheel_right_back_link"/>
          <xacro:gazebo_material_black reference="wheel_left_back_link"/>
          <xacro:gazebo_material_black reference="back_shell_link"/>
          <xacro:gazebo_material_black reference="mic_link"/>
          <xacro:gazebo_material_black reference="lidar_link"/>
          <xacro:gazebo_material_black reference="depth_cam_link"/> -->
      
          <gazebo reference='wheel_left_front_link'>    
              <collision>
                  <surface>
                  <friction>
                      <ode>
                      <mu>1.0</mu>
                      <mu2>0.0</mu2>
                      <fdir1 ignition:expressed_in="base_link">1 -1 0</fdir1>
                      </ode>
                  </friction>
                  </surface>
              </collision>
          </gazebo>
      
          <gazebo reference='wheel_left_back_link'>
              <collision>
                  <surface>
                  <friction>
                      <ode>
                      <mu>1.0</mu>
                      <mu2>0.0</mu2>
                      <fdir1 ignition:expressed_in="base_link">1 1 0</fdir1>
                      </ode>
                  </friction>
                  </surface>
              </collision>
          </gazebo>
      
          <gazebo reference='wheel_right_front_link'>
              <collision>
                  <surface>
                  <friction>
                      <ode>
                      <mu>1.0</mu>
                      <mu2>0.0</mu2>
                      <fdir1 ignition:expressed_in="base_link">1 1 0</fdir1>
                      </ode>
                  </friction>
                  </surface>
              </collision>
          </gazebo>
      
          <gazebo reference='wheel_right_back_link'>
              <collision>
                  <surface>
                  <friction>
                      <ode>
                      <mu>1.0</mu>
                      <mu2>0.0</mu2>
                      <fdir1 ignition:expressed_in="base_link">1 -1 0</fdir1>
                      </ode>
                  </friction>
                  </surface>
              </collision>
          </gazebo>
      
      </robot>
      
      
      <?xml version="1.0"?>
      <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
          <xacro:macro name="jetauto_ros2_control">
              <ros2_control name="jetauto_hardware_interface" type="system">
                  <hardware>
                      <plugin>gz_ros2_control/GazeboSimSystem</plugin>
                  </hardware>
      
                  <!-- 定义轮子 注意,在gazebo中速度属性只能使用velocity-->
                  <!--左侧前轮-->
                  <joint name="wheel_left_front_joint">
                      <command_interface name="velocity">
                          <param name="min">-10</param><!-- 速度大小范围 -->
                          <param name="max">10</param>
                      </command_interface>
                      <state_interface name="position" />
                      <state_interface name="velocity" />
                  </joint>
                  <!--右侧后轮-->
                  <joint name="wheel_left_back_joint">
                      <command_interface name="velocity">
                          <param name="min">-10</param>
                          <param name="max">10</param>
                      </command_interface>
                      <state_interface name="position" />
                      <state_interface name="velocity" />
                  </joint>
      
                  <!--右侧前轮-->
                  <joint name="wheel_right_front_joint">
                      <command_interface name="velocity">
                          <param name="min">-10</param>
                          <param name="max">10</param>
                      </command_interface>
                      <state_interface name="position" />
                      <state_interface name="velocity" />
                  </joint>
                  <!--右侧后轮-->
                  <joint name="wheel_right_back_joint">
                      <command_interface name="velocity">
                          <param name="min">-10</param>
                          <param name="max">10</param>
                      </command_interface>
                      <state_interface name="position" />
                      <state_interface name="velocity" />
                  </joint>
      
                  <!-- IMU传感器 -->
                  <sensor name="imu_sensor">
                      <state_interface name="orientation.x"/>
                      <state_interface name="orientation.y"/>
                      <state_interface name="orientation.z"/>
                      <state_interface name="orientation.w"/>
                      <state_interface name="angular_velocity.x"/>
                      <state_interface name="angular_velocity.y"/>
                      <state_interface name="angular_velocity.z"/>
                      <state_interface name="linear_acceleration.x"/>
                      <state_interface name="linear_acceleration.y"/>
                      <state_interface name="linear_acceleration.z"/>
                  </sensor>
              </ros2_control>
              
              <!-- Gazebo ros2_control插件 -->
              <gazebo>
                  <plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
                      <parameters>$(find gazebo_simulation)/config/jetauto_controllers.yaml</parameters>
                      <robot_param>robot_description</robot_param>
                      <robot_param_node>robot_state_publisher</robot_param_node>
                      <ros>
                          <remapping>/jetauto_base_controller/reference_unstamped:=/cmd_vel</remapping>
                          <remapping>/jetauto_base_controller/odometry:=/odom</remapping>
                      </ros>
                  </plugin>
              </gazebo>
          </xacro:macro>
      </robot>
      
      
      controller_manager:
        ros__parameters:
          update_rate: 100  # Hz
          use_sim_time: true
          
          joint_state_broadcaster:
            type: joint_state_broadcaster/JointStateBroadcaster
            
          jetauto_base_controller:
            type: mecanum_drive_controller/MecanumDriveController
      
      jetauto_base_controller:
        ros__parameters:
          reference_time: 1.0
          front_left_wheel_command_joint_name: "wheel_left_front_joint"
          front_right_wheel_command_joint_name: "wheel_right_front_joint" 
          rear_left_wheel_command_joint_name: "wheel_left_back_joint"
          rear_right_wheel_command_joint_name: "wheel_right_back_joint"
          
          # 运动学参数
          kinematics:
            # wheel_separation_x: 0.194794
            # wheel_separation_y: 0.17225
            base_frame_offset: { x: 0.0, y: 0.0, theta: 0.0 }
            wheels_radius: 0.049
            sum_of_robot_center_projection_on_X_Y_axis: 0.183522
          
          # 坐标系参数
          base_frame_id: "base_link"
          odom_frame_id: "odom"
          
          # 发布参数
          publish_rate: 100
          enable_odom_tf: true
          pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
          twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
          
          cmd_vel_timeout: 0.5
          open_loop: false
          use_stamped_vel: false
         
      
      #!/usr/bin/python3
      
      from os.path import join
      from launch import LaunchDescription
      from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, TimerAction
      from launch.substitutions import LaunchConfiguration, PythonExpression, Command, PathJoinSubstitution
      from launch.launch_description_sources import PythonLaunchDescriptionSource
      from ament_index_python.packages import get_package_share_directory
      from launch.actions import AppendEnvironmentVariable
      from launch_ros.actions import Node
      from launch_ros.substitutions import FindPackageShare
      from launch_ros.parameter_descriptions import ParameterValue
      
      
      def generate_launch_description():
          use_sim_time = LaunchConfiguration("use_sim_time", default="true")
      
          jetauto_bot_path = get_package_share_directory("gazebo_simulation")
          world_file = LaunchConfiguration("world_file", default=join(jetauto_bot_path, "worlds", "world.sdf"))
          gz_sim_share = get_package_share_directory("ros_gz_sim")
          
          pkg_gazebo_simulation = FindPackageShare("gazebo_simulation")
      
          # Launch Configurations for robot spawn
          #LaunchConfiguration 是启动时的“权威来源”,它们可以在运行时覆盖内部参数
          robot_name = LaunchConfiguration("robot_name", default="jetauto")
          position_x = LaunchConfiguration("position_x", default="0.0")
          position_y = LaunchConfiguration("position_y", default="0.0")
          orientation_yaw = LaunchConfiguration("orientation_yaw", default="0.0")
      
          # File paths
          xacro_file = PathJoinSubstitution([
              pkg_gazebo_simulation,
              'urdf', 'jetauto.xacro'
          ])
      
          controller_config_file = PathJoinSubstitution([
              pkg_gazebo_simulation,
              'config', 'jetauto_controllers.yaml'
          ])
      
          # 添加更多Gazebo配置选项
          gz_verbose = LaunchConfiguration("gz_verbose", default="false")
          gz_gui = LaunchConfiguration("gz_gui", default="true")
          
          # Gazebo simulation
          gz_sim = IncludeLaunchDescription(
              PythonLaunchDescriptionSource(join(gz_sim_share, "launch", "gz_sim.launch.py")),
              launch_arguments={
                  "gz_args": [world_file, " -r "]
              }.items()
          )
      
          # 独立的robot_state_publisher - 专门负责发布机器人描述和静态变换
          
          robot_state_publisher = Node(
              package="robot_state_publisher",
              executable="robot_state_publisher",
              name="robot_state_publisher",
              # namespace=robot_name, 
              output="screen",
              parameters=[
                  {'use_sim_time': use_sim_time},
                  {'robot_description': ParameterValue(
                      Command([
                      'xacro ', xacro_file,
                      ' robot_name:=', robot_name,
                      ' sim_ign:=', "true"
                  ]),
                  value_type=str
                  )}
              ],
          )
      
          # Spawn robot entity - delayed to ensure Gazebo is ready
          gz_spawn_entity = TimerAction(
              period=3.0,
              actions=[
                  Node(
                      package="ros_gz_sim",
                      executable="create",
                      output="screen",
                      arguments=[
                          "-topic", "/robot_description",
                          "-name", robot_name,
                          "-allow_renaming", "true",
                          "-z", "0.28",
                          "-x", position_x,
                          "-y", position_y,
                          "-Y", orientation_yaw
                      ]
                  )
              ]
          )
      
          # Controller manager
          controller_manager = TimerAction(
              period=5.0,
              actions=[
                  Node(
                      package="controller_manager",
                      executable="ros2_control_node",
                      parameters=[controller_config_file, {'use_sim_time': use_sim_time}],
                      output="screen",
                  )
              ]
          )
      
          # Joint state broadcaster spawner - delayed
          joint_state_broadcaster_spawner = TimerAction(
              period=5.0,
              actions=[
                  Node(
                      package="controller_manager",
                      executable="spawner",
                      arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
                      output="screen",
                  )
              ]
          )
      
          # Mecanum drive controller spawner - delayed
          mecanum_drive_spawner = TimerAction(
              period=5.0,
              actions=[
                  Node(
                      package="controller_manager",
                      executable="spawner",
                      arguments=["jetauto_base_controller", "--controller-manager", "/controller_manager"],
                      output="screen",
                  )
              ]
          )
      
          # ROS-Gazebo bridge
          ign_ros2_bridge = Node(
              package="ros_gz_bridge",
              executable="parameter_bridge",
              name="ros_gz_bridge",
              output="screen",
              arguments=[
                  # LaserScan
                  "/model/jetauto/lidar_scan@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan",
      
                  # IMU
                  "/model/jetauto/imu_data@sensor_msgs/msg/Imu[ignition.msgs.IMU",
      
                  # 深度相机 - 图像
                  "/model/jetauto/depth_camera@sensor_msgs/msg/Image[ignition.msgs.Image",
      
                  # 深度相机 - 点云
                  "/model/jetauto/depth_camera/points@sensor_msgs/msg/PointCloud2[ignition.msgs.PointCloudPacked",
      
                  # _深度相机 - CameraInfo_ (ROS Topic Name 和 IGN Topic Name 不同)
                  "/model/jetauto/camera_info@sensor_msgs/msg/CameraInfo[ignition.msgs.CameraInfo",
              ],
              # arguments=[
              #     "/clock@rosgraph_msgs/msg/Clock]ignition.msgs.Clock",
              #     "/odom@nav_msgs/msg/Odometry]@/odom@ignition.msgs.Odometry",
              #     # LaserScan
              #     "/model/jetauto/lidar_scan@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan",
              #     # IMU
              #     "/model/jetauto/imu_data@sensor_msgs/msg/Imu[ignition.msgs.IMU",
              #     # 深度相机数据桥接
              #     "/model/jetauto/depth_camera@sensor_msgs/msg/Image[ignition.msgs.Image",
              #     "/model/jetauto/depth_camera/points@sensor_msgs/msg/PointCloud2[ignition.msgs.PointCloudPacked",
              #     "/model/jetauto/depth_camera/camera_info@sensor_msgs/msg/CameraInfo[ignition.msgs.CameraInfo",
              # ],
              parameters=[ {'use_sim_time': use_sim_time}]
              )
          return LaunchDescription([
      
              AppendEnvironmentVariable(
                  name='IGN_GAZEBO_RESOURCE_PATH',
                  value=join(jetauto_bot_path, "worlds")),
      
              AppendEnvironmentVariable(
                  name='IGN_GAZEBO_RESOURCE_PATH',
                  value=join(jetauto_bot_path, "models")),
      
              # Declare arguments
              DeclareLaunchArgument("use_sim_time", default_value="true"),
              DeclareLaunchArgument("world_file", default_value=join(jetauto_bot_path, "worlds", "world.sdf")),
              DeclareLaunchArgument("robot_name", default_value="jetauto", description="Name of the robot"),
              DeclareLaunchArgument("position_x", default_value="0.0", description="Initial X position"),
              DeclareLaunchArgument("position_y", default_value="0.0", description="Initial Y position"),
              DeclareLaunchArgument("orientation_yaw", default_value="0.0", description="Initial Yaw orientation"),
              
              # 添加新的launch参数声明
              DeclareLaunchArgument("gz_verbose", default_value="false", description="Enable verbose Gazebo output"),
              DeclareLaunchArgument("gz_gui", default_value="true", description="Enable Gazebo GUI"),
              DeclareLaunchArgument("physics_engine", 
                                  default_value="ode", 
                                  description="Physics engine (ode/bullet/dart/simbody). ODE is default and most stable."),
              
              # Launch nodes
              gz_sim,
              robot_state_publisher,
              gz_spawn_entity,
              controller_manager,
              joint_state_broadcaster_spawner,
              mecanum_drive_spawner,
              ign_ros2_bridge,
          ])
      
      
      1 条回复 最后回复 回复 引用 0
      • 第一个帖子
        最后一个帖子
      皖ICP备16016415号-7
      Powered by NodeBB | 鱼香ROS