增加可以配置电机未收到移动命令后停止转动的延迟参数,dwa_local_planner中stop_time_buffer优化为0.2

src/robot_navigation/config/dwa_local_planner_params.yaml

@@ -51,7 +51,7 @@ DWAPlannerROS:
   goal_distance_bias: 18.0
   occdist_scale: 0.02
   forward_point_distance: 0.2
-  stop_time_buffer: 0.6
+  stop_time_buffer: 0.2
   scaling_speed: 0.25
   max_scaling_factor: 0.2
 

src/ros_arduino_bridge/ros_arduino_python/cfg/my_arduino_params.yaml → src/ros_arduino_bridge/ros_arduino_python/cfg/arduino_node_params.yaml

@@ -31,13 +31,15 @@ motor_accel_limit: 0.1   #配置电机转动时脉冲加速度值
 linear_scale_correction: 1.00
 angular_scale_correction: 1.00
 
+default_motor_stop_delay: 100   #支撑轮模式下电机没有收到命令后停止转动的延时,单位ms
+
 #是否开启平衡车模式,直立pd控制
 balance_mode: False    #开启时设置为True
 balance_main_rate: 100 #在平衡小车模式下主循环的频率
 middle_val: 4.24       #小车物理平衡时的俯仰角度
-motor_stop_delay: 11   #单位ms
 check_down_pitch: 18   #当倾斜角超过该度数,认为小车已经倒下,停止电机转动
 pitch_tolerance: 0.2   #当平衡模式时,该角度倾斜范围内不进行调平
+balance_motor_stop_delay: 11   #单位ms
 
 balance_kp: 10.2   #直立环kp参数,25最大值,调试值17,17*0.6=10.2为正常值
 balance_kd: 0.65   #直立环kd参数,2.0最大值,调试值1.2,1.2*0.6=0.65
@@ -62,4 +64,3 @@ sensors: {
   GP2Y0A41: {pin: 0, type: GP2Y0A41, rate: 5},
   batPercent: {pin: 0, type: BAT_PERCENT, rate: 0.1},
 }
-

src/ros_arduino_bridge/ros_arduino_python/launch/arduino.launch

@@ -8,7 +8,7 @@
     20210602:修改加载yaml配置文件时从cfg目录下加载.
 -->
 <launch>
-   <node name="mobilebase_arduino" pkg="ros_arduino_python" type="arduino_node.py" output="screen" required="true">
-      <rosparam file="$(find ros_arduino_python)/cfg/my_arduino_params.yaml" command="load" />
+   <node pkg="ros_arduino_python" type="arduino_node.py" name="mobilebase_arduino" output="screen" required="true">
+      <rosparam file="$(find ros_arduino_python)/cfg/arduino_node_params.yaml" command="load" />
    </node>
 </launch>

src/ros_arduino_bridge/ros_arduino_python/nodes/arduino_node.py

@@ -14,26 +14,29 @@ from ros_arduino_msgs.srv import *
 from geometry_msgs.msg import Twist
 from serial.serialutil import SerialException
 from ros_arduino_python.arduino_sensors import *
-from ros_arduino_python.arduino_driver import Arduino
+from ros_arduino_python.arduino_driver import ArduinoDriver
 from ros_arduino_python.base_controller import BaseController
 
 class ArduinoROS():
     def __init__(self):
         rospy.init_node('mobilebase_arduino_node', log_level=rospy.INFO)
 
-        self.is_serial        = rospy.get_param("~is_use_serial", True)
-        self.serial_port      = rospy.get_param("~serial_port", "/dev/ttyACM0")
-        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)
-        self.motor_stop_delay = rospy.get_param("~motor_stop_delay", 11)
+        self.is_serial    = rospy.get_param("~is_use_serial", True)
+        self.serial_port  = rospy.get_param("~serial_port", "/dev/ttyACM0")
+        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)
 
         #根据参数中配置的频率来设置主循环频率,平衡小车模式为100hz,默认支撑轮模式20hz
+        #获取电机没有收到新的移动命令后延迟多长时间停止转动的参数,单位ms
         if self.balance_mode:
             self.rate = int(rospy.get_param("~balance_main_rate", 100))
+            self.motor_stop_delay = rospy.get_param("~balance_motor_stop_delay", 11);
         else:
             self.rate = int(rospy.get_param("~default_main_rate", 20))
+            self.motor_stop_delay = rospy.get_param("~default_motor_stop_delay", 100)
 
+        #配置主循环的频率
         loop_rate = rospy.Rate(self.rate)
 
         #初始化控制移动的Twist发布者,用于断开连接时停止小车移动
@@ -63,14 +66,14 @@ class ArduinoROS():
         #当节点退出的时候执行的回调函数
         rospy.on_shutdown(self.shutdown)
 
-        # Initialize the controlller
-        self.controller = Arduino(self.is_serial, self.serial_port)
+        #初始化ArduinoDriver对象,开始建立通信
+        self.controller = ArduinoDriver(self.is_serial, self.serial_port)
 
         #与下位机Arduino建立连接
         self.controller.connect()
 
-        #初始化一个线程锁对象
-        mutex = thread.allocate_lock()
+        #配置电机停止转动的延时,延迟参数从配置文件中读取
+        self.controller.update_motor_stop_delay(self.motor_stop_delay)
 
         #获取传感器列表
         self.mySensors = list()
@@ -97,11 +100,8 @@ class ArduinoROS():
         #初始化底盘控制器
         self.myBaseController = BaseController(self.is_serial,self.controller,self.base_frame,self.cmd_topic,self.balance_mode)
 
-        #配置电机停止转动的延时,从配置文件中读取延迟参数
-        if self.balance_mode:
-            self.controller.update_motor_stop_delay(self.motor_stop_delay)
-        else:
-            self.controller.update_motor_stop_delay(100)
+        #初始化一个线程锁对象
+        mutex = thread.allocate_lock()
 
         #开始主循环,控制底盘转动和发布各传感器信息
         while not rospy.is_shutdown():

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

@@ -17,7 +17,7 @@ import thread, smbus, rospy, time, os
 from serial.serialutil import SerialException
 
 
-class Arduino:
+class ArduinoDriver:
     def __init__(self, is_use_serial, serial_port):
         self.is_use_serial = is_use_serial  #与下位机arduino的通信方式是否使用串口还是IIC
         self.serial_port   = serial_port
@@ -34,6 +34,7 @@ class Arduino:
     def connect(self):
         if self.is_use_serial:  #使用串口建立连接
             self.serial_connect()
+            self.get_firmware_ver()
         else:
             self.i2c_bus = smbus.SMBus(1)
             self.i2c_connect()
@@ -89,8 +90,10 @@ class Arduino:
         else:
             self.i2c_bus.close()
 
-    def send(self, cmd):
-        self.serial_port.write(cmd + '\r')
+    #获取下位机固件版本号
+    def get_firmware_ver(self):
+        version = self.execute('v')
+        rospy.loginfo("Get arduino firmware version:" + str(version))
 
     def recv(self, timeout=0.5):
         timeout = min(timeout, self.timeout)
@@ -125,28 +128,15 @@ class Arduino:
             return map(float, values)
         except:
             return []
-
+    
+    #执行单独的字符命令,然后获取到返回值
     def execute(self, cmd):
         self.mutex.acquire()
 
         try:
             self.serial_port.flushInput()
-        except:
-            pass
-
-        ntries   = 1
-        attempts = 0
-        try:
             self.serial_port.write(cmd + '\r')
             value = self.recv(self.timeout)
-            while attempts < ntries and (value == '' or value == 'Invalid Command' or value == None):
-                try:
-                    self.serial_port.flushInput()
-                    self.serial_port.write(cmd + '\r')
-                    value = self.recv(self.timeout)
-                except:
-                    rospy.logerr("Exception executing command: " + cmd)
-                attempts += 1
         except:
             self.mutex.release()
             rospy.logerr("Exception executing command: " + cmd)