网站开发的工作经验要求,地方网站商城怎么做,wordpress超级密码破解,建材销售网站手机模板move_near
之前有写过, 将ROS官方的move_basic包改写成了python形式, 同时将它写成了一个完整的action接口
最近测试时发现了问题, odom的数据波动可能会导致机器人陷入正反馈从而一直移动
具体表现为: 机器人移动精度设置为0.005 [m] 机器人在移动到接近0.005的位置, odom…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更好, 应该能达到更好的效果
调用
# 填充需要前往的位置, 在本例中使用的是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_size5)self.current_goal_pub rospy.Publisher(current_goal, PoseStamped, queue_size1)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_nearself._as actionlib.SimpleActionServer(self._action_name, MoveBaseAction, execute_cbself.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()
