TB3-源码剖析-6.二维码识别
import rospy
import numpy as np
import tf
from enum import Enum
from nav_msgs.msg import Odometry
from ar_track_alvar_msgs.msg import AlvarMarkers
from geometry_msgs.msg import Twist
from tf.transformations import euler_from_quaternion, quaternion_from_euler
import math
import time
MARKER_ID_DETECTION = 17
class AutomaticParkingVision():
def __init__(self):
self.sub_odom_robot = rospy.Subscriber('/odom', Odometry, self.cbGetRobotOdom, queue_size = 1)
self.sub_info_marker = rospy.Subscriber('/ar_pose_marker', AlvarMarkers, self.cbGetMarkerOdom, queue_size = 1)
self.pub_cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
self.ParkingSequence = Enum('ParkingSequence', 'searching_parking_lot changing_direction moving_nearby_parking_lot parking stop finished')
self.NearbySequence = Enum('NearbySequence', 'initial_turn go_straight turn_right parking')
self.current_nearby_sequence = self.NearbySequence.initial_turn.value
self.current_parking_sequence = self.ParkingSequence.searching_parking_lot.value
self.robot_2d_pose_x = .0
self.robot_2d_pose_y = .0
self.robot_2d_theta = .0
self.marker_2d_pose_x = .0
self.marker_2d_pose_y = .0
self.marker_2d_theta = .0
self.previous_robot_2d_theta = .0
self.total_robot_2d_theta = .0
self.is_triggered = False
self.is_sequence_finished = False
self.is_odom_received = False
self.is_marker_pose_received = False
loop_rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
if self.is_odom_received is True:
self.fnParking()
loop_rate.sleep()
rospy.on_shutdown(self.fnShutDown)
def cbGetRobotOdom(self, robot_odom_msg):
if self.is_odom_received == False:
self.is_odom_received = True
pos_x, pos_y, theta = self.fnGet2DRobotPose(robot_odom_msg)
self.robot_2d_pose_x = pos_x
self.robot_2d_pose_y = pos_y
self.robot_2d_theta = theta
if (self.robot_2d_theta - self.previous_robot_2d_theta) > 5.:
d_theta = (self.robot_2d_theta - self.previous_robot_2d_theta) - 2 * math.pi
elif (self.robot_2d_theta - self.previous_robot_2d_theta) < -5.:
d_theta = (self.robot_2d_theta - self.previous_robot_2d_theta) + 2 * math.pi
else:
d_theta = (self.robot_2d_theta - self.previous_robot_2d_theta)
self.total_robot_2d_theta = self.total_robot_2d_theta + d_theta
self.previous_robot_2d_theta = self.robot_2d_theta
self.robot_2d_theta = self.total_robot_2d_theta
def cbGetMarkerOdom(self, markers_odom_msg):
for marker_odom_msg in markers_odom_msg.markers:
if marker_odom_msg.id == MARKER_ID_DETECTION:
if self.is_marker_pose_received == False:
self.is_marker_pose_received = True
pos_x, pos_y, theta = self.fnGet2DMarkerPose(marker_odom_msg)
self.marker_2d_pose_x = pos_x
self.marker_2d_pose_y = pos_y
self.marker_2d_theta = theta - math.pi
def fnParking(self):
if self.current_parking_sequence == self.ParkingSequence.searching_parking_lot.value:
self.is_sequence_finished = self.fnSeqSearchingGoal()
if self.is_sequence_finished == True:
print "Finished 1"
self.current_parking_sequence = self.ParkingSequence.changing_direction.value
self.is_sequence_finished = False
elif self.current_parking_sequence == self.ParkingSequence.changing_direction.value:
print "changing_direction"
self.is_sequence_finished = self.fnSeqChangingDirection()
if self.is_sequence_finished == True:
print "Finished 2"
self.current_parking_sequence = self.ParkingSequence.moving_nearby_parking_lot.value
self.is_sequence_finished = False
elif self.current_parking_sequence == self.ParkingSequence.moving_nearby_parking_lot.value:
print "moving_nearby_parking_lot"
self.is_sequence_finished = self.fnSeqMovingNearbyParkingLot()
if self.is_sequence_finished == True:
print "Finished 3"
self.current_parking_sequence = self.ParkingSequence.parking.value
self.is_sequence_finished = False
elif self.current_parking_sequence == self.ParkingSequence.parking.value:
self.is_sequence_finished = self.fnSeqParking()
if self.is_sequence_finished == True:
print "Finished 4"
self.current_parking_sequence = self.ParkingSequence.stop.value
self.is_sequence_finished = False
elif self.current_parking_sequence == self.ParkingSequence.stop.value:
self.fnStop()
print "Finished 5"
self.current_parking_sequence = self.ParkingSequence.finished.value
rospy.on_shutdown(self.fnShutDown)
def fnSeqSearchingGoal(self):
if self.is_marker_pose_received is False:
self.desired_angle_turn = -0.6
self.fnTurn(self.desired_angle_turn)
else:
self.fnStop()
return True
def fnSeqChangingDirection(self):
desired_angle_turn = -1. * math.atan2(self.marker_2d_pose_y - 0, self.marker_2d_pose_x - 0)
# rospy.loginfo("desired_angle_turn %f self.marker_2d_pose_x %f self.marker_2d_pose_y %f"
# , desired_angle_turn, self.marker_2d_pose_x, self.marker_2d_pose_y)
self.fnTurn(desired_angle_turn)
if abs(desired_angle_turn) < 0.01:
self.fnStop()
return True
else:
return False
def fnSeqMovingNearbyParkingLot(self):
if self.current_nearby_sequence == self.NearbySequence.initial_turn.value:
if self.is_triggered == False:
self.is_triggered = True
self.initial_robot_pose_theta = self.robot_2d_theta
self.initial_robot_pose_x = self.robot_2d_pose_x
self.initial_robot_pose_y = self.robot_2d_pose_y
self.initial_marker_pose_theta = self.marker_2d_theta
self.initial_marker_pose_x = self.marker_2d_pose_x
if self.initial_marker_pose_theta < 0.0:
desired_angle_turn = (math.pi / 2.0) + self.initial_marker_pose_theta - (self.robot_2d_theta - self.initial_robot_pose_theta)
elif self.initial_marker_pose_theta > 0.0:
desired_angle_turn = -(math.pi / 2.0) + self.initial_marker_pose_theta - (self.robot_2d_theta - self.initial_robot_pose_theta)
# rospy.loginfo("desired_angle_turn %f self.initial_marker_pose_theta %f self.robot_2d_theta %f self.initial_robot_pose_theta %f"
# , desired_angle_turn, self.initial_marker_pose_theta, self.robot_2d_theta, self.initial_robot_pose_theta)
desired_angle_turn = -1. * desired_angle_turn
self.fnTurn(desired_angle_turn)
if abs(desired_angle_turn) < 0.05:
self.fnStop()
self.current_nearby_sequence = self.NearbySequence.go_straight.value
self.is_triggered = False
elif self.current_nearby_sequence == self.NearbySequence.go_straight.value:
dist_from_start = self.fnCalcDistPoints(self.initial_robot_pose_x, self.robot_2d_pose_x, self.initial_robot_pose_y, self.robot_2d_pose_y)
desired_dist = self.initial_marker_pose_x * abs(math.cos((math.pi / 2.) - self.initial_marker_pose_theta))
remained_dist = desired_dist - dist_from_start
# rospy.loginfo("remained_dist %f desired_dist %f dist_from_start %f", remained_dist, desired_dist, dist_from_start)
self.fnGoStraight()
if remained_dist < 0.01:
self.fnStop()
self.current_nearby_sequence = self.NearbySequence.turn_right.value
elif self.current_nearby_sequence == self.NearbySequence.turn_right.value:
if self.is_triggered == False:
self.is_triggered = True
self.initial_robot_pose_theta = self.robot_2d_theta
if self.initial_marker_pose_theta < 0.0:
desired_angle_turn = -(math.pi / 2.0) + (self.robot_2d_theta - self.initial_robot_pose_theta)
elif self.initial_marker_pose_theta > 0.0:
desired_angle_turn = (math.pi / 2.0) + (self.robot_2d_theta - self.initial_robot_pose_theta)
# rospy.loginfo("desired_angle_turn %f self.robot_2d_theta %f self.initial_robot_pose_theta %f"
# , desired_angle_turn, self.robot_2d_theta, self.initial_robot_pose_theta)
self.fnTurn(desired_angle_turn)
if abs(desired_angle_turn) < 0.05:
self.fnStop()
self.current_nearby_sequence = self.NearbySequence.parking.value
self.is_triggered = False
return True
return False
def fnSeqParking(self):
desired_angle_turn = math.atan2(self.marker_2d_pose_y - 0, self.marker_2d_pose_x - 0)
self.fnTrackMarker(-desired_angle_turn)
print self.marker_2d_pose_x
if abs(self.marker_2d_pose_x) < 0.22:
self.fnStop()
return True
else:
return False
def fnStop(self):
twist = Twist()
twist.linear.x = 0
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = 0
self.pub_cmd_vel.publish(twist)
def fnTurn(self, theta):
Kp = 0.8
angular_z = Kp * theta
twist = Twist()
twist.linear.x = 0
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = -angular_z
self.pub_cmd_vel.publish(twist)
def fnGoStraight(self):
twist = Twist()
twist.linear.x = 0.2
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = 0
self.pub_cmd_vel.publish(twist)
def fnTrackMarker(self, theta):
Kp = 1.2
angular_z = Kp * theta
twist = Twist()
twist.linear.x = 0.10
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = -angular_z
self.pub_cmd_vel.publish(twist)
def fnGet2DRobotPose(self, robot_odom_msg):
quaternion = (robot_odom_msg.pose.pose.orientation.x, robot_odom_msg.pose.pose.orientation.y, robot_odom_msg.pose.pose.orientation.z, robot_odom_msg.pose.pose.orientation.w)
theta = tf.transformations.euler_from_quaternion(quaternion)[2]
if theta < 0:
theta = theta + np.pi * 2
if theta > np.pi * 2:
theta = theta - np.pi * 2
pos_x = robot_odom_msg.pose.pose.position.x
pos_y = robot_odom_msg.pose.pose.position.y
return pos_x, pos_y, theta
def fnGet2DMarkerPose(self, marker_odom_msg):
quaternion = (marker_odom_msg.pose.pose.orientation.x, marker_odom_msg.pose.pose.orientation.y, marker_odom_msg.pose.pose.orientation.z, marker_odom_msg.pose.pose.orientation.w)
theta = tf.transformations.euler_from_quaternion(quaternion)[2]
theta = theta + np.pi / 2.
# rospy.loginfo("theta : %f", theta)
if theta < 0:
theta = theta + np.pi * 2
if theta > np.pi * 2:
theta = theta - np.pi * 2
pos_x = marker_odom_msg.pose.pose.position.x
pos_y = marker_odom_msg.pose.pose.position.y
return pos_x, pos_y, theta
def fnCalcDistPoints(self, x1, x2, y1, y2):
return math.sqrt((x1 - x2) ** 2. + (y1 - y2) ** 2.)
def fnShutDown(self):
rospy.loginfo("Shutting down. cmd_vel will be 0")
twist = Twist()
twist.linear.x = 0
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = 0
self.pub_cmd_vel.publish(twist)
def main(self):
rospy.spin()
if __name__ == '__main__':
rospy.init_node('automatic_parking_vision')
node = AutomaticParkingVision()
node.main()<< 上一篇
下一篇 >>