예제 #1
0
class Robot_PID():
    def __init__(self):
        self.node_name = rospy.get_name()
        self.dis4constV = 3.  # Distance for constant velocity
        self.pos_ctrl_max = 0.7
        self.pos_ctrl_min = 0
        self.ang_ctrl_max = 1.0
        self.ang_ctrl_min = -1.0
        self.pos_station_max = 0.5
        self.pos_station_min = -0.5
        self.turn_threshold = 20
        self.cmd_ctrl_max = 0.7
        self.cmd_ctrl_min = -0.7
        self.station_keeping_dis = 0.5  # meters
        self.frame_id = 'map'
        self.is_station_keeping = False
        self.stop_pos = []
        self.final_goal = None  # The final goal that you want to arrive
        self.goal = self.final_goal
        self.robot_position = None

        rospy.loginfo("[%s] Initializing " % (self.node_name))
        self.sub_goal = rospy.Subscriber("/planning_path",
                                         Path,
                                         self.path_cb,
                                         queue_size=1)
        #self.sub_goal = rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.goal_cb, queue_size=1)
        rospy.Subscriber('/odometry/ground_truth',
                         Odometry,
                         self.odom_cb,
                         queue_size=1,
                         buff_size=2**24)
        self.pub_cmd = rospy.Publisher("/X1/cmd_vel", Twist, queue_size=1)
        self.pub_lookahead = rospy.Publisher("/lookahead_point",
                                             Marker,
                                             queue_size=1)
        self.pub_goal = rospy.Publisher("/goal_point", Marker, queue_size=1)
        self.station_keeping_srv = rospy.Service("/station_keeping", SetBool,
                                                 self.station_keeping_cb)

        self.pos_control = PID_control("Position")
        self.ang_control = PID_control("Angular")

        self.ang_station_control = PID_control("Angular_station")
        self.pos_station_control = PID_control("Position_station")

        self.purepursuit = PurePursuit()

        self.pos_srv = Server(pos_PIDConfig, self.pos_pid_cb, "Position")
        self.ang_srv = Server(ang_PIDConfig, self.ang_pid_cb, "Angular")
        self.pos_station_srv = Server(pos_PIDConfig, self.pos_station_pid_cb,
                                      "Angular_station")
        self.ang_station_srv = Server(ang_PIDConfig, self.ang_station_pid_cb,
                                      "Position_station")

        self.initialize_PID()

    def path_cb(self, msg):
        self.final_goal = []
        for pose in msg.poses:
            self.final_goal.append(
                [pose.pose.position.x, pose.pose.position.y])
        self.goal = self.final_goal
        self.is_station_keeping = False
        self.start_navigation = True
        self.purepursuit.set_goal(self.robot_position, self.goal)

    def odom_cb(self, msg):
        self.frame_id = msg.header.frame_id
        self.robot_position = [
            msg.pose.pose.position.x, msg.pose.pose.position.y
        ]
        if not self.is_station_keeping:
            self.stop_pos = [
                msg.pose.pose.position.x, msg.pose.pose.position.y
            ]
        quat = (msg.pose.pose.orientation.x,\
          msg.pose.pose.orientation.y,\
          msg.pose.pose.orientation.z,\
          msg.pose.pose.orientation.w)
        _, _, yaw = tf.transformations.euler_from_quaternion(quat)

        if self.goal is None:  # if the robot haven't recieve any goal
            return

        reach_goal = self.purepursuit.set_robot_pose(self.robot_position, yaw)
        pursuit_point = self.purepursuit.get_pursuit_point()

        if reach_goal or reach_goal is None:
            pos_output, ang_output = 0, 0
        else:
            self.publish_lookahead(self.robot_position, pursuit_point)
            goal_distance = self.get_distance(self.robot_position,
                                              pursuit_point)
            goal_angle = self.get_goal_angle(yaw, self.robot_position,
                                             pursuit_point)
            pos_output, ang_output = self.control(goal_distance, goal_angle)
        #yaw = yaw + np.pi/2
        '''goal_distance = self.get_distance(robot_position, self.goal)
								goal_angle = self.get_goal_angle(yaw, robot_position, self.goal)
								
								if goal_distance < self.station_keeping_dis or self.is_station_keeping:
									rospy.loginfo("Station Keeping")
									pos_output, ang_output = self.station_keeping(goal_distance, goal_angle)
								else:
									pos_output, ang_output = self.control(goal_distance, goal_angle)'''

        cmd_msg = Twist()
        cmd_msg.linear.x = pos_output
        cmd_msg.angular.z = ang_output
        self.pub_cmd.publish(cmd_msg)
        #self.publish_goal(self.goal)

    def control(self, goal_distance, goal_angle):
        self.pos_control.update(goal_distance)
        self.ang_control.update(goal_angle)

        # pos_output will always be positive
        pos_output = self.pos_constrain(-self.pos_control.output /
                                        self.dis4constV)

        # -1 = -180/180 < output/180 < 180/180 = 1
        ang_output = self.ang_constrain(self.ang_control.output * 2 / 180.)
        if abs(self.ang_control.output) > self.turn_threshold:
            if pos_output > 0.1:
                pos_output = 0.1
        return pos_output, ang_output

    def station_keeping(self, goal_distance, goal_angle):
        self.pos_station_control.update(goal_distance)
        self.ang_station_control.update(goal_angle)

        # pos_output will always be positive
        pos_output = self.pos_station_constrain(
            -self.pos_station_control.output / self.dis4constV)

        # -1 = -180/180 < output/180 < 180/180 = 1
        ang_output = self.ang_station_control.output / 180.

        # if the goal is behind the robot
        if abs(goal_angle) > 90:
            pos_output = -pos_output
            ang_output = -ang_output
        return pos_output, ang_output

    def station_keeping_cb(self, req):
        if req.data == True:
            self.goal = self.stop_pos
            self.is_station_keeping = True
        else:
            self.goal = self.final_goal
            self.is_station_keeping = False
        res = SetBoolResponse()
        res.success = True
        res.message = "recieved"
        return res

    def cmd_constarin(self, input):
        if input > self.cmd_ctrl_max:
            return self.cmd_ctrl_max
        if input < self.cmd_ctrl_min:
            return self.cmd_ctrl_min
        return input

    def pos_constrain(self, input):
        if input > self.pos_ctrl_max:
            return self.pos_ctrl_max
        if input < self.pos_ctrl_min:
            return self.pos_ctrl_min
        return input

    def ang_constrain(self, input):
        if input > self.ang_ctrl_max:
            return self.ang_ctrl_max
        if input < self.ang_ctrl_min:
            return self.ang_ctrl_min
        return input

    def pos_station_constrain(self, input):
        if input > self.pos_station_max:
            return self.pos_station_max
        if input < self.pos_station_min:
            return self.pos_station_min
        return input

    def initialize_PID(self):
        self.pos_control.setSampleTime(1)
        self.ang_control.setSampleTime(1)
        self.pos_station_control.setSampleTime(1)
        self.ang_station_control.setSampleTime(1)

        self.pos_control.SetPoint = 0.0
        self.ang_control.SetPoint = 0.0
        self.pos_station_control.SetPoint = 0.0
        self.ang_station_control.SetPoint = 0.0

    def get_goal_angle(self, robot_yaw, robot, goal):
        robot_angle = np.degrees(robot_yaw)
        p1 = [robot[0], robot[1]]
        p2 = [robot[0], robot[1] + 1.]
        p3 = goal
        angle = self.get_angle(p1, p2, p3)
        result = angle - robot_angle
        result = self.angle_range(-(result + 90.))
        return result

    def get_angle(self, p1, p2, p3):
        v0 = np.array(p2) - np.array(p1)
        v1 = np.array(p3) - np.array(p1)
        angle = np.math.atan2(np.linalg.det([v0, v1]), np.dot(v0, v1))
        return np.degrees(angle)

    def angle_range(self,
                    angle):  # limit the angle to the range of [-180, 180]
        if angle > 180:
            angle = angle - 360
            angle = self.angle_range(angle)
        elif angle < -180:
            angle = angle + 360
            angle = self.angle_range(angle)
        return angle

    def get_distance(self, p1, p2):
        return math.sqrt((p1[0] - p2[0])**2 + (p1[1] - p2[1])**2)

    def publish_goal(self, goal):
        marker = Marker()
        marker.header.frame_id = self.frame_id
        marker.header.stamp = rospy.Time.now()
        marker.ns = "pure_pursuit"
        marker.type = marker.SPHERE
        marker.action = marker.ADD
        marker.pose.orientation.w = 1
        marker.pose.position.x = goal[0]
        marker.pose.position.y = goal[1]
        marker.id = 0
        marker.scale.x = 0.6
        marker.scale.y = 0.6
        marker.scale.z = 0.6
        marker.color.a = 1.0
        marker.color.g = 1.0
        self.pub_goal.publish(marker)

    def pos_pid_cb(self, config, level):
        print("Position: [Kp]: {Kp}   [Ki]: {Ki}   [Kd]: {Kd}\n".format(
            **config))
        Kp = float("{Kp}".format(**config))
        Ki = float("{Ki}".format(**config))
        Kd = float("{Kd}".format(**config))
        self.pos_control.setKp(Kp)
        self.pos_control.setKi(Ki)
        self.pos_control.setKd(Kd)
        return config

    def ang_pid_cb(self, config, level):
        print(
            "Angular: [Kp]: {Kp}   [Ki]: {Ki}   [Kd]: {Kd}\n".format(**config))
        Kp = float("{Kp}".format(**config))
        Ki = float("{Ki}".format(**config))
        Kd = float("{Kd}".format(**config))
        self.ang_control.setKp(Kp)
        self.ang_control.setKi(Ki)
        self.ang_control.setKd(Kd)
        return config

    def pos_station_pid_cb(self, config, level):
        print("Position: [Kp]: {Kp}   [Ki]: {Ki}   [Kd]: {Kd}\n".format(
            **config))
        Kp = float("{Kp}".format(**config))
        Ki = float("{Ki}".format(**config))
        Kd = float("{Kd}".format(**config))
        self.pos_station_control.setKp(Kp)
        self.pos_station_control.setKi(Ki)
        self.pos_station_control.setKd(Kd)
        return config

    def ang_station_pid_cb(self, config, level):
        print(
            "Angular: [Kp]: {Kp}   [Ki]: {Ki}   [Kd]: {Kd}\n".format(**config))
        Kp = float("{Kp}".format(**config))
        Ki = float("{Ki}".format(**config))
        Kd = float("{Kd}".format(**config))
        self.ang_station_control.setKp(Kp)
        self.ang_station_control.setKi(Ki)
        self.ang_station_control.setKd(Kd)
        return config

    def publish_lookahead(self, robot, lookahead):
        marker = Marker()
        marker.header.frame_id = "/map"
        marker.header.stamp = rospy.Time.now()
        marker.ns = "pure_pursuit"
        marker.type = marker.LINE_STRIP
        marker.action = marker.ADD
        wp = Point()
        wp.x, wp.y = robot[:2]
        wp.z = 0
        marker.points.append(wp)
        wp = Point()
        wp.x, wp.y = lookahead[0], lookahead[1]
        wp.z = 0
        marker.points.append(wp)
        marker.id = 0
        marker.scale.x = 0.2
        marker.scale.y = 0.2
        marker.scale.z = 0.2
        marker.color.a = 1.0
        marker.color.b = 1.0
        marker.color.g = 1.0
        self.pub_lookahead.publish(marker)
class Robot_PID():
	def __init__(self):
		self.node_name = rospy.get_name()
		self.dis4constV = 1. # Distance for constant velocity
		self.pos_ctrl_max = 1
		self.pos_ctrl_min = 0.0
		self.pos_station_max = 0.5
		self.pos_station_min = -0.5
		self.cmd_ctrl_max = 0.95
		self.cmd_ctrl_min = -0.95
		self.station_keeping_dis = 1

		self.is_station_keeping = False
		self.start_navigation = False
		self.stop_pos = []
		self.final_goal = None # The final goal that you want to arrive
		self.goal = self.final_goal
		self.robot_position = None

		#parameter
		self.sim  = rospy.get_param('sim', False)

		rospy.loginfo("[%s] Initializing " %(self.node_name))

		self.sub_goal = rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.goal_cb, queue_size=1)
		rospy.Subscriber('odometry', Odometry, self.odom_cb, queue_size = 1, buff_size = 2**24)

		if self.sim:
			from duckiepond_vehicle.msg import UsvDrive
			self.pub_cmd = rospy.Publisher("cmd_drive", UsvDrive, queue_size = 1)
		else :
			self.pub_cmd = rospy.Publisher("cmd_drive", MotorCmd, queue_size = 1)

		self.pub_lookahead = rospy.Publisher("lookahead_point", Marker, queue_size = 1)
		self.station_keeping_srv = rospy.Service("station_keeping", SetBool, self.station_keeping_cb)
		self.navigate_srv = rospy.Service("navigation", SetBool, self.navigation_cb)

		self.pos_control = PID_control("Position")
		self.ang_control = PID_control("Angular")

		self.ang_station_control = PID_control("Angular_station")
		self.pos_station_control = PID_control("Position_station")

		self.purepursuit = PurePursuit()

		self.pos_srv = Server(pos_PIDConfig, self.pos_pid_cb, "Position")
		self.ang_srv = Server(ang_PIDConfig, self.ang_pid_cb, "Angular")
		self.pos_station_srv = Server(pos_PIDConfig, self.pos_station_pid_cb, "Angular_station")
		self.ang_station_srv = Server(ang_PIDConfig, self.ang_station_pid_cb, "Position_station")
		self.lookahead_srv = Server(lookaheadConfig, self.lookahead_cb, "LookAhead")

		self.initialize_PID()

	def odom_cb(self, msg):
		robot_position = [msg.pose.pose.position.x, msg.pose.pose.position.y]
		if not self.is_station_keeping:
			self.stop_pos = [[msg.pose.pose.position.x, msg.pose.pose.position.y]]
		quat = (msg.pose.pose.orientation.x,\
				msg.pose.pose.orientation.y,\
				msg.pose.pose.orientation.z,\
				msg.pose.pose.orientation.w)
		_, _, yaw = tf.transformations.euler_from_quaternion(quat)

		self.robot_position = robot_position
		if self.goal is None: # if the robot haven't recieve any goal
			return

		if not self.start_navigation:
			return
		reach_goal = self.purepursuit.set_robot_pose(robot_position, yaw)
		pursuit_point = self.purepursuit.get_pursuit_point()
		
		#yaw = yaw + np.pi/2.
		if reach_goal or reach_goal is None:
			self.publish_lookahead(robot_position, self.final_goal[-1])
			goal_distance = self.get_distance(robot_position, self.final_goal[-1])
			goal_angle = self.get_goal_angle(yaw, robot_position, self.final_goal[-1])
			pos_output, ang_output = self.station_keeping(goal_distance, goal_angle)
		elif self.is_station_keeping:
			rospy.loginfo("Station Keeping")
			self.publish_lookahead(robot_position, self.goal[0])
			goal_distance = self.get_distance(robot_position, self.goal[0])
			goal_angle = self.get_goal_angle(yaw, robot_position, self.goal[0])
			pos_output, ang_output = self.station_keeping(goal_distance, goal_angle)
		else:
			self.publish_lookahead(robot_position, pursuit_point)
			goal_distance = self.get_distance(robot_position, pursuit_point)
			goal_angle = self.get_goal_angle(yaw, robot_position, pursuit_point)
			pos_output, ang_output = self.control(goal_distance, goal_angle)
		
		if self.sim:
			cmd_msg = UsvDrive()
		else:
			cmd_msg = MotorCmd()
		cmd_msg.left = self.cmd_constarin(pos_output - ang_output)
		cmd_msg.right = self.cmd_constarin(pos_output + ang_output)
		self.pub_cmd.publish(cmd_msg)

	def control(self, goal_distance, goal_angle):
		self.pos_control.update(goal_distance)
		self.ang_control.update(goal_angle)

		# pos_output will always be positive
		pos_output = self.pos_constrain(-self.pos_control.output/self.dis4constV)

		# -1 = -180/180 < output/180 < 180/180 = 1
		ang_output = self.ang_control.output/180.
		return pos_output, ang_output

	def station_keeping(self, goal_distance, goal_angle):
		self.pos_station_control.update(goal_distance)
		self.ang_station_control.update(goal_angle)

		# pos_output will always be positive
		pos_output = self.pos_station_constrain(-self.pos_station_control.output/self.dis4constV)

		# -1 = -180/180 < output/180 < 180/180 = 1
		ang_output = self.ang_station_control.output/180.

		# if the goal is behind the robot
		if abs(goal_angle) > 90: 
			pos_output = - pos_output
			ang_output = - ang_output
		return pos_output, ang_output

	def goal_cb(self, p):
		if self.final_goal is None:
			self.final_goal = []
		self.final_goal.append([p.pose.position.x, p.pose.position.y])
		self.goal = self.final_goal

	def station_keeping_cb(self, req):
		if req.data == True:
			self.goal = self.stop_pos
			self.is_station_keeping = True
		else:
			self.is_station_keeping = False
		res = SetBoolResponse()
		res.success = True
		res.message = "recieved"
		return res

	def navigation_cb(self, req):
		if req.data == True:
			if not self.is_station_keeping:
				self.purepursuit.set_goal(self.robot_position, self.goal)
			self.is_station_keeping = False
			self.start_navigation = True
		else:
			self.start_navigation = False
			self.final_goal = None
			self.goal = self.stop_pos
		res = SetBoolResponse()
		res.success = True
		res.message = "recieved"
		return res

	def cmd_constarin(self, input):
		if input > self.cmd_ctrl_max:
			return self.cmd_ctrl_max
		if input < self.cmd_ctrl_min:
			return self.cmd_ctrl_min
		return input

	def pos_constrain(self, input):
		if input > self.pos_ctrl_max:
			return self.pos_ctrl_max
		if input < self.pos_ctrl_min:
			return self.pos_ctrl_min
		return input

	def pos_station_constrain(self, input):
		if input > self.pos_station_max:
			return self.pos_station_max
		if input < self.pos_station_min:
			return self.pos_station_min
		return input

	def initialize_PID(self):
		self.pos_control.setSampleTime(1)
		self.ang_control.setSampleTime(1)
		self.pos_station_control.setSampleTime(1)
		self.ang_station_control.setSampleTime(1)

		self.pos_control.SetPoint = 0.0
		self.ang_control.SetPoint = 0.0
		self.pos_station_control.SetPoint = 0.0
		self.ang_station_control.SetPoint = 0.0

	def get_goal_angle(self, robot_yaw, robot, goal):
		robot_angle = np.degrees(robot_yaw)
		p1 = [robot[0], robot[1]]
		p2 = [robot[0], robot[1]+1.]
		p3 = goal
		angle = self.get_angle(p1, p2, p3)
		result = angle - robot_angle
		result = self.angle_range(-(result + 90.))
		return result

	def get_angle(self, p1, p2, p3):
		v0 = np.array(p2) - np.array(p1)
		v1 = np.array(p3) - np.array(p1)
		angle = np.math.atan2(np.linalg.det([v0,v1]),np.dot(v0,v1))
		return np.degrees(angle)

	def angle_range(self, angle): # limit the angle to the range of [-180, 180]
		if angle > 180:
			angle = angle - 360
			angle = self.angle_range(angle)
		elif angle < -180:
			angle = angle + 360
			angle = self.angle_range(angle)
		return angle

	def get_distance(self, p1, p2):
		return math.sqrt((p1[0] - p2[0])**2 + (p1[1] - p2[1])**2)

	def pos_pid_cb(self, config, level):
		print("Position: [Kp]: {Kp}   [Ki]: {Ki}   [Kd]: {Kd}\n".format(**config))
		Kp = float("{Kp}".format(**config))
		Ki = float("{Ki}".format(**config))
		Kd = float("{Kd}".format(**config))
		self.pos_control.setKp(Kp)
		self.pos_control.setKi(Ki)
		self.pos_control.setKd(Kd)
		return config

	def publish_lookahead(self, robot, lookahead):
		marker = Marker()
		marker.header.frame_id = "map"
		marker.header.stamp = rospy.Time.now()
		marker.ns = "pure_pursuit"
		marker.type = marker.LINE_STRIP
		marker.action = marker.ADD
		wp = Point()
		wp.x, wp.y = robot[:2]
		wp.z = 0
		marker.points.append(wp)
		wp = Point()
		wp.x, wp.y = lookahead[0], lookahead[1]
		wp.z = 0
		marker.points.append(wp)
		marker.id = 0
		marker.scale.x = 0.2
		marker.scale.y = 0.2
		marker.scale.z = 0.2
		marker.color.a = 1.0
		marker.color.b = 1.0
		marker.color.g = 1.0
		self.pub_lookahead.publish(marker)

	def ang_pid_cb(self, config, level):
		print("Angular: [Kp]: {Kp}   [Ki]: {Ki}   [Kd]: {Kd}\n".format(**config))
		Kp = float("{Kp}".format(**config))
		Ki = float("{Ki}".format(**config))
		Kd = float("{Kd}".format(**config))
		self.ang_control.setKp(Kp)
		self.ang_control.setKi(Ki)
		self.ang_control.setKd(Kd)
		return config

	def pos_station_pid_cb(self, config, level):
		print("Position: [Kp]: {Kp}   [Ki]: {Ki}   [Kd]: {Kd}\n".format(**config))
		Kp = float("{Kp}".format(**config))
		Ki = float("{Ki}".format(**config))
		Kd = float("{Kd}".format(**config))
		self.pos_station_control.setKp(Kp)
		self.pos_station_control.setKi(Ki)
		self.pos_station_control.setKd(Kd)
		return config

	def ang_station_pid_cb(self, config, level):
		print("Angular: [Kp]: {Kp}   [Ki]: {Ki}   [Kd]: {Kd}\n".format(**config))
		Kp = float("{Kp}".format(**config))
		Ki = float("{Ki}".format(**config))
		Kd = float("{Kd}".format(**config))
		self.ang_station_control.setKp(Kp)
		self.ang_station_control.setKi(Ki)
		self.ang_station_control.setKd(Kd)
		return config

	def lookahead_cb(self, config, level):
		print("Look Ahead Distance: {Look_Ahead}\n".format(**config))
		lh = float("{Look_Ahead}".format(**config))
		self.purepursuit.set_lookahead(lh)
		return config
예제 #3
0
class NAVIGATION():
    def __init__(self):
        self.node_name = rospy.get_name()

        self.state = "normal"

        self.station_keeping_dis = 1
        self.is_station_keeping = False
        self.start_navigation = False
        self.over_bridge_count = 4
        self.stop_pos = []
        self.goals = []
        self.full_goals = []
        self.get_path = False
        self.final_goal = None  # The final goal that you want to arrive
        self.goal = self.final_goal
        self.robot_position = None
        self.cycle = rospy.get_param("~cycle", True)
        self.gazebo = rospy.get_param("~gazebo", False)
        self.lookahead = rospy.get_param("~lookahead", 2.2)
        rospy.loginfo("LookAhead: " + str(self.lookahead))

        self.over_bridge_counter = 0
        self.satellite_list = []
        self.satellite_thres = 15
        self.imu_angle = 0
        self.angle_thres = 0.85
        self.pre_pose = []
        self.bridge_mode = False

        self.stop_list = []
        self.stop_start_timer = rospy.get_time()
        self.stop_end_timer = rospy.get_time()

        self.satellite_avg = 0
        self.satellite_curr = 0

        self.log_string = ""

        rospy.loginfo("[%s] Initializing " % (self.node_name))

        # self.pub_lookahead = rospy.Publisher("lookahead_point", Marker, queue_size = 1)
        self.pub_robot_goal = rospy.Publisher("robot_goal",
                                              RobotGoal,
                                              queue_size=1)
        self.pub_fake_goal = rospy.Publisher("fake_goal", Marker, queue_size=1)
        self.pub_log_str = rospy.Publisher("log_str", String, queue_size=1)
        self.path_srv = rospy.Service("set_path", SetRobotPath, self.path_cb)
        self.reset_srv = rospy.Service("reset_goals", SetBool, self.reset_cb)
        # self.lookahead_srv = Server(lookaheadConfig, self.lookahead_cb, "LookAhead")

        self.purepursuit = PurePursuit()
        self.purepursuit.set_lookahead(self.lookahead)

        rospy.Subscriber("odometry",
                         Odometry,
                         self.odom_cb,
                         queue_size=1,
                         buff_size=2**24)
        rospy.Subscriber("/mavros/global_position/raw/satellites",
                         UInt32,
                         self.satellite_cb,
                         queue_size=1,
                         buff_size=2**24)
        # rospy.Subscriber("imu/data", Imu, self.imu_cb, queue_size = 1, buff_size = 2**24)

    def stop_state(self, time_threshold):
        if (rospy.get_time() - self.stop_start_timer) > time_threshold:
            self.state = "normal"
            return
        rg = RobotGoal()
        rg.goal.position.x, rg.goal.position.y = pursuit_point[
            0], pursuit_point[1]
        rg.robot = msg.pose.pose

    def imu_cb(self, msg):
        quat = (msg.orientation.x,\
          msg.orientation.y,\
          msg.orientation.z,\
          msg.orientation.w)
        _, _, angle = tf.transformations.euler_from_quaternion(quat)
        while angle >= np.pi:
            angle = angle - 2 * np.pi
        while angle < -np.pi:
            angle = angle + 2 * np.pi
        self.imu_angle = angle

    def satellite_cb(self, msg):
        self.satellite_curr = msg.data

    def publish_fake_goal(self, x, y):
        marker = Marker()
        marker.header.frame_id = "map"
        marker.header.stamp = rospy.Time.now()
        marker.ns = "fake_goal"
        marker.type = marker.CUBE
        marker.action = marker.ADD
        marker.pose.position.x = x
        marker.pose.position.y = y
        marker.pose.orientation.x = 0
        marker.pose.orientation.y = 0
        marker.pose.orientation.z = 0
        marker.pose.orientation.w = 1
        marker.scale.x = 0.7
        marker.scale.y = 0.7
        marker.scale.z = 0.7
        marker.color.a = 1.0
        marker.color.b = 1.0
        marker.color.g = 1.0
        marker.color.r = 1.0
        self.pub_fake_goal.publish(marker)

    def odom_cb(self, msg):
        self.robot_position = [
            msg.pose.pose.position.x, msg.pose.pose.position.y
        ]

        if not self.is_station_keeping:
            self.stop_pos = [[
                msg.pose.pose.position.x, msg.pose.pose.position.y
            ]]
        quat = (msg.pose.pose.orientation.x,\
          msg.pose.pose.orientation.y,\
          msg.pose.pose.orientation.z,\
          msg.pose.pose.orientation.w)
        _, _, yaw = tf.transformations.euler_from_quaternion(quat)

        while yaw >= np.pi:
            yaw = yaw - 2 * np.pi
        while yaw < -np.pi:
            yaw = yaw + 2 * np.pi
        self.imu_angle = yaw

        if len(
                self.goals
        ) == 0 or not self.get_path:  # if the robot haven't recieve any goal
            return

        reach_goal = self.purepursuit.set_robot_pose(self.robot_position, yaw)
        pursuit_point = self.purepursuit.get_pursuit_point()
        is_last_idx = self.purepursuit.is_last_idx()

        if reach_goal or pursuit_point is None or is_last_idx:
            if self.cycle:
                # The start point is the last point of the list
                self.stop_list = []
                start_point = [
                    self.goals[-1].waypoint.position.x,
                    self.goals[-1].waypoint.position.y
                ]
                self.full_goals[0] = self.full_goals[-1]
                self.purepursuit.set_goal(start_point, self.goals)
            else:
                rg = RobotGoal()
                rg.goal.position.x, rg.goal.position.y = self.goals[
                    -1].waypoint.position.x, self.goals[-1].waypoint.position.y
                rg.robot = msg.pose.pose
                rg.only_angle.data = False
                rg.mode.data = "normal"
                self.pub_robot_goal.publish(rg)
            return

        rg = RobotGoal()

        # if AUV is under the bridge
        if self.full_goals[self.purepursuit.current_waypoint_index -
                           1].bridge_start.data:
            self.bridge_mode = True
            rg.mode.data = "bridge"
            fake_goal, is_robot_over_goal = self.purepursuit.get_parallel_fake_goal(
            )
            if fake_goal is None:
                return
            self.publish_fake_goal(fake_goal[0], fake_goal[1])
            rg.goal.position.x, rg.goal.position.y = fake_goal[0], fake_goal[1]

            if is_robot_over_goal:
                if self.legal_angle():
                    if self.satellite_curr >= int(
                            self.satellite_avg) or self.gazebo:
                        self.over_bridge_counter = self.over_bridge_counter + 1
                        self.log_string = "over bridge, leagal angle, satellite"
                    else:
                        self.over_bridge_counter = 0
                        self.log_string = "over bridge, leagal angle, " + str(
                            self.satellite_curr) + "," + str(
                                self.satellite_avg)
                else:
                    self.over_bridge_counter = 0
                    self.log_string = "over bridge, illeagal angle"
            else:
                self.over_bridge_counter = 0
                self.log_string = "not over the bridge"

            if self.over_bridge_counter > self.over_bridge_count:
                if not (not self.cycle
                        and self.purepursuit.current_waypoint_index
                        == len(self.purepursuit.waypoints) - 1):
                    rospy.loginfo("[%s]Arrived waypoint: %d" %
                                  ("Over Bridge",
                                   self.purepursuit.current_waypoint_index))
                    if self.purepursuit.status != -1:
                        self.purepursuit.status = self.purepursuit.status + 1
                self.purepursuit.current_waypoint_index = self.purepursuit.current_waypoint_index + 1

        else:
            rg.mode.data = "normal"
            self.log_string = "not under bridge"
            self.bridge_mode = False
            rg.goal.position.x, rg.goal.position.y = pursuit_point[
                0], pursuit_point[1]

            if self.satellite_avg == 0:
                self.satellite_avg = self.satellite_curr
            else:
                self.satellite_avg = (self.satellite_avg * 3. +
                                      self.satellite_curr) / 4.

        if self.full_goals[self.purepursuit.current_waypoint_index -
                           1].stop_time.data != 0:
            if self.purepursuit.current_waypoint_index not in self.stop_list:
                self.stop_list.append(self.purepursuit.current_waypoint_index)
                self.state = "stop"
                self.stop_start_timer = rospy.get_time()

            if self.state == "stop":
                time_threshold = self.full_goals[
                    self.purepursuit.current_waypoint_index - 1].stop_time.data
                if (rospy.get_time() - self.stop_start_timer) > time_threshold:
                    self.state = "normal"
                else:
                    rg.goal.position.x, rg.goal.position.y = self.robot_position[
                        0], self.robot_position[1]

        rg.robot = msg.pose.pose
        self.purepursuit.bridge_mode = self.bridge_mode
        self.pub_robot_goal.publish(rg)

        self.pre_pose = [msg.pose.pose.position.x, msg.pose.pose.position.y]

        #yaw = yaw + np.pi/2.
        # if reach_goal or reach_goal is None:
        # 	self.publish_lookahead(self.robot_position, self.final_goal[-1])
        # else:
        # 	self.publish_lookahead(self.robot_position, pursuit_point)
        ss = String()
        ss.data = self.log_string
        self.pub_log_str.publish(ss)

    def legal_angle(self):
        if self.pre_pose != []:
            angle = self.getAngle()
            if angle < self.angle_thres:
                return True
        return False

    # Calculate the angle difference between robot heading and vector start from start_pose, end at end_pose and unit x vector of odom frame,
    # in radian
    def getAngle(self):
        if self.pre_pose == []:
            return
        delta_x = self.robot_position[0] - self.pre_pose[0]
        delta_y = self.robot_position[1] - self.pre_pose[1]
        theta = np.arctan2(delta_y, delta_x)
        angle = theta - self.imu_angle
        # Normalize in [-pi, pi)
        while angle >= np.pi:
            angle = angle - 2 * np.pi
        while angle < -np.pi:
            angle = angle + 2 * np.pi
        # print(theta, self.imu_angle, abs(angle))
        return abs(angle)

    def reset_cb(self, req):
        if req.data == True:
            self.full_goals = []
            self.goals = []
            self.get_path = False
        res = SetBoolResponse()
        res.success = True
        res.message = "recieved"
        return res

    def path_cb(self, req):
        rospy.loginfo("Get Path")
        res = SetRobotPathResponse()
        wp = WayPoint()
        wp.bridge_start.data = False
        wp.bridge_end.data = False
        self.full_goals.append(wp)
        if len(req.data.list) > 0:
            self.goals = req.data.list
            self.full_goals = self.full_goals + req.data.list
            self.get_path = True
            self.purepursuit.set_goal(self.robot_position, self.goals)
        res.success = True
        return res

    def get_goal_angle(self, robot_yaw, robot, goal):
        robot_angle = np.degrees(robot_yaw)
        p1 = [robot[0], robot[1]]
        p2 = [robot[0], robot[1] + 1.]
        p3 = goal
        angle = self.get_angle(p1, p2, p3)
        result = angle - robot_angle
        result = self.angle_range(-(result + 90.))
        return result

    def get_angle(self, p1, p2, p3):
        v0 = np.array(p2) - np.array(p1)
        v1 = np.array(p3) - np.array(p1)
        angle = np.math.atan2(np.linalg.det([v0, v1]), np.dot(v0, v1))
        return np.degrees(angle)

    def angle_range(self,
                    angle):  # limit the angle to the range of [-180, 180]
        if angle > 180:
            angle = angle - 360
            angle = self.angle_range(angle)
        elif angle < -180:
            angle = angle + 360
            angle = self.angle_range(angle)
        return angle

    def get_distance(self, p1, p2):
        return math.sqrt((p1[0] - p2[0])**2 + (p1[1] - p2[1])**2)

    def publish_lookahead(self, robot, lookahead):
        marker = Marker()
        marker.header.frame_id = "map"
        marker.header.stamp = rospy.Time.now()
        marker.ns = "pure_pursuit"
        marker.type = marker.LINE_STRIP
        marker.action = marker.ADD
        wp = Point()
        wp.x, wp.y = robot[:2]
        wp.z = 0
        marker.points.append(wp)
        wp = Point()
        wp.x, wp.y = lookahead[0], lookahead[1]
        wp.z = 0
        marker.points.append(wp)
        marker.id = 0
        marker.scale.x = 0.5
        marker.scale.y = 0.5
        marker.scale.z = 0.5
        marker.color.a = 1.0
        marker.color.b = 1.0
        marker.color.g = 1.0
        marker.color.r = 0.0
        #self.pub_lookahead.publish(marker)

    def lookahead_cb(self, config, level):
        print("Look Ahead Distance: {Look_Ahead}\n".format(**config))
        lh = float("{Look_Ahead}".format(**config))
        self.purepursuit.set_lookahead(lh)
        return config
예제 #4
0
class NAVIGATION():
    def __init__(self):
        self.node_name = rospy.get_name()
        self.station_keeping_dis = 1
        self.is_station_keeping = False
        self.start_navigation = False
        self.stop_pos = []
        self.goals = []
        self.diving_points = []
        self.diving_points_hold = []
        self.get_path = False
        self.yaw = 0
        self.dive = False
        self.finish_diving = True
        self.final_goal = None  # The final goal that you want to arrive
        self.goal = self.final_goal
        self.robot_position = None
        self.dive_dis = 5
        self.cycle = rospy.get_param("~cycle", True)

        rospy.loginfo("[%s] Initializing " % (self.node_name))

        # self.pub_lookahead = rospy.Publisher("lookahead_point", Marker, queue_size = 1)
        self.pub_robot_goal = rospy.Publisher("robot_goal",
                                              RobotGoal,
                                              queue_size=1)
        self.path_srv = rospy.Service("set_path", SetRobotPath, self.path_cb)
        self.finish_diving_srv = rospy.Service("finish_diving", SetBool,
                                               self.finish_diving_cb)
        # self.lookahead_srv = Server(lookaheadConfig, self.lookahead_cb, "LookAhead")

        self.purepursuit = PurePursuit()
        self.purepursuit.set_lookahead(5)

        self.odom_sub = rospy.Subscriber("odometry",
                                         Odometry,
                                         self.odom_cb,
                                         queue_size=1,
                                         buff_size=2**24)
        self.imu_sub = rospy.Subscriber("imu/data",
                                        Imu,
                                        self.imu_cb,
                                        queue_size=1,
                                        buff_size=2**24)

    def odom_cb(self, msg):
        if self.dive and not self.finish_diving:
            return
        self.robot_position = [
            msg.pose.pose.position.x, msg.pose.pose.position.y
        ]
        if not self.is_station_keeping:
            self.stop_pos = [[
                msg.pose.pose.position.x, msg.pose.pose.position.y
            ]]
        quat = (msg.pose.pose.orientation.x,\
          msg.pose.pose.orientation.y,\
          msg.pose.pose.orientation.z,\
          msg.pose.pose.orientation.w)
        _, _, yaw = tf.transformations.euler_from_quaternion(quat)

        if len(
                self.goals
        ) == 0 or not self.get_path:  # if the robot haven't recieve any goal
            return

        reach_goal = self.purepursuit.set_robot_pose(self.robot_position, yaw)
        pursuit_point = self.purepursuit.get_pursuit_point()
        is_last_idx = self.purepursuit.is_last_idx()

        if reach_goal or pursuit_point is None or is_last_idx:
            if self.cycle:
                # The start point is the last point of the list
                start_point = [
                    self.goals[-1].position.x, self.goals[-1].position.y
                ]
                self.purepursuit.set_goal(start_point, self.goals)
                self.diving_points = self.diving_points_hold[:]
            else:
                rg = RobotGoal()
                rg.goal.position.x, rg.goal.position.y = self.goals[
                    -1].position.x, self.goals[-1].position.y
                rg.robot = msg.pose.pose
                self.pub_robot_goal.publish(rg)
            return
        self.dive = self.if_dive()

        rg = RobotGoal()
        rg.goal.position.x, rg.goal.position.y = pursuit_point[
            0], pursuit_point[1]
        rg.robot = msg.pose.pose
        rg.only_angle.data = False
        self.pub_robot_goal.publish(rg)

        #yaw = yaw + np.pi/2.
        # if reach_goal or reach_goal is None:
        # 	self.publish_lookahead(self.robot_position, self.final_goal[-1])
        # else:
        # 	self.publish_lookahead(self.robot_position, pursuit_point)

    def imu_cb(self, msg):
        quat = (msg.orientation.x,\
          msg.orientation.y,\
          msg.orientation.z,\
          msg.orientation.w)
        _, _, yaw = tf.transformations.euler_from_quaternion(quat)
        self.yaw = yaw
        if self.dive:
            if self.finish_diving:
                self.dive = False
            reach_goal = self.purepursuit.set_robot_pose(
                self.robot_position, yaw)
            pursuit_point = self.purepursuit.get_pursuit_point()
            is_last_idx = self.purepursuit.is_last_idx()
            if reach_goal or pursuit_point is None or is_last_idx:
                rg = RobotGoal()
                rg.goal.position.x, rg.goal.position.y = self.goals[
                    -1].position.x, self.goals[-1].position.y
                rg.robot = msg.pose.pose
                self.pub_robot_goal.publish(rg)
                return

            rg = RobotGoal()
            rg.goal.position.x, rg.goal.position.y = pursuit_point[
                0], pursuit_point[1]
            p = Pose()
            p.position.x = self.robot_position[0]
            p.position.y = self.robot_position[1]
            rg.robot = p
            rg.only_angle.data = True
            self.pub_robot_goal.publish(rg)

    def path_cb(self, req):
        rospy.loginfo("Get Path")
        res = SetRobotPathResponse()
        if len(req.data.list) > 0:
            self.goals = req.data.list
            self.diving_points_hold = self.goals[:]
            self.diving_points = self.diving_points_hold[:]
            self.get_path = True
            self.purepursuit.set_goal(self.robot_position, self.goals)
        res.success = True
        return res

    def finish_diving_cb(self, req):
        if req.data == True:
            rospy.loginfo("Finish Diving")
            self.finish_diving = True
        res = SetBoolResponse()
        res.success = True
        res.message = "recieved"
        return res

    def if_dive(self):
        arr = False
        del_pt = None
        for dv_pt in self.diving_points:
            p1 = [dv_pt.position.x, dv_pt.position.y]
            p2 = self.robot_position
            if self.get_distance(p1, p2) <= self.dive_dis:
                print("DIVE")
                arr = True
                del_pt = dv_pt
                self.finish_diving = False
                self.srv_dive()
        if arr:
            self.diving_points.remove(del_pt)
            return True
        return False

    def srv_dive(self):
        #rospy.wait_for_service('/set_path')
        rospy.loginfo("SRV: Send diving")
        set_bool = SetBoolRequest()
        set_bool.data = True
        try:
            srv = rospy.ServiceProxy('dive', SetBool)
            resp = srv(set_bool)
            return resp
        except rospy.ServiceException, e:
            print "Service call failed: %s" % e