修正小车转向问题

src/robot_bringup/launch/robot_bringup.launch

@@ -22,7 +22,10 @@
     <!-- (3) startup rasp imu-6DOF board-->
     <include file="$(find rasp_imu_hat_6dof)/launch/imu_data_pub.launch" />
 
-    <!-- (4) startup robot_pose_ekf node -->
+    <!-- (4) startup rasp cam-->
+    <include file="$(find raspicam_node)/launch/camerav1_1280x720.launch" />
+
+    <!-- (5) startup robot_pose_ekf node -->
     <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
         <param name="output_frame" value="odom_combined"/>
         <param name="freq" value="10.0"/>
@@ -34,7 +37,7 @@
         <param name="self_diagnose" value="false"/>
     </node>
 
-    <!-- (5) /odom_combined view in rviz -->
+    <!-- (6) /odom_combined view in rviz -->
     <node pkg="robot_bringup" type="odom_ekf.py" name="odom_ekf" output="screen">
         <remap from="input" to="/robot_pose_ekf/odom_combined" />
         <remap from="output" to="/odom_ekf" />

src/ros_arduino_bridge/ros_arduino_python/config/my_arduino_params.yaml

@@ -37,7 +37,7 @@ wheel_track: 0.15     #L value
 encoder_resolution: 13 #12V DC motors
 gear_reduction: 30     #empty payload rpm 130 r/m
 linear_scale_correction: 1.028
-angular_scale_correction: 1.00
+angular_scale_correction: 2.00
 
 # === PID parameters
 debugPID: False

src/ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/base_controller.py

@@ -195,12 +195,12 @@ class BaseController:
             vb = dist_B/dt
 
             #forward kinemation
-            vx  = (va + vb)/2.0
+            vx  = (-va + vb)/2.0
             vy  = 0
-            vth = (vb - va)/(self.wheel_track)
+            vth = (-va - vb)/(self.wheel_track)
 
-            delta_x  = (vx*cos(self.th) - vy*sin(self.th))*dt
-            delta_y  = (vx*sin(self.th) + vy*cos(self.th))*dt
+            delta_x  = (vx*cos(self.th))*dt
+            delta_y  = (vx*sin(self.th))*dt
             delta_th = vth*dt
 
             self.x  += delta_x
@@ -325,8 +325,8 @@ class BaseController:
         th = req.angular.z # rad/s
 
         #Inverse Kinematic
-        vA = (-x + 0.5*self.wheel_track*th)
-        vB = (x + 0.5*self.wheel_track*th)
+        vA = -(x + 0.5*self.wheel_track*th)
+        vB = (x - 0.5*self.wheel_track*th)
 
         self.v_des_AWheel = int(vA * self.ticks_per_meter / self.arduino.PID_RATE)
         self.v_des_BWheel = int(vB * self.ticks_per_meter / self.arduino.PID_RATE)

src/teleop_twist_keyboard/teleop_twist_keyboard.py

@@ -69,7 +69,7 @@ if __name__=="__main__":
     rospy.init_node('teleop_twist_keyboard')
 
     speed = rospy.get_param("~speed", 0.2)
-    turn = rospy.get_param("~turn", 0.8)
+    turn = rospy.get_param("~turn", 1.4)
 
     x = 0
     y = 0