更改机器人模型

src/dual_iic/src/main.cpp

@@ -31,7 +31,7 @@ int main()
         // int battery = battery_dev.I2C_Read_Battery();
         std::string bat_percent = std::to_string(battery_dev.I2C_Read_Battery()) + "%";
         oled_driver.GUI_Show(bat_percent);
-		sleep(60);
+		sleep(20);
 	}
     //3.System Exit
 	Device::DEV_ModuleExit();

src/robot_description/urdf/robot.urdf

@@ -2,32 +2,35 @@
   <material name="blue">
     <color rgba="0.1 0.1 0.75 0.9"/>
   </material>
-
-  <link name="base_link">
+  
+  <link
+    name="base_link">
     <inertial>
       <origin
-        xyz="-0.00915640899856677 -1.23594917594765E-18 0.116771439156292"
+        xyz="-1.30824055561016E-05 0.000107844191407841 0.0577157555063256"
         rpy="0 0 0" />
       <mass
-        value="1.53513901604802" />
+        value="0.715394481464921" />
       <inertia
-        ixx="0.0429859907839231"
-        ixy="1.40607469244214E-18"
-        ixz="0.00161915133178891"
-        iyy="0.0421680076448672"
-        iyz="-2.37846851589008E-18"
-        izz="0.0250023780420302" />
-    </inertial> 
+        ixx="0.000779865515276959"
+        ixy="1.02445392057065E-07"
+        ixz="6.95832095402999E-08"
+        iyy="0.000862321943367308"
+        iyz="2.63518497731006E-08"
+        izz="0.00160121250638599" />
+    </inertial>
     <visual>
       <origin
         xyz="0 0 0"
         rpy="0 0 0" />
       <geometry>
-        <mesh filename="package://robot_description/meshes/base_link.STL" />
+        <mesh
+          filename="package://robot_description/meshes/base_link.STL" />
       </geometry>
       <material
         name="">
-        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+        <color
+          rgba="0.941176470588235 0.941176470588235 0.941176470588235 1" />
       </material>
     </visual>
     <collision>
@@ -35,79 +38,457 @@
         xyz="0 0 0"
         rpy="0 0 0" />
       <geometry>
-        <mesh filename="package://robot_description/meshes/base_link.STL" />
+        <mesh
+          filename="package://robot_description/meshes/base_link.STL" />
       </geometry>
     </collision>
   </link>
-
-  <link name="base_footprint">
+  <link
+    name="left_wheel_link">
+    <inertial>
+      <origin
+        xyz="-7.66164786097183E-18 8.40811733798796E-05 -3.46944695195361E-18"
+        rpy="0 0 0" />
+      <mass
+        value="0.056636933308092" />
+      <inertia
+        ixx="2.5061060504477E-05"
+        ixy="-8.26321294879238E-18"
+        ixz="7.70223898149877E-08"
+        iyy="4.40641001526507E-05"
+        iyz="-1.61023780640282E-17"
+        izz="2.52148616363739E-05" />
+    </inertial>
     <visual>
-        <geometry>
-          <box size="0.005 0.005 0.005" />
-        </geometry>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://robot_description/meshes/left_wheel_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.972549019607843 1 1" />
+      </material>
     </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://robot_description/meshes/left_wheel_link.STL" />
+      </geometry>
+    </collision>
   </link>
-  <joint name="base_footprint_joint" type="fixed">
-    <origin xyz="0 0 0.05"/>
-    <parent link="base_footprint"/>
-    <child link="base_link"/>
+  <joint
+    name="left_wheel_joint"
+    type="continuous">
+    <origin
+      xyz="0 0.087226 -0.02"
+      rpy="0 0 0" />
+    <parent
+      link="base_link" />
+    <child
+      link="left_wheel_link" />
+    <axis
+      xyz="0 1 0" />
   </joint>
-
-  <link name="base_imu_link" type="fixed">
+  <link
+    name="right_wheel_link">
+    <inertial>
+      <origin
+        xyz="-1.13392370823798E-18 -8.40811733799074E-05 -1.38777878078145E-17"
+        rpy="0 0 0" />
+      <mass
+        value="0.056636933308092" />
+      <inertia
+        ixx="2.5061060504477E-05"
+        ixy="-8.27358108286742E-18"
+        ixz="-7.7022389814995E-08"
+        iyy="4.40641001526506E-05"
+        iyz="1.6102381956134E-17"
+        izz="2.52148616363739E-05" />
+    </inertial>
     <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
       <geometry>
-        <box size="0.056 0.065 0.002" />
+        <mesh
+          filename="package://robot_description/meshes/right_wheel_link.STL" />
       </geometry>
-        <material name="blue" />
+      <material
+        name="">
+        <color
+          rgba="0.43921568627451 0.43921568627451 0.43921568627451 1" />
+      </material>
     </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://robot_description/meshes/right_wheel_link.STL" />
+      </geometry>
+    </collision>
   </link>
-  <joint name="base_link_to_IMU" type="fixed">
-    <origin xyz="0 0 0.10" />
-    <parent link="base_link" />
-    <child link="base_imu_link" />
+  <joint
+    name="right_wheel_joint"
+    type="continuous">
+    <origin
+      xyz="0 -0.087226 -0.02"
+      rpy="0 0 0" />
+    <parent
+      link="base_link" />
+    <child
+      link="right_wheel_link" />
+    <axis
+      xyz="0 -1 0" />
   </joint>
-
-  <link name="lidar" type="fixed">
+  <link
+    name="front_wheel_link">
     <inertial>
-      <origin>
+      <origin
+        xyz="0.0152655251450065 0.000504489640389129 -0.0275267772713533"
+        rpy="0 0 0" />
+      <mass
+        value="0.0323710595794446" />
+      <inertia
+        ixx="5.13858672949531E-06"
+        ixy="-7.37337358165628E-08"
+        ixz="2.43411911798529E-07"
+        iyy="7.36892066003594E-06"
+        iyz="8.05146812807082E-09"
+        izz="5.0594631354647E-06" />
+    </inertial>
+    <visual>
+      <origin
         xyz="0 0 0"
-        rpy="0 0 0"
-      </origin>
-      <mass value="0.19" />
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://robot_description/meshes/front_wheel_link.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://robot_description/meshes/front_wheel_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="front_wheel_joint"
+    type="continuous">
+    <origin
+      xyz="0.075 0 -0.0023763"
+      rpy="0 0 0" />
+    <parent
+      link="base_link" />
+    <child
+      link="front_wheel_link" />
+    <axis
+      xyz="0 0 -1" />
+  </joint>
+  <link
+    name="back_wheel_link">
+    <inertial>
+      <origin
+        xyz="-0.0152655251450065 -0.000504489640389126 -0.0275267772713533"
+        rpy="0 0 0" />
+      <mass
+        value="0.0323710595794446" />
       <inertia
-        ixx="0.0429859907839231"
-        ixy="1.40607469244214E-18"
-        ixz="0.00161915133178891"
-        iyy="0.0421680076448672"
-        iyz="-2.37846851589008E-18"
-        izz="0.0250023780420302" />
+        ixx="5.13858672949531E-06"
+        ixy="-7.37337358165606E-08"
+        ixz="-2.43411911798526E-07"
+        iyy="7.36892066003594E-06"
+        iyz="-8.0514681280719E-09"
+        izz="5.0594631354647E-06" />
     </inertial>
     <visual>
-      <origin>
+      <origin
         xyz="0 0 0"
-        rpy="1.57079 0 0"
-      </origin>
+        rpy="0 0 0" />
       <geometry>
-        <cylinder length="0.0535" radius="0.03" />
+        <mesh
+          filename="package://robot_description/meshes/back_wheel_link.STL" />
       </geometry>
-      <material name="">
-        <color rgba="0.79216 0.81961 0.9333 0.5" />
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
       </material>
     </visual>
     <collision>
-      <origin>
+      <origin
         xyz="0 0 0"
-        rpy="1.57.79 0 0"
-      </origin>
+        rpy="0 0 0" />
       <geometry>
-        <cylinder length="0.042" radius="0.038" />
+        <mesh
+          filename="package://robot_description/meshes/back_wheel_link.STL" />
       </geometry>
     </collision>
   </link>
-  <joint name="lidar_to_base" type="fixed">
-    <origin xyz="0 0 0.1817"/>
-    <parent link="base_link"/>
-    <child link="lidar"/>
+  <joint
+    name="back_wheel_joint"
+    type="continuous">
+    <origin
+      xyz="-0.075 0 -0.0023763"
+      rpy="0 0 0" />
+    <parent
+      link="base_link" />
+    <child
+      link="back_wheel_link" />
+    <axis
+      xyz="0 0 -1" />
+  </joint>
+  <link
+    name="servo_left_right_link">
+    <inertial>
+      <origin
+        xyz="-0.00342029959962582 -8.31447029908761E-05 0.0164292484197976"
+        rpy="0 0 0" />
+      <mass
+        value="0.0144748073722143" />
+      <inertia
+        ixx="1.72385755753649E-06"
+        ixy="-4.21182747869309E-08"
+        ixz="-2.42902265623225E-07"
+        iyy="2.23868737048435E-06"
+        iyz="-2.7900037637553E-08"
+        izz="2.38358373792003E-06" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://robot_description/meshes/servo_left_right_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.647058823529412 0.647058823529412 0.647058823529412 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://robot_description/meshes/servo_left_right_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="servo_left_right_joint"
+    type="revolute">
+    <origin
+      xyz="0.06041 0 0.08525"
+      rpy="0 0 0" />
+    <parent
+      link="base_link" />
+    <child
+      link="servo_left_right_link" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-1.57"
+      upper="1.57"
+      effort="0"
+      velocity="0" />
+  </joint>
+  <link
+    name="servo_up_down_link">
+    <inertial>
+      <origin
+        xyz="0.00464326452490638 0.0203000236898356 0.00545497641365948"
+        rpy="0 0 0" />
+      <mass
+        value="0.0132043090272951" />
+      <inertia
+        ixx="1.42353069009727E-06"
+        ixy="1.0990050953523E-09"
+        ixz="5.62721878758881E-09"
+        iyy="7.90092710603907E-07"
+        iyz="-2.31532216640704E-08"
+        izz="9.07216677821165E-07" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://robot_description/meshes/servo_up_down_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.933333333333333 0.854901960784314 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://robot_description/meshes/servo_up_down_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="servo_up_down_joint"
+    type="revolute">
+    <origin
+      xyz="0.018197 -0.018843 0.02995"
+      rpy="0 0 0" />
+    <parent
+      link="servo_left_right_link" />
+    <child
+      link="servo_up_down_link" />
+    <axis
+      xyz="-0.016183 -0.99987 0" />
+    <limit
+      lower="-1"
+      upper="1"
+      effort="0"
+      velocity="0" />
+  </joint>
+  <link
+    name="lidar">
+    <inertial>
+      <origin
+        xyz="0 -2.9549326452619E-18 -6.1819252281381E-06"
+        rpy="0 0 0" />
+      <mass
+        value="0.0480239560991004" />
+      <inertia
+        ixx="1.19703636663716E-05"
+        ixy="8.06458768868579E-10"
+        ixz="5.15393269333354E-24"
+        iyy="1.19498999654021E-05"
+        iyz="-9.10638182720746E-24"
+        izz="2.16092905406288E-05" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://robot_description/meshes/lidar_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0 0.36078431372549 0.580392156862745 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://robot_description/meshes/lidar_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="lidar_joint"
+    type="fixed">
+    <origin
+      xyz="-0.0021454 0 0.1992"
+      rpy="0 0 0" />
+    <parent
+      link="base_link" />
+    <child
+      link="lidar" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+  <link
+    name="base_imu_link">
+    <inertial>
+      <origin
+        xyz="2.881138598591E-05 -0.000165484463082012 -0.00158258525002669"
+        rpy="0 0 0" />
+      <mass
+        value="0.000546284032069355" />
+      <inertia
+        ixx="1.04502955010604E-08"
+        ixy="5.20039658558943E-13"
+        ixz="-3.8366937703985E-13"
+        iyy="7.88905624806412E-09"
+        iyz="2.70332847255175E-12"
+        izz="1.81411505296782E-08" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://robot_description/meshes/imu_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.223529411764706 0.223529411764706 0.223529411764706 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://robot_description/meshes/imu_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="imu_joint"
+    type="fixed">
+    <origin
+      xyz="-0.022128 0.00056024 0.10609"
+      rpy="0 0 0" />
+    <parent
+      link="base_link" />
+    <child
+      link="base_imu_link" />
+    <axis
+      xyz="0 0 0" />
+  </joint>
+
+  <link name="base_footprint">
+    <visual>
+        <geometry>
+          <box size="0.005 0.005 0.005" />
+        </geometry>
+    </visual>
+  </link>
+  <joint name="base_footprint_joint" type="fixed">
+    <origin xyz="0 0 0.05"/>
+    <parent link="base_footprint"/>
+    <child link="base_link"/>
   </joint>
 
 </robot>