ROS move_basic基础移动功能的python实现

在一些小场景，可能需要机器人进行小范围的腾挪，因此在github上，以及ros官方给出了move_basic这个包，可以完成包括避障之内的，一些简单的移动。

但是！这个虽然是一个action的类型，却没有完整的反馈机制，也就是说，在他完成了这个动作之后，没有返回一个成功的消息，就无法用于，在一个过程中导航到目标点，收到反馈信息之后，再进行下一步，这么一个流程中。

因此，基于move_basic，我将其改写成了一个python的action，可以收到请求，并且给出反馈。最重要的是，这是python！不需要编译！太棒啦！

move_basic.py的文件放在下面，其他的因为是公司其他人开发的，因此并不能给出完整的包，不过有了这个文件，稍微完善一下应该是可以用的！

launch文件如下

<launch>

  <node pkg="move_near" type="move_basic.py" name="move_near">
    <rosparam command="load" file="$(find move_near)/param/move_basic.yaml"/>
  </node>

</launch>

总的流程是：先发一个目标点，包含 (x, y, 0) (0,0,yaw) 的信息，要求机器人以一个固定的位姿走到目标位置

机器人会先转向目标点，然后前进到目标点，最后转到要求的位姿，大概就这样，可能新人并不是很友好？属实不知道怎么描述了emmm

move_basic.py

#! /usr/bin/env python3

import rospy

import actionlib

import actionlib_tutorials.msg
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):
    # create messages that are used to publish feedback/result
    _feedback = actionlib_tutorials.msg.FibonacciFeedback()
    _result = actionlib_tutorials.msg.FibonacciResult()

    def __init__(self, name):
        rospy.Subscriber("/imu", Imu, self.imu_cb)
        self.now_imu = Imu()

        rospy.Subscriber("/odom", Odometry, self.odom_cb)
        self.now_odom = Odometry()

        self.debug_pub = rospy.Publisher('/debug_pub', Pose, queue_size=5)
        self.cmd_pub = rospy.Publisher('/mobile_base/commands/velocity', Twist, queue_size=5)
        
		# 这里要注意，再实际使用的时候，如果匀速转的时间过长，imu会失效！
        # 可能是选型的原因，因此可以考虑适当调大角速度和角加速度
        self.minAngularVelocity = rospy.get_param("/move_near/min_angular_velocity",0.02)
        self.maxAngularVelocity = rospy.get_param("/move_near/max_angular_velocity",0.05)
        self.angularAcceleration = rospy.get_param("/move_near/angular_acceleration",0.05)
        # 真的能实现哦！虽然只是里程计度数，似乎是能完成0.01个弧度的误差
        self.angularTolerance = rospy.get_param("/move_near/angularTolerance",0.01)

        self.minLinearVelocity = rospy.get_param("/move_near/min_linear_velocity",0.05)
        self.maxLinearVelocity = rospy.get_param("/move_near/max_linear_velocity",0.1)
        self.linearAcceleration = rospy.get_param("/move_near/linear_acceleration",0.05)
        self.linearTolerance = rospy.get_param("/move_near/linearTolerance",0.01)
        
        self._action_name = "move_near_action"
        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(50)

      currentYaw = self.getCurrentYaw()

      requestedYaw = currentYaw + yaw
      requestedYaw = self.normalizeAngle(requestedYaw)
      done = False

      while(not done and not rospy.is_shutdown()):
        currentYaw = self.getCurrentYaw()
        angleRemaining = requestedYaw - currentYaw
        angleRemaining = self.normalizeAngle(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
          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
      rospy.Rate(50)
      originX = self.initialPose['x']
      originY = self.initialPose['y']
      print("initialPose: ",self.initialPose)

      currentX = self.now_odom.pose.pose.position.x
      currentY = self.now_odom.pose.pose.position.y
      print("currentPose x: ",currentX, "y: ",currentY)

      travelledDist = abs(math.hypot(currentX,currentY)  - math.hypot(originX,originY))
      while(not done and not rospy.is_shutdown()):
        currentX = self.now_odom.pose.pose.position.x
        currentY = self.now_odom.pose.pose.position.y
        travelledDist = math.hypot(currentX - originX ,currentY-originY)
        distRemaining = math.hypot(self.goalPose['x'] - originX,self.goalPose['y']-originY) - travelledDist
        vel = Twist()
        speed = 0.1
        if abs(travelledDist>abs(dist) - self.linearTolerance):
          speed = 0
          done = True
          rospy.loginfo("Linear movement finished! error: {} meters".format(distRemaining))
        
        if dist < 0:
          speed = -speed

        vel.linear.x = speed
        
        try:
          self.cmd_pub.publish(vel)
        except Exception as e:
          rospy.logerr("Error while publishing: {}".format(e))
          
      return True


    def execute_cb(self, goal):
        r = rospy.Rate(50)
        success = True
        behind = False
        self.initialPose['x'] = self.now_odom.pose.pose.position.x
        self.initialPose['y'] = self.now_odom.pose.pose.position.y
        self.initialPose['yaw'] = self.getCurrentYaw()

        self.goalPose['x'] = goal.target_pose.pose.position.x + self.initialPose['x']
        self.goalPose['y'] = goal.target_pose.pose.position.y + self.initialPose['y']

        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)

        yaw_value = self.goalPose['yaw']
        goal_msg = "Goal pose: x = {}, y = {}, yaw = {}, degrees".format(self.goalPose['x'], self.goalPose['y'], self.rad2deg(yaw_value))
        rospy.loginfo(goal_msg)

        face2goalYaw = math.atan2(self.goalPose['y']-self.initialPose['y'],
                                  self.goalPose['x']-self.initialPose['x'])
        
        # 检查目标点是不是在机器人后面
        if face2goalYaw > math.pi/2 or face2goalYaw < -math.pi/2:
            behind = True
            face2goalYaw = self.normalizeAngle(face2goalYaw + math.pi)

        # 转向目标点
        if self.rotate(face2goalYaw):
          pass
        else:
          rospy.logwarn("Trun to goal failed!")

        # 移动到目标点
        dist2goal = math.hypot(self.initialPose['x']-self.goalPose['x'],self.initialPose['y']-self.goalPose['y'])
        
        # 如果目标点在机器人后面，就倒过去
        if behind:
          dist2goal = -dist2goal
        else:
          dist2goal = dist2goal

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

        # 转向目标角度
        goal_yaw_value = self.goalPose['yaw']
        initial_yaw_value = self.initialPose['yaw']
        target_yaw_in_world_frame = self.normalizeAngle(initial_yaw_value + goal_yaw_value)

        current_yaw_after_movement = self.getCurrentYaw()
        finalYaw = self.normalizeAngle(target_yaw_in_world_frame - current_yaw_after_movement)
        finalYaw = self.normalizeAngle(finalYaw)

        if self.rotate(finalYaw):
          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()