机器人精确移动包

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 python3import 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, MoveBaseGoalimport math
import tf2_ros
from tf.transformations import euler_from_quaternion, quaternion_from_eulerclass 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 = 0self.prevAngleRemaining = 0def imu_cb(self, msg):self.now_imu = msgdef odom_cb(self, msg):self.now_odom = msgdef normalizeAngle(self,angle):if angle < -math.pi:angle += 2* math.piif angle > math.pi:angle -= 2*math.pireturn angledef rad2deg(self,rad):return rad * 180 / math.pidef sign(self,n):if n < 0:return -1else:return 1def 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_yawdef rotate(self,yaw):rospy.loginfo("Requested rotation: {} degrees".format(self.rad2deg(yaw)))r = rospy.Rate(20)initial_yaw = self.getCurrentYaw()done = Falsewhile(not done and not rospy.is_shutdown()):rotated_yaw = self.getCurrentYaw() - initial_yawangleRemaining = yaw - rotated_yawangleRemaining = 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 = -speedelse:vel.angular.z = speedif (abs(angleRemaining) < self.angularTolerance):vel.angular.z = 0done = Truer.sleep()rotated_yaw = self.getCurrentYaw() - initial_yawangleRemaining = yaw - rotated_yawrospy.loginfo("Rotate finished! error: {} degrees".format(self.rad2deg(angleRemaining)))self.cmd_pub.publish(vel)r.sleep()return Truedef moveLinear(self,dist):done = Falser = rospy.Rate(20)initial_odom = self.now_odomdistRemaining = distwhile(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_odomrospy.logdebug("travelledDist: {}".format(travelledDist))# for speed direction judgementif dist <= 0:distRemaining += travelledDistdist += travelledDistelse:distRemaining -= travelledDistdist -= travelledDistrospy.logdebug("distRemaining: {}".format(distRemaining))vel = Twist()speed = max(self.minLinearVelocity, min(self.maxLinearVelocity, 2.5* abs(distRemaining)))if abs(distRemaining) < self.linearTolerance:speed = 0done = Truerospy.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 = -speedvel.linear.x = speedtry:self.cmd_pub.publish(vel)except Exception as e:rospy.logerr("Error while publishing: {}".format(e))r.sleep()return Truedef execute_cb(self, goal):success = Truebehind = Falseself.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 robotif face2goalYaw > math.pi/2 or face2goalYaw < -math.pi/2:behind = Trueface2goalYaw = self.normalizeAngle(face2goalYaw + math.pi)# face2goalYaw = self.normalizeAngle(face2goalYaw)# face to goalif self.rotate(face2goalYaw):passelse:rospy.logwarn("Trun to goal failed!")# Move to goaldist2goal = math.hypot(goal.target_pose.pose.position.x, goal.target_pose.pose.position.y)# if the goal is behind the robot, move backwardif behind:dist2goal = -dist2goalelse:dist2goal = dist2goalif self.moveLinear(dist2goal):passelse:success = Falserospy.logwarn("Move to goal failed!")# Turn to  goal yawrelative_yaw = self.goalPose['yaw'] - face2goalYawrelative_yaw = self.normalizeAngle(relative_yaw)if self.rotate(relative_yaw):passelse:success = Falserospy.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()