机器人精确移动包

move_near

之前有写过, 将ROS官方的move_basic包改写成了python形式, 同时将它写成了一个完整的action接口

最近测试时发现了问题, odom的数据波动可能会导致机器人陷入正反馈从而一直移动

具体表现为:

1. 机器人移动精度设置为0.005 [m]

2. 机器人在移动到接近0.005的位置, odom发生微小的跳变

   本来distRemaining应该是从 1 降到 0.5, 降到 0.006, 然后小于0.005, 机器人停住, 但是里程计波动, 使得distRemaining变成-0.006, 此时机器人还要继续后退, 就会导致distRemaining持续增大, 机器人无法停止

修改

​ 将计算机器人移动距离的distRemaining修改为累加制, 通过odom的逐差来减小odom的累进误差

结果

​ 机器人移动精度可以达到0.0005 [m], 甚至还能降, 但是已经超出了需求, 如果odom更好, 应该能达到更好的效果

调用

# 填充需要前往的位置, 在本例中使用的是base_link, 让机器人相对自身运动
$ rostopic pub /move_near/goal move_base_msgs/MoveBaseActionGoal 
1
2

注意事项

​ 在机器人移动过程中没有避障! 没有避障! 这不是move_base的接口! 不会调用costmap, 无避障操作!

#! /usr/bin/env python3

import rospy
import actionlib
from sensor_msgs.msg import Imu
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Pose, Twist, PoseStamped
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal

import math
import tf2_ros
from tf.transformations import euler_from_quaternion, quaternion_from_euler

class MoveNear(object):
    def __init__(self, name):
        
        self.now_imu = Imu()
        self.now_odom = Odometry()
        self.current_goal = PoseStamped()
        rospy.Subscriber("/imu", Imu, self.imu_cb)
        rospy.Subscriber("/odom", Odometry, self.odom_cb)
        self.cmd_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
        self.current_goal_pub = rospy.Publisher('current_goal', PoseStamped, queue_size=1)

        self.minAngularVelocity = rospy.get_param("~min_angular_velocity",0.02)
        self.maxAngularVelocity = rospy.get_param("~max_angular_velocity",0.2)
        self.angularAcceleration = rospy.get_param("~angular_acceleration",0.2)
        self.angularTolerance = rospy.get_param("~angularTolerance",0.01)
        self.minSpeedDistance = rospy.get_param("~minSpeedDistance", 0.03)

        self.minLinearVelocity = rospy.get_param("~min_linear_velocity",0.01)
        self.maxLinearVelocity = rospy.get_param("~max_linear_velocity",0.2)
        self.linearAcceleration = rospy.get_param("~linear_acceleration",0.2)
        self.linearTolerance = rospy.get_param("~linearTolerance",0.0005)
        self.minSpeedDistance = rospy.get_param("~minSpeedDistance",0.05)
        
        self._action_name = "move_near"
        self._as = actionlib.SimpleActionServer(self._action_name, MoveBaseAction, execute_cb=self.execute_cb, auto_start = False)
        self._as.start()

        self.initialPose = {'x':0.0, 'y':0.0, 'yaw':0.0}
        self.goalPose = {'x':0.0, 'y':0.0, 'yaw':0.0}
        self.oscillation = 0
        self.prevAngleRemaining = 0

    def imu_cb(self, msg):
        self.now_imu = msg

    def odom_cb(self, msg):
        self.now_odom = msg

    def normalizeAngle(self,angle):
        if angle < -math.pi:
            angle += 2* math.pi
        if angle > math.pi:
            angle -= 2*math.pi
        return angle

    def rad2deg(self,rad):
      return rad * 180 / math.pi

    def sign(self,n):
      if n < 0:
        return -1
      else:
        return 1

    def getCurrentYaw(self):
      orientation_list = [
        self.now_imu.orientation.x,
        self.now_imu.orientation.y,
        self.now_imu.orientation.z,
        self.now_imu.orientation.w]
      (_,_,current_yaw) = euler_from_quaternion(orientation_list)
      return current_yaw

    def rotate(self,yaw):
      rospy.loginfo("Requested rotation: {} degrees".format(self.rad2deg(yaw)))
      r = rospy.Rate(20)
      initial_yaw = self.getCurrentYaw()
      done = False

      while(not done and not rospy.is_shutdown()):
        rotated_yaw = self.getCurrentYaw() - initial_yaw
        angleRemaining = yaw - rotated_yaw
        angleRemaining = self.normalizeAngle(angleRemaining)
        rospy.logdebug("angleRemaining: {} degrees".format(self.rad2deg(angleRemaining)))
        vel = Twist()
        speed = max(self.minAngularVelocity,
                    min(self.maxAngularVelocity,
                        math.sqrt(max(2.0 * self.angularAcceleration *(abs(angleRemaining) - self.angularTolerance),0))))
        if angleRemaining < 0:
          vel.angular.z = -speed
        else:
          vel.angular.z = speed
        if (abs(angleRemaining) < self.angularTolerance):
          vel.angular.z = 0
          done = True
          r.sleep()
          rotated_yaw = self.getCurrentYaw() - initial_yaw
          angleRemaining = yaw - rotated_yaw
          rospy.loginfo("Rotate finished! error: {} degrees".format(self.rad2deg(angleRemaining)))
        self.cmd_pub.publish(vel)
        r.sleep()

      return True

    def moveLinear(self,dist):
      done = False
      r = rospy.Rate(20)
      initial_odom = self.now_odom
      distRemaining = dist
      while(not done and not rospy.is_shutdown()):
        travelledDist = math.hypot(self.now_odom.pose.pose.position.x - initial_odom.pose.pose.position.x,
                                   self.now_odom.pose.pose.position.y - initial_odom.pose.pose.position.y)
        # 保持了之前的命名, 在这里更新odom的值
        initial_odom = self.now_odom
        rospy.logdebug("travelledDist: {}".format(travelledDist))
        # for speed direction judgement
        if dist <= 0:
          distRemaining += travelledDist
          dist += travelledDist
        else:
          distRemaining -= travelledDist
          dist -= travelledDist
        rospy.logdebug("distRemaining: {}".format(distRemaining))
        vel = Twist()
        speed = max(self.minLinearVelocity, min(self.maxLinearVelocity, 2.5* abs(distRemaining)))

        if abs(distRemaining) < self.linearTolerance:
          speed = 0
          done = True
          rospy.loginfo("Linear movement finished! error: {} meters".format(distRemaining))
          rospy.loginfo("finished, breaking!")
          break
		# 在即将到达目的地时用最小速度跑, 提高精度
        if abs(distRemaining) < self.minSpeedDistance:
           rospy.loginfo_once("disRemaining is less than minSpeedDistance, slow down!")
           speed = self.minLinearVelocity
        # 这里可以控制机器人即使移动超过了距离, 则将速度反向
        # 避免之前移动越界导致的正反馈, 避免越走离目的地越远的行为
        if distRemaining < 0 :
          speed = -speed

        vel.linear.x = speed

        try:
          self.cmd_pub.publish(vel)
        except Exception as e:
          rospy.logerr("Error while publishing: {}".format(e))
        r.sleep()

      return True


    def execute_cb(self, goal):
        success = True
        behind = False
        self.current_goal_pub.publish(goal.target_pose)

        orientation_list = [
        goal.target_pose.pose.orientation.x,
        goal.target_pose.pose.orientation.y,
        goal.target_pose.pose.orientation.z,
        goal.target_pose.pose.orientation.w]
        (_,_,self.goalPose['yaw']) = euler_from_quaternion (orientation_list)

        face2goalYaw = math.atan2(goal.target_pose.pose.position.y, goal.target_pose.pose.position.x)
        
        # Check if the goal is behind the robot
        if face2goalYaw > math.pi/2 or face2goalYaw < -math.pi/2:
            behind = True
            face2goalYaw = self.normalizeAngle(face2goalYaw + math.pi)
            # face2goalYaw = self.normalizeAngle(face2goalYaw)

        # face to goal
        if self.rotate(face2goalYaw):
          pass
        else:
          rospy.logwarn("Trun to goal failed!")

        # Move to goal
        dist2goal = math.hypot(goal.target_pose.pose.position.x, goal.target_pose.pose.position.y)
        
        # if the goal is behind the robot, move backward
        if behind:
          dist2goal = -dist2goal
        else:
          dist2goal = dist2goal

        if self.moveLinear(dist2goal):
          pass
        else:
          success = False
          rospy.logwarn("Move to goal failed!")

        # Turn to  goal yaw
        relative_yaw = self.goalPose['yaw'] - face2goalYaw
        relative_yaw = self.normalizeAngle(relative_yaw)

        if self.rotate(relative_yaw):
          pass
        else:
          success = False
          rospy.loginfo("Trun to goal failed!")
        
        if success:
            result = PoseStamped()
            rospy.loginfo('%s: Succeeded' % self._action_name)
            self._as.set_succeeded(result)
        else:
          rospy.logerr("CHECK MOVE_NEAR!!!!")
        
if __name__ == '__main__':
    rospy.init_node('move_near')
    server = MoveNear(rospy.get_name())
    rospy.spin()
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217