ignition中麦克纳姆轮仿真,小车无法左右移动
-
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, ])