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

    Gazebo与RViz联合仿真时odom消失

    已定时 已固定 已锁定 已移动 已解决
    综合问题
    3
    21
    3.2k
    正在加载更多帖子
    • 从旧到新
    • 从新到旧
    • 最多赞同
    回复
    • 在新帖中回复
    登录后回复
    此主题已被删除。只有拥有主题管理权限的用户可以查看。
    • 9
      924453613
      最后由 编辑

      解决后代码:
      irobot.urdf

      <?xml version="1.0" encoding="utf-8"?>
      <robot
        name="irobot">
        <link
          name="base_link">
          <inertial>
            <origin
              xyz="0.035298 6.2375E-05 0.0016857"
              rpy="0 0 0" />
            <mass
              value="5" />
            <inertia
              ixx="0.038"
              ixy="0"
              ixz="0"
              iyy="0.026"
              iyz="0"
              izz="0.06" />
          </inertial>
          <visual>
            <origin
              xyz="0 0 0"
              rpy="0 0 0" />
            <geometry>
              <mesh
                filename="package://irobot/meshes/base_link.STL" />
            </geometry>
            <material
              name="">
              <color
                rgba="0 0 0 1" />
            </material>
          </visual>
          <collision>
            <origin
              xyz="0 0 0"
              rpy="0 0 0" />
            <geometry>
              <mesh
                filename="package://irobot/meshes/base_link.STL" />
            </geometry>
          </collision>
        </link>
        <link
          name="top_link">
          <inertial>
            <origin
              xyz="-0.0050829 -0.00015289 0.0008765"
              rpy="0 0 0" />
            <mass
              value="0.2" />
            <inertia
              ixx="0.00081"
              ixy="0"
              ixz="0"
              iyy="0.00071"
              iyz="0"
              izz="0.0015" />
          </inertial>
          <visual>
            <origin
              xyz="0 0 0"
              rpy="0 0 0" />
            <geometry>
              <mesh
                filename="package://irobot/meshes/top_link.STL" />
            </geometry>
            <material
              name="">
              <color
                rgba="0 1 0 1" />
            </material>
          </visual>
          <collision>
            <origin
              xyz="0 0 0"
              rpy="0 0 0" />
            <geometry>
              <mesh
                filename="package://irobot/meshes/top_link.STL" />
            </geometry>
          </collision>
        </link>
        <joint
          name="top_link2base_link"
          type="fixed">
          <origin
            xyz="0 0 0.033795"
            rpy="0 0 0" />
          <parent
            link="base_link" />
          <child
            link="top_link" />
          <axis
            xyz="0 -1 0" />
          <limit
            lower="0"
            upper="0"
            effort="0"
            velocity="0" />
        </joint>
        <link
          name="laser_link">
          <inertial>
            <origin
              xyz="1.1845E-09 4.4075E-09 0.0030295"
              rpy="0 0 0" />
            <mass
              value="0.1" />
            <inertia
              ixx="4.11E-08"
              ixy="0"
              ixz="0"
              iyy="4.11E-08"
              iyz="0"
              izz="7.88E-08" />
          </inertial>
          <visual>
            <origin
              xyz="0 0 0"
              rpy="0 0 0" />
            <geometry>
              <mesh
                filename="package://irobot/meshes/laser_link.STL" />
            </geometry>
            <material
              name="">
              <color
                rgba="1 0 0 1" />
            </material>
          </visual>
          <collision>
            <origin
              xyz="0 0 0"
              rpy="0 0 0" />
            <geometry>
              <mesh
                filename="package://irobot/meshes/laser_link.STL" />
            </geometry>
          </collision>
        </link>
        <joint
          name="laser_link2base_link"
          type="fixed">
          <origin
            xyz="0.155 0 0.038215"
            rpy="0 0 0" />
          <parent
            link="base_link" />
          <child
            link="laser_link" />
          <axis
            xyz="0 -1 0" />
          <limit
            lower="0"
            upper="0"
            effort="0"
            velocity="0" />
        </joint>
        <link
          name="right_wheel">
          <inertial>
            <origin
              xyz="-1.0317E-18 1.3878E-17 -1.7347E-18"
              rpy="0 0 0" />
            <mass
              value="0.05" />
            <inertia
              ixx="2.3E-05"
              ixy="0"
              ixz="0"
              iyy="4.3E-05"
              iyz="0"
              izz="2.3E-05" />
          </inertial>
          <visual>
            <origin
              xyz="0 0 0"
              rpy="0 0 0" />
            <geometry>
              <mesh
                filename="package://irobot/meshes/right_wheel.STL" />
            </geometry>
            <material
              name="">
              <color
                rgba="1 1 1 1" />
            </material>
          </visual>
          <collision>
            <origin
              xyz="0 0 0"
              rpy="0 0 0" />
            <geometry>
              <mesh
                filename="package://irobot/meshes/right_wheel.STL" />
            </geometry>
          </collision>
        </link>
        <joint
          name="right_wheel2base_link"
          type="continuous">
          <origin
            xyz="0 0.10045 -0.010735"
            rpy="0 0 0" />
          <parent
            link="base_link" />
          <child
            link="right_wheel" />
          <axis
            xyz="0 -1 0" />
          <limit
            lower="-3.14"
            upper="3.14"
            effort="100"
            velocity="2" />
        </joint>
        <link
          name="left_wheel">
          <inertial>
            <origin
              xyz="-1.1842E-18 0 -1.7347E-18"
              rpy="0 0 0" />
            <mass
              value="0.05" />
            <inertia
              ixx="2.3E-05"
              ixy="0"
              ixz="0"
              iyy="4.3E-05"
              iyz="0"
              izz="2.3E-05" />
          </inertial>
          <visual>
            <origin
              xyz="0 0 0"
              rpy="0 0 0" />
            <geometry>
              <mesh
                filename="package://irobot/meshes/left_wheel.STL" />
            </geometry>
            <material
              name="">
              <color
                rgba="1 1 1 1" />
            </material>
          </visual>
          <collision>
            <origin
              xyz="0 0 0"
              rpy="0 0 0" />
            <geometry>
              <mesh
                filename="package://irobot/meshes/left_wheel.STL" />
            </geometry>
          </collision>
        </link>
        <joint
          name="left_wheel2base_link"
          type="continuous">
          <origin
            xyz="0 -0.10045 -0.010735"
            rpy="0 0 0" />
          <parent
            link="base_link" />
          <child
            link="left_wheel" />
          <axis
            xyz="0 -1 0" />
          <limit
            lower="-3.14"
            upper="3.14"
            effort="100"
            velocity="2" />
        </joint>
        <link
          name="driven_wheel">
          <inertial>
            <origin
              xyz="0 -3.4443E-19 0"
              rpy="0 0 0" />
            <mass
              value="0.02" />
            <inertia
              ixx="1.27E-06"
              ixy="0"
              ixz="0"
              iyy="1.27E-06"
              iyz="0"
              izz="1.27E-06" />
          </inertial>
          <visual>
            <origin
              xyz="0 0 0"
              rpy="0 0 0" />
            <geometry>
              <mesh
                filename="package://irobot/meshes/driven_wheel.STL" />
            </geometry>
            <material
              name="">
              <color
                rgba="1 1 1 1" />
            </material>
          </visual>
          <collision>
            <origin
              xyz="0 0 0"
              rpy="0 0 0" />
            <geometry>
              <mesh
                filename="package://irobot/meshes/driven_wheel.STL" />
            </geometry>
          </collision>
        </link>
        <joint
          name="driven_wheel2base_link"
          type="fixed">
          <origin
            xyz="0.13 0 -0.031735"
            rpy="0 0 0" />
          <parent
            link="base_link" />
          <child
            link="driven_wheel" />
          <axis
            xyz="0 -1 0" />
          <limit
            lower="0"
            upper="0"
            effort="0"
            velocity="0" />
        </joint>
        <link
          name="base_footprint">
          <visual>
            <geometry>
              <sphere
                radius="0.001" />
            </geometry>
            <material
              name="">
              <color
                rgba="0 0 0 0" />
            </material>
          </visual>
        </link>
        <joint
          name="base_link2base_footprint"
          type="fixed">
              <parent
                link="base_footprint" />
              <child
                link="base_link" />
              <origin
                xyz="0 0 0.047" />
        </joint>
      <!--color-->
        <gazebo reference="base_link">
          <material>Gazebo/Black</material>
        </gazebo>
        <gazebo reference="top_link">
          <material>Gazebo/Green</material>
        </gazebo>
        <gazebo reference="laser_link">
          <material>Gazebo/Red</material>
        </gazebo>
        <gazebo reference="camera_link">
          <material>Gazebo/Red</material>
        </gazebo>
        <gazebo reference="driven_wheel">
          <material>Gazebo/White</material>
        </gazebo>
        <gazebo reference="left_wheel">
          <material>Gazebo/White</material>
        </gazebo>
        <gazebo reference="right_wheel">
          <material>Gazebo/White</material>
        </gazebo>
        <!-- controller -->
        <transmission name="left_wheel2base_link_trans">
          <type>transmission_interface/SimpleTransmission</type>
          <joint name="left_wheel2base_link">
            <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
          </joint>
          <actuator name="left_wheel2base_link_motor">
            <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
          </actuator>
        </transmission>
        <transmission name="right_wheel2base_link_trans">
          <type>transmission_interface/SimpleTransmission</type>
          <joint name="right_wheel2base_link">
            <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
          </joint>
          <actuator name="right_wheel2base_link_motor">
            <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
          </actuator>
        </transmission>
        <gazebo>
          <plugin filename="libgazebo_ros_diff_drive.so" name="differential_drive_controller">
            <rosDebugLevel>Debug</rosDebugLevel>
            <publishWheelTF>true</publishWheelTF>
            <robotNamespace>/</robotNamespace>
            <publishTf>1</publishTf>
            <publishWheelJointState>true</publishWheelJointState>
            <alwaysOn>true</alwaysOn>
            <updateRate>100.0</updateRate>
            <legacyMode>true</legacyMode>
            <leftJoint>left_wheel2base_link</leftJoint>
            <rightJoint>right_wheel2base_link</rightJoint>
            <wheelSeparation>0.277</wheelSeparation>
            <wheelDiameter>0.073</wheelDiameter>
            <broadcastTF>1</broadcastTF>
            <wheelTorque>30</wheelTorque>
            <wheelAcceleration>1.8</wheelAcceleration>
            <commandTopic>cmd_vel</commandTopic>
            <odometryFrame>odom</odometryFrame>
            <odometryTopic>odom</odometryTopic>
            <robotBaseFrame>base_footprint</robotBaseFrame>
          </plugin>
        </gazebo>
        <!--laser-->
         <gazebo reference="laser_link">
          <sensor name="rplidar" type="ray">
            <pose>0 0 0 0 0 0</pose>
            <visualize>true</visualize>
            <update_rate>5.5</update_rate>
            <ray>
              <scan>
                <horizontal>
                  <samples>360</samples>
                  <resolution>1</resolution>
                  <min_angle>-3</min_angle>
                  <max_angle>3</max_angle>
                </horizontal>
              </scan>
              <range>
                <min>0.10</min>
                <max>30.0</max>
                <resolution>0.01</resolution>
              </range>
              <noise>
                <type>gaussian</type>
                <mean>0.0</mean>
                <stddev>0.01</stddev>
              </noise>
            </ray>
            <plugin filename="libgazebo_ros_laser.so" name="gazebo_rplidar">
              <topicName>/scan</topicName>
              <frameName>laser_link</frameName>
            </plugin>
          </sensor>
        </gazebo> 
      </robot>
      

      odom_gazebo.launch

      <launch>
        <include
          file="$(find gazebo_ros)/launch/empty_world.launch" />
        <node
          name="spawn_model"
          pkg="gazebo_ros"
          type="spawn_model"
          args="-file $(find irobot)/urdf/irobot.urdf -urdf -model irobot"
          output="screen" />
        <node
          name="fake_joint_calibration"
          pkg="rostopic"
          type="rostopic"
          args="pub /calibrated std_msgs/Bool true" />
      </launch>
      

      odom_rviz.launch

      <launch>
        <param
          name="robot_description"
          textfile="$(find irobot)/urdf/irobot.urdf" />
        <node
          name="joint_state_publisher_gui"
          pkg="joint_state_publisher_gui"
          type="joint_state_publisher_gui" />
        <node
          name="robot_state_publisher"
          pkg="robot_state_publisher"
          type="robot_state_publisher" />
        <node
          name="rviz"
          pkg="rviz"
          type="rviz"
          args="-d $(find irobot)/config/irobot.rviz" />
      </launch>
      
      1 条回复 最后回复 回复 引用 0
      • 小鱼小 小鱼 将这个主题转为问答主题,在
      • 小鱼小 小鱼 将这个主题标记为已解决,在
      • 第一个帖子
        最后一个帖子
      皖ICP备16016415号-7
      Powered by NodeBB | 鱼香ROS