机器人精确移动包

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 

注意事项

​ 在机器人移动过程中没有避障! 没有避障! 这不是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()