更新arduino部分代码,删除一些无需配置的参数,提高代码稳定性

src/robot_navigation/config/dwa_local_planner_params.yaml

@@ -46,8 +46,8 @@ DWAPlannerROS:
   vth_samples: 20
 
 # Trajectory Scoring Parameters
-  path_distance_bias: 32.0
-  goal_distance_bias: 24.0
+  path_distance_bias: 48.0
+  goal_distance_bias: 18.0
   occdist_scale: 0.02
   forward_point_distance: 0.2
   stop_time_buffer: 0.6

src/ros_arduino_bridge/ros_arduino_python/cfg/my_arduino_params.yaml

@@ -2,8 +2,6 @@
 # Description:arduino节点运行时加载的参数,下面对参数进行解释:
 #   is_use_serial:与arduino通信是否使用串口,false的话就是IIC通信
 #   serial_port:串口通信时arduino的端口号
-#   i2c_smbus_num:系统管理总线号,树莓派为1
-#   isc_slave_addr:arduino的IIC设备地址
 #   default_main_rate:默认主循环的频率,不在平衡小车模式下
 #   cmd_topic:底盘订阅的控制移动的话题名称
 # Author: corvin
@@ -12,15 +10,13 @@
 #   20211016:增加两轮小车平衡模式下的参数.
 #   20211116:增加默认主循环和平衡小车模式下两个不同配置参数.
 #   20220224:删除通信波特率配置参数,因为是固定无需修改.
+#   20220929:删除i2c_smbus_num和i2c_slave_addr参数,因为无需修改.
 
 is_use_serial: True
 
-#连接arduino设备的串口号和波特率
+#连接arduino设备的串口号
 serial_port: /dev/ttyACM0
 
-i2c_smbus_num: 1   #raspberryPi:1, huawei altals 200DK: 2
-i2c_slave_addr: 0x08  #Get arduino due i2c slave addr
-
 default_main_rate: 20
 cmd_topic: cmd_vel
 

src/ros_arduino_bridge/ros_arduino_python/nodes/arduino_node.py

@@ -23,8 +23,6 @@ class ArduinoROS():
 
         self.is_serial        = rospy.get_param("~is_use_serial", True)
         self.serial_port      = rospy.get_param("~serial_port", "/dev/ttyACM0")
-        self.i2c_smbus_num    = rospy.get_param("~i2c_smbus_num", 1)
-        self.i2c_slave_addr   = rospy.get_param("~i2c_slave_addr", 8)
         self.base_frame       = rospy.get_param("~base_frame", 'base_footprint')
         self.cmd_topic        = rospy.get_param("~cmd_topic", "cmd_vel")
         self.balance_mode     = rospy.get_param("~balance_mode", False)
@@ -66,9 +64,9 @@ class ArduinoROS():
         rospy.on_shutdown(self.shutdown)
 
         # Initialize the controlller
-        self.controller = Arduino(self.is_serial, self.serial_port, self.i2c_smbus_num, self.i2c_slave_addr)
+        self.controller = Arduino(self.is_serial, self.serial_port)
 
-        #与下位机建立连接
+        #与下位机Arduino建立连接
         self.controller.connect()
 
         #初始化一个线程锁对象
@@ -92,7 +90,7 @@ class ArduinoROS():
 
             try:
                 self.mySensors.append(sensor)
-                rospy.loginfo(name + " " + str(params) + " published on topic " + "/sensor/" + name)
+                #rospy.loginfo(name + " " + str(params) + " published on topic " + "/sensor/" + name)
             except:
                 rospy.logerr("Sensor type " + str(params['type']) + " not recognized.")
 
@@ -154,7 +152,6 @@ class ArduinoROS():
         self.controller.close()
         os._exit(0)
 
-
 if __name__ == '__main__':
     try:
         myArduino = ArduinoROS()

src/ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/arduino_driver.py

@@ -8,35 +8,34 @@
   History:
     20200706:创建代码.
     20210913:为了两轮平衡车模式优化代码.
+    20220929:删除i2c_smbus_num和i2c_slave_addr参数,因为是固定值无需修改,在树莓派中
+      i2c_smbus_num固定为1,下位机arduino的i2c地址参数i2c_slave_addr固定为0x08.
 """
-from serial.serialutil import SerialException
-import thread, smbus, rospy, time, os
-from serial import Serial
 import sys, traceback
+from serial import Serial
+import thread, smbus, rospy, time, os
+from serial.serialutil import SerialException
 
 
 class Arduino:
-
-    def __init__(self, is_use_serial, serial_port, i2c_smbus_num, i2c_slave_addr):
+    def __init__(self, is_use_serial, serial_port):
         self.is_use_serial = is_use_serial  #与下位机arduino的通信方式是否使用串口还是IIC
         self.serial_port   = serial_port
 
-        self.i2c_smbus_num  = i2c_smbus_num
-        self.i2c_slave_addr = i2c_slave_addr
-
         self.timeout          = 0.5
         self.writeTimeout     = 0.5
         self.interCharTimeout = 0.02
+        self.i2c_slave_addr   = 0x08
 
         #初始化一个线程锁对象
         self.mutex = thread.allocate_lock()
 
-    #与下位机arduino建立连接,根据串口或IIC做区分
+    #与下位机arduino建立连接,根据串口或IIC做区分,树莓派中SMBus参数固定为1
     def connect(self):
         if self.is_use_serial:  #使用串口建立连接
             self.serial_connect()
         else:
-            self.i2c_bus = smbus.SMBus(self.i2c_smbus_num)
+            self.i2c_bus = smbus.SMBus(1)
             self.i2c_connect()
 
     #使用串口建立连接,进行通信
@@ -85,7 +84,6 @@ class Arduino:
     #断开上下位机建立的连接,串口或者IIC连接
     def close(self):
         self.beep_ring(0)  #断开连接时的提示音
-        rospy.sleep(1)
         if self.is_use_serial:
             self.serial_port.close()
         else:
@@ -223,7 +221,7 @@ class Arduino:
                          BWheel_Kp, BWheel_Kd, BWheel_Ki, BWheel_Ko ):
         ''' Set the PID parameters on the Arduino by serial
         '''
-        rospy.loginfo("Updating Motors PID parameters ...")
+        #rospy.loginfo("Updating Motors PID parameters ...")
         cmd = 'u '+str(AWheel_Kp)+':'+str(AWheel_Kd)+':'+str(AWheel_Ki)+':'+str(AWheel_Ko)+':'+str(BWheel_Kp)+':'+str(BWheel_Kd)+':'+str(BWheel_Ki)+':'+str(BWheel_Ko)
         self.execute_ack(cmd)
         #rospy.loginfo("pid params now:"+cmd)

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

@@ -16,10 +16,10 @@ import os,rospy,roslib
 import dynamic_reconfigure.client
 
 from math import sin, cos, pi
-from geometry_msgs.msg import Quaternion, Twist
+from std_msgs.msg import Int32
 from nav_msgs.msg import Odometry
+from geometry_msgs.msg import Quaternion,Twist
 from tf.broadcaster import TransformBroadcaster
-from std_msgs.msg import Int32
 from ros_arduino_python.balance_mode import BalanceMode
 
 
@@ -113,16 +113,6 @@ class BaseController:
             self.BVelPub     = rospy.Publisher('Bvel',     Int32, queue_size=5)
 
     def setup_motor_pid(self, pid_params):
-        # Check to see if any PID parameters are missing
-        missing_params = False
-        for param in pid_params:
-            if pid_params[param] == "":
-                print("*** PID Parameter " + param + " is missing. ***")
-                missing_params = True
-
-        if missing_params:
-            os._exit(1)
-
         self.AWheel_Kp = pid_params['AWheel_Kp']
         self.AWheel_Kd = pid_params['AWheel_Kd']
         self.AWheel_Ki = pid_params['AWheel_Ki']
@@ -260,21 +250,21 @@ class BaseController:
         odom.pose.pose.position.y  = self.y
         odom.pose.pose.position.z  = 0
         odom.pose.pose.orientation = quaternion
-        odom.pose.covariance = [1e-9, 0, 0, 0, 0, 0,
-                                0, 1e-3, 1e-9, 0, 0, 0,
-                                0, 0, 1e6, 0, 0, 0,
-                                0, 0, 0, 1e6, 0, 0,
-                                0, 0, 0, 0, 1e6, 0,
-                                0, 0, 0, 0, 0, 1e-9]
+        odom.pose.covariance = [1e-3, 0, 0, 0, 0, 0,
+                                0, 1e-3, 0, 0, 0, 0,
+                                0, 0, 1e6,  0, 0, 0,
+                                0, 0, 0, 1e6,  0, 0,
+                                0, 0, 0, 0, 1e6,  0,
+                                0, 0, 0, 0, 0, 1e-3]
         odom.twist.twist.linear.x  = vx
         odom.twist.twist.linear.y  = 0
         odom.twist.twist.angular.z = vth
-        odom.twist.covariance = [1e-9, 0, 0, 0, 0, 0,
-                                0, 1e-3, 1e-9, 0, 0, 0,
-                                0, 0, 1e6, 0, 0, 0,
-                                0, 0, 0, 1e6, 0, 0,
-                                0, 0, 0, 0, 1e6, 0,
-                                0, 0, 0, 0, 0, 1e-9]
+        odom.twist.covariance = [1e-3, 0, 0, 0, 0, 0,
+                                 0, 1e-3, 0, 0, 0, 0,
+                                 0, 0, 1e6,  0, 0, 0,
+                                 0, 0, 0, 1e6,  0, 0,
+                                 0, 0, 0, 0, 1e6,  0,
+                                 0, 0, 0, 0, 0, 1e-3]
         self.odomPub.publish(odom)
 
         #向电机发送控制命令,当前电机为运动状态,发送停止控制命令