更新robot_pose_ekf发布的坐标变化从odom_combined到odom,这样就不用在arduino代码处发布轮式里程计的odom变换了,同步更新amcl,local_costmap中的odom_combined为odom

src/robot_bringup/launch/odom_ekf.py

@@ -1,10 +1,5 @@
 #!/usr/bin/env python
 
-"""
-    Republish the /robot_pose_ekf/odom_combined topic which is of type
-    geometry_msgs/PoseWithCovarianceStamped as an equivalent message of
-    type nav_msgs/Odometry so we can view it in RViz.
-"""
 import rospy
 from nav_msgs.msg import Odometry
 from geometry_msgs.msg import PoseWithCovarianceStamped
@@ -14,20 +9,20 @@ class OdomEKF():
         rospy.init_node('odom_ekf_node', anonymous=False)
 
         # Publisher of type nav_msgs/Odometry
-        self.ekf_pub = rospy.Publisher('output', Odometry, queue_size=20)
+        self.ekf_pub = rospy.Publisher('output_topic', Odometry, queue_size=20)
 
         # Wait for the /odom_combined topic to become available
-        rospy.wait_for_message('input', PoseWithCovarianceStamped)
+        rospy.wait_for_message('input_topic', PoseWithCovarianceStamped)
 
         # Subscribe to the /odom_combined topic
-        rospy.Subscriber('input', PoseWithCovarianceStamped, self.pub_ekf_odom)
-        rospy.loginfo("Publishing combined odometry on /odom_ekf")
+        rospy.Subscriber('input_topic', PoseWithCovarianceStamped, self.pub_ekf_odom)
+        rospy.loginfo("Publishing combined odometry on /odom_ekf topic...")
 
     def pub_ekf_odom(self, msg):
         odom = Odometry()
         odom.header = msg.header
-        odom.header.frame_id = '/odom_combined'
-        odom.child_frame_id = 'base_footprint'
+        odom.header.frame_id = '/odom'
+        odom.child_frame_id  = 'base_footprint'
         odom.pose = msg.pose
 
         self.ekf_pub.publish(odom)
@@ -38,4 +33,3 @@ if __name__ == '__main__':
         rospy.spin()
     except:
         pass
-

src/robot_bringup/launch/robot_bringup.launch

@@ -4,13 +4,14 @@
     首先就需要启动机器人的urdf文件,这样就可以在rviz中可视化显示机器人模型.然后
     启动下位机arduino的程序,上下位机建立连接,这样上位机就可以发送控制命令.接下来
     启动IMU模块,开始发布姿态信息.接下来就开始进行信息融合,使用imu信息和轮式里程计
-    这样得出的里程计更为准确.
+    这样得出的里程计更为准确,同时需要注意将融合后的tf更新为odom.
   Author: corvin
   History:
     20200716:init this file.
     20210602:使用串口获取imu模块的数据.
     20220902:增加启动oled显示屏的launch.
-    20220928:将robot_pose_ekf的数据融合发布频率从10hz提高为20hz.
+    20220928:将robot_pose_ekf的数据融合发布频率从10hz提高为20hz,同时将输出的
+      tf变换从odom_combined更新为odom,这样就不用在arduino轮式里程计那边发布odom的tf了.
 -->
 <launch>
     <!-- (1) startup panda robot urdf description -->
@@ -27,20 +28,20 @@
 
     <!-- (5) startup robot_pose_ekf node -->
     <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf" output="screen">
-        <param name="output_frame" value="odom_combined"/>
-        <param name="freq" value="20.0"/>
-        <param name="sensor_timeout" value="1.2"/>
-        <param name="odom_used" value="true"/>
-        <param name="imu_used"  value="true"/>
-        <param name="vo_used"   value="false"/>
-        <param name="debug"     value="false"/>
-        <param name="self_diagnose" value="false"/>
+        <param name="output_frame" value="odom"/>
+        <param name="freq"         value="20.0"/>
+        <param name="odom_used"    value="true"/>
+        <param name="imu_used"     value="true"/>
+        <param name="vo_used"      value="false"/>
+        <param name="debug"        value="false"/>
+        <param name="sensor_timeout" value="1.0"/>
+        <param name="self_diagnose"  value="false"/>
     </node>
 
-    <!-- (6) /odom_combined view in rviz,dwa_local_planner subscribe /odom_ekf topic -->
+    <!-- (6) dwa_local_planner subscribe /odom_ekf topic -->
     <node pkg="robot_bringup" type="odom_ekf.py" name="odom_ekf_node" output="screen">
-        <remap from="input"  to="/robot_pose_ekf/odom_combined" />
-        <remap from="output" to="/odom_ekf" />
+        <remap from="input_topic"  to="/robot_pose_ekf/odom_combined" />
+        <remap from="output_topic" to="/odom_ekf" />
     </node>
 
     <!-- (7) startup get battery oled show -->

src/robot_navigation/config/dwa_local_planner_params.yaml

@@ -26,8 +26,8 @@ DWAPlannerROS:
   trans_stopped_vel: 0.1
 
   max_vel_theta: 2.0
-  min_vel_theta: 0.4
-  theta_stopped_vel: 0.4
+  min_vel_theta: 0.5
+  theta_stopped_vel: 0.5
 
   acc_lim_x: 0.7
   acc_lim_y: 0
@@ -46,7 +46,7 @@ DWAPlannerROS:
   vth_samples: 20
 
 # Trajectory Scoring Parameters
-  path_distance_bias: 48.0
+  path_distance_bias: 32.0
   goal_distance_bias: 18.0
   occdist_scale: 0.02
   forward_point_distance: 0.2

src/robot_navigation/config/local_costmap_params.yaml

@@ -1,5 +1,5 @@
 #Description: 本地代价地图配置文件，各参数意义如下：
-#  global_frame:指定本地代价地图参考系,使用融合后frame:odom_combined;
+#  global_frame:指定本地代价地图参考系,使用融合后frame:odom;
 #  robot_base_frame:指定机器人的base_frame:base_footprint;
 #  update_frequency:指定local代价地图更新频率
 #  publish_frequency:代价地图发布可视化信息的频率
@@ -14,9 +14,10 @@
 #  20200413:更新本地代价地图配置参数.
 #  20200927:添加plugins参数,配置障碍物层和膨胀层.
 #  20220927:更新width和height为2.0,以机器人为中心,前1m后1m,左1m右1m.
+#    robot_pose_ekf输出的tf变换已经从odom_combined更新为odom,这里也同步更新.
 
 local_costmap:
-    global_frame: odom_combined
+    global_frame: odom
     robot_base_frame: base_footprint
 
     update_frequency: 4.0

src/robot_navigation/launch/amcl_dwa.launch

@@ -10,6 +10,8 @@
     20200326:init this file.
     20200804:将odom_mode_type改为omni-corrected,对应odom_alpha参数也更新.
     20220810:在启动amcl算法进行自动导航时,先将robot_bringup.launch启动.
+    20220929:将amcl节点参数odom_frame_id从odom_combined更新为odom,因为已经将robot_pose_ekf
+      发布的odom_combined坐标变换更新为odom了.
 -->
 <launch>
   <!--(1) startup robot bringup -->
@@ -30,7 +32,7 @@
 
     <param name="base_frame_id"   value="base_footprint"/>
     <param name="global_frame_id" value="map"/>
-    <param name="odom_frame_id"   value="odom_combined"/>
+    <param name="odom_frame_id"   value="odom"/>
     <param name="tf_broadcast"    value="true"/>
 
     <param name="use_map_topic"  value="false"/>

src/robot_navigation/launch/move_base.launch

@@ -35,7 +35,7 @@
 
     <!-- 加载move_base使用的全局路径规划和局部路径规划算法及对应的参数  -->
     <rosparam file="$(find robot_navigation)/config/base_global_planner_params.yaml" command="load"/>
-    <rosparam file="$(find robot_navigation)/config/dwa_local_planner_params.yaml" command="load"/>
+    <rosparam file="$(find robot_navigation)/config/dwa_local_planner_params.yaml"   command="load"/>
     <remap from="odom" to="$(arg odom_topic)"/>
   </node>
 </launch>

src/ros_arduino_bridge/ros_arduino_python/nodes/arduino_node.py

@@ -23,8 +23,8 @@ class ArduinoROS():
 
         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.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)
 
@@ -106,7 +106,7 @@ class ArduinoROS():
         #开始主循环,控制底盘转动和发布各传感器信息
         while not rospy.is_shutdown():
             mutex.acquire()
-            self.myBaseController.poll()
+            self.myBaseController.main_poll()
             mutex.release()
 
             for sensor in self.mySensors:

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

@@ -11,6 +11,8 @@
     20210913:为增加两轮平衡车模式优化代码.
     20220927:将电机编码器计数的参数获取改为一个参数motor_encoder_cnt,直接为
       CHANGE模式四倍频的总计数1320个,不使用减速比和编码器分辨率参数.
+    20220929:删除在init函数中,发送清除编码器计数值操作,因为上下位机建立通信时,
+      会自动的将下位机程序初始化,编码器计数值自然为0.
 """
 import os,rospy,roslib
 import dynamic_reconfigure.client
@@ -19,7 +21,6 @@ from math import sin, cos, pi
 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 ros_arduino_python.balance_mode import BalanceMode
 
 
@@ -67,9 +68,6 @@ class BaseController:
         self.accel_limit = rospy.get_param('~motor_accel_limit', 0.1)
         self.max_accel   = self.accel_limit*self.ticks_per_meter
 
-        now = rospy.Time.now()
-        self.old_time = now  # time for determining dx/dy
-
         #用于保存读取的电机编码器计数值
         self.enc_A = 0
         self.enc_B = 0
@@ -83,7 +81,9 @@ class BaseController:
 
         self.v_des_AWheel = 0  #计算出来应该发送的脉冲数
         self.v_des_BWheel = 0
-
+        
+        now = rospy.Time.now()
+        self.old_time = now
         self.last_cmd_time = now
 
         #设置订阅控制移动话题的回调函数
@@ -93,15 +93,8 @@ class BaseController:
         if self.isBalance:
             self.balanceControl = BalanceMode()
 
-        #初始化时,将电机编码器计数值清零
-        if self.use_serial:
-            self.arduino.reset_encoders()
-        else:
-            self.arduino.i2c_reset_encoders()
-
-        #配置用于里程计信息发布
+        #配置用于里程计信息发布和里程计坐标变换
         self.odomPub = rospy.Publisher(self.odom_name, Odometry, queue_size=5)
-        self.odomBroadcaster = TransformBroadcaster()
 
         #增加用于调试PID参数的发布者
         if self.debugPID:
@@ -117,7 +110,6 @@ class BaseController:
         self.AWheel_Kd = pid_params['AWheel_Kd']
         self.AWheel_Ki = pid_params['AWheel_Ki']
         self.AWheel_Ko = pid_params['AWheel_Ko']
-
         self.BWheel_Kp = pid_params['BWheel_Kp']
         self.BWheel_Kd = pid_params['BWheel_Kd']
         self.BWheel_Ki = pid_params['BWheel_Ki']
@@ -144,8 +136,8 @@ class BaseController:
 
         return int(pwmA), int(pwmB)
 
-    #底盘运动控制的主循环,执行频率由arduino_node中的频率决定,默认100hz
-    def poll(self):
+    #底盘运动控制的主循环,执行频率由arduino_node中的频率决定
+    def main_poll(self):
         #发送调试pid参数的数据
         self.send_debug_pid()
 
@@ -171,17 +163,19 @@ class BaseController:
         if aWheel_enc == 0 and bWheel_enc == 0:
             self.enc_A = 0
             self.enc_B = 0
+            encoder_diff_A = 0
+            encoder_diff_B = 0
             if self.isBalance:
                 self.balanceControl.encoder_integral = 0
                 self.balanceControl.encoder_lowout = 0
+        else:
+            #计算编码器计数差值,当前值减去上次记录的计数值
+            encoder_diff_A = aWheel_enc - self.enc_A
+            encoder_diff_B = bWheel_enc - self.enc_B
 
-        #计算编码器计数差值,当前值减去上次记录的计数值
-        encoder_diff_A = aWheel_enc - self.enc_A
-        encoder_diff_B = bWheel_enc - self.enc_B
-
-        #保存当前编码器计数值,用于下次计算
-        self.enc_A = aWheel_enc
-        self.enc_B = bWheel_enc
+            #保存当前读取到的最新编码器计数值,用于下次计算
+            self.enc_A = aWheel_enc
+            self.enc_B = bWheel_enc
 
         #是否开启平衡车模式,在控制周期内计算控制电机的pwm值
         if self.isBalance:
@@ -317,7 +311,7 @@ class BaseController:
         self.v_des_BWheel = int(vB * self.ticks_per_meter / self.PID_rate)
         #rospy.loginfo("cmdCallback(): A v_des:"+str(self.v_des_AWheel)+",B v_des:"+str(self.v_des_BWheel))
 
-    #用于调试电机PID参数
+    #通过topic发布用于调试电机的各项PID参数
     def send_debug_pid(self):
         if self.debugPID:
             try: