move_near
之前有写过, 将ROS官方的move_basic包改写成了python形式, 同时将它写成了一个完整的action接口
最近测试时发现了问题, odom的数据波动可能会导致机器人陷入正反馈从而一直移动
具体表现为:
-
机器人移动精度设置为0.005 m
-
机器人在移动到接近0.005的位置, odom发生微小的跳变
本来distRemaining应该是从 1 降到 0.5, 降到 0.006, 然后小于0.005, 机器人停住, 但是里程计波动, 使得distRemaining变成-0.006, 此时机器人还要继续后退, 就会导致distRemaining持续增大, 机器人无法停止
修改
将计算机器人移动距离的distRemaining修改为累加制, 通过odom的逐差来减小odom的累进误差
结果
机器人移动精度可以达到0.0005 m, 甚至还能降, 但是已经超出了需求, 如果odom更好, 应该能达到更好的效果
调用
shell
# 填充需要前往的位置, 在本例中使用的是base_link, 让机器人相对自身运动
$ rostopic pub /move_near/goal move_base_msgs/MoveBaseActionGoal 注意事项
在机器人移动过程中没有避障! 没有避障! 这不是move_base的接口! 不会调用costmap, 无避障操作!
python
#! /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()