def averageSpeed(path): Length=len(path.poses) finalpath=[] last=PathFilter.quat_to_angle(path.poses[0].pose.orientation) goalposition=[] for i in range(1,Length): GD=PathFilter.quat_to_angle(path.poses[i].pose.orientation) #if() last=GD
def averageSpeed(path): Length=len(path.poses) finalpath=[] last=PathFilter.quat_to_angle(path.poses[0].pose.orientation) goalposition=[] for i in range(1,Length): GD=PathFilter.quat_to_angle(path.poses[i].pose.orientation) #if() last=GD
def PathCallback(path): newpath=PathFilter.newPathFromAStar(path) mPath2.publish(newpath) #两次滤波对路径进行规整 newpath=PathFilter.Lvbo(newpath,0.5) newpath=PathFilter.Lvbo(newpath,0.8) mPath.publish(newpath) poses=newpath.poses POSES=poses[:] NewPath=True
def rotateToGoalDirection(pose,rot,final):
goalAngle=0.0
#获取当前位置
(position, rotation) = get_odom()
if(final!=True):
#获取机器人目标的位置 角度
GX=pose.position.x
GY=pose.position.y
CX=position.x
CY=position.y
#计算坐标差值
#计算两个目标点与当前点的角度差值
goalAngle=PathFilter.canculateAngle(GX,GY,CX,CY)
else:
goalAngle=PathFilter.quat_to_angle(rot)
goalAngleC=0.0
last_angleC=0.0
#将四元素转化为角度
#goalAngle=quat_to_angle(rot)
if(goalAngle<0.0):
goalAngleC=2*pi+goalAngle
else:
goalAngleC=goalAngle
#rospy.loginfo("Goal Angle is %f",goalAngleC)
# Track the last angle measured
last_angle = float(rotation)
# Track how far we have turned
if(rotation<0.0):
last_angleC=2*pi+rotation
else:
last_angleC=last_angle
#rospy.loginfo("curretn Angle is %f",rotation)
turn_angle=goalAngleC-last_angleC
move_cmd=Twist()
if (0<=turn_angle and turn_angle<=pi) or ((-2*pi)<=turn_angle and turn_angle<=(0-pi)):
move_cmd.angular.z = ROTATE_SPEED
else:
move_cmd.angular.z = 0-ROTATE_SPEED
#转到运动方向上
turn_angle=goalAngle-last_angle
#0.02 大约１．１４度
while abs(turn_angle)>0.05 and not rospy.is_shutdown():
cmd_vel.publish(move_cmd)
r.sleep()
(position, rotation) = get_odom()
turn_angle=goalAngle-rotation
move_cmd=Twist()
cmd_vel.publish(move_cmd)
def rotateToMoveDirection(pose):
#计算目标点与当前点的全局坐标系下的角度值
(position, rotation) = get_odom()
goalAngle = PathFilter.canculateAngle(pose.pose.position.x,
pose.pose.position.y, position.x,
position.y)
rotateToGoal(goalAngle)
def PathCallback(path):
global POSES
global NewPath
newpath = PathFilter.newPathFromAStar(path)
#两次滤波对路径进行规整
newpath = PathFilter.Lvbo(newpath, 0.5)
newpath = PathFilter.Lvbo(newpath, 0.8)
#mPath2.publish(newpath)
newpath = PathFilter.Lvbo(newpath, 1.0)
#newpath=PathFilter.Lvbo(newpath,1.6)
#最后的滤波 选择长距离点
newpath = PathFilter.ChooseMainPath(newpath)
mPath.publish(newpath)
poses = newpath.poses
POSES = poses[:]
NewPath = True
def rotateToMoveDirection(pose): goalAngle=0.0 #获取当前位置 (position, rotation) = get_odom() #计算目标点与当前点的全局坐标系下的角度值 goalAngle=PathFilter.canculateAngle(pose.position.x,pose.position.y,position.x,position.y) current_angle = float(rotation) #判断左转还是右转 turn_angle=normalize_angle(goalAngle-current_angle) ROTATE_ACC_SPEED=ACC_ANGULAR_Z if turn_angle<0.0: ROTATE_ACC_SPEED=0-ROTATE_ACC_SPEED #记录最初的旋转角 O_turn_angle=turn_angle move_cmd=Twist() #转到运动方向上 if abs(O_turn_angle)<MODE1_ANGLE: #模式１ 直接加速 然后减速 while abs(turn_angle)>0.02 and not rospy.is_shutdown(): cmd_vel.publish(move_cmd) r.sleep() (position, rotation) = get_odom() turn_angle=goalAngle-rotation if(abs(turn_angle)>(abs(O_turn_angle)/2.0)): move_cmd.angular.z+=ROTATE_ACC_SPEED/RATE else: move_cmd.angular.z-=ROTATE_ACC_SPEED/RATE else: #模式２ 直接加速 匀速 然后减速 while abs(turn_angle)>0.02 and not rospy.is_shutdown(): cmd_vel.publish(move_cmd) r.sleep() (position, rotation) = get_odom() turn_angle=goalAngle-rotation if(abs(turn_angle)>MODE1_ANGLE): if abs(move_cmd.angular.z)>=MAX_ANGULAR_Z: ROTATE_ACC_SPEED/RATE=0.0 move_cmd.angular.z+=ROTATE_ACC_SPEED/RATE else: move_cmd.angular.z-=ROTATE_ACC_SPEED/RATE move_cmd=Twist() cmd_vel.publish(move_cmd)
def PathCallback(path):
global POSES
global NewPath
if(len(path.poses)<1):
rospy.loginfo("a valid plan")
return
newpath=PathFilter.newPathFromAStar(path)
#两次滤波对路径进行规整
newpath=PathFilter.Lvbo(newpath,0.5)
newpath=PathFilter.Lvbo(newpath,0.8)
#mPath2.publish(newpath)
#newpath=PathFilter.Lvbo(newpath,1.0)
#newpath=PathFilter.Lvbo(newpath,1.)
#最后的滤波 选择长距离点
newpath=PathFilter.ChooseMainPath(newpath)
mPath.publish(newpath)
poses=newpath.poses
POSES=poses[:]
NewPath=True
def PathCallback(path): global POSES global NewPath newpath=PathFilter.newPathFromAStar(path) #两次滤波对路径进行规整 newpath=PathFilter.Lvbo(newpath,0.5) newpath=PathFilter.Lvbo(newpath,0.8) #mPath2.publish(newpath) newpath=PathFilter.Lvbo(newpath,1.0) #newpath=PathFilter.Lvbo(newpath,1.6) #最后的滤波 选择长距离点 newpath=PathFilter.ChooseMainPath(newpath) mPath.publish(newpath) poses=newpath.poses POSES=poses[:] NewPath=True
def get_odom():
(trans, rot) = tf_listener.lookupTransform('/map', '/base_link',
rospy.Time(0))
return Point(*trans), PathFilter.quat_to_angle(Quaternion(*rot))
def rotateToGoalDirection(pose):
goalAngle = PathFilter.quat_to_angle(pose.pose.orientation)
rotateToGoal(goalAngle)
def rotateToGoalDirection(pose): goalAngle=PathFilter.quat_to_angle(pose.pose.orientation) rotateToGoal(goalAngle)
def rotateToMoveDirection(pose): #计算目标点与当前点的全局坐标系下的角度值 (position, rotation) = get_odom() goalAngle=PathFilter.canculateAngle(pose.pose.position.x,pose.pose.position.y,position.x,position.y) rotateToGoal(goalAngle)
def get_odom():
(trans, rot) = tf_listener.lookupTransform('/odom','/base_link', rospy.Time(0))
return Point(*trans),PathFilter.quat_to_angle(Quaternion(*rot))