Ejemplo n.º 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.6
		self.pos_ctrl_min = 0
		self.ang_ctrl_max = 0.4
		self.ang_ctrl_min = -0.4
		self.turn_threshold = 30
		self.cmd_ctrl_max = 0.1
		self.cmd_ctrl_min = -0.1
		self.arrived_dis = 0.5 # meters
		self.frame_id = 'map'
		self.emergency_stop = False
		self.final_goal = None # The final goal that you want to arrive
		self.goal = self.final_goal
		self.cmd = Twist()
		self.spaceship_cmd = MotorsCmd()

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

		self.sub_goal = rospy.Subscriber("pursue_point", PoseStamped, self.goal_cb, queue_size=1)
		self.pub_spaceship_cmd = rospy.Publisher("/spaceship/motors_cmd", MotorsCmd, queue_size = 1)
		self.pub_cmd = rospy.Publisher("/husky_velocity_controller/cmd_vel", Twist, queue_size = 1)

		
		self.pub_cmd2d = rospy.Publisher("/spaceship/vel_cmd", Twist2D, queue_size = 1)
		self.cmd2d = Twist2D()
	
		self.pub_goal = rospy.Publisher("/goal_point", Marker, queue_size = 1)
		self.emergency_stop_srv = rospy.Service("/emergency_stop", SetBool, self.emergency_stop_cb)

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

		self.pos_srv = Server(pos_PIDConfig, self.pos_pid_cb, "Position")
		self.ang_srv = Server(ang_PIDConfig, self.ang_pid_cb, "Angular")
		
		self.initialize_PID()


		while not rospy.is_shutdown():
			#self.pub_spaceship_cmd.publish(self.spaceship_cmd)
			#self.pub_cmd.publish(self.cmd)
			##### omni version #####
			self.pub_cmd2d.publish(self.cmd2d)

			if self.goal is not None:
				self.publish_goal(self.goal)
			rospy.sleep(0.1)

	def goal_cb(self, p):
		robot_position = [0, 0]
		self.final_goal = [p.pose.position.x, p.pose.position.y]
		if self.final_goal == [0, 0]:
			cmd_msg = Twist()
			cmd_msg.linear.x = 0
			cmd_msg.angular.z = 0
			self.cmd = cmd_msg
			return
		self.goal = self.final_goal
		goal_distance = self.get_distance(robot_position, self.goal)
		goal_angle = self.get_goal_angle(0, robot_position, self.goal)
		if goal_distance < self.arrived_dis or self.emergency_stop:
			rospy.loginfo("Stop!!!")
			pos_output, ang_output = (0, 0)
		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.cmd = cmd_msg

		##### omni version #####
		cmd2d = Twist2D()
		cmd2d.v = pos_output + 0.2
		cmd2d.omega = ang_output
		self.cmd2d = cmd2d
		print('===',self.cmd2d)
		
		#spaceship_cmd_msg = MotorsCmd()
		#self.spaceship_cmd.vel_front_left = 1300 + (pos_output + ang_output) * 600 / (self.pos_ctrl_max + self.ang_ctrl_max)
	        #self.spaceship_cmd.vel_front_right = 1300 + (pos_output - ang_output) * 600 / ((self.pos_ctrl_max + self.ang_ctrl_max))

	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*3/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 emergency_stop_cb(self, req):
		if req.data == True:
			self.emergency_stop = True
		else:
			self.emergency_stop = 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 initialize_PID(self):
		self.pos_control.setSampleTime(1)
		self.ang_control.setSampleTime(1)

		self.pos_control.SetPoint = 0.0
		self.ang_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
Ejemplo n.º 2
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.turn_threshold = 20
        self.slow_speed = 0.1
        self.cmd_ctrl_max = 0.7
        self.cmd_ctrl_min = -0.7
        self.arrived_dis = 0.5  # meters
        self.frame_id = 'map'
        self.emergency_stop = False
        self.final_goal = None  # The final goal that you want to arrive
        self.goal = self.final_goal
        self.arrive = True

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

        self.sub_goal = rospy.Subscriber("pursue_point",
                                         PoseStamped,
                                         self.goal_cb,
                                         queue_size=1)
        self.sub_arrive = rospy.Subscriber("arrive",
                                           Bool,
                                           self.arrive_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_goal = rospy.Publisher("goal_point", Marker, queue_size=1)
        self.emergency_stop_srv = rospy.Service("/emergency_stop", SetBool,
                                                self.emergency_stop_cb)

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

        self.pos_srv = Server(pos_PIDConfig, self.pos_pid_cb, "Position")
        self.ang_srv = Server(ang_PIDConfig, self.ang_pid_cb, "Angular")

        self.initialize_PID()

    def arrive_cb(self, msg):
        self.arrive = msg.data

    def odom_cb(self, msg):
        self.frame_id = msg.header.frame_id
        robot_position = [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

        #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.arrived_dis or self.emergency_stop or self.arrive:
            rospy.loginfo("Stop!!!")
            pos_output, ang_output = (0, 0)
        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 * 3 / 180.)
        if abs(self.ang_control.output) > self.turn_threshold:
            if pos_output > 0.1:
                pos_output = self.slow_speed
        return pos_output, ang_output

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

    def emergency_stop_cb(self, req):
        if req.data == True:
            self.emergency_stop = True
        else:
            self.emergency_stop = 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 initialize_PID(self):
        self.pos_control.setSampleTime(1)
        self.ang_control.setSampleTime(1)

        self.pos_control.SetPoint = 0.0
        self.ang_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
Ejemplo n.º 3
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.turn_threshold = 20
        self.slow_speed = 0.1
        self.cmd_ctrl_max = 0.7
        self.cmd_ctrl_min = -0.7
        self.arrived_dis = 0.2  # meters
        #self.frame_id = 'map'
        self.frame_id = 'laser_frame'
        #self.frame_id = 'camera_link'
        self.emergency_stop = True
        self.final_goal = None  # The final goal that you want to arrive
        self.goal = self.final_goal

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

        self.use_odom = rospy.get_param("use_odom")
        if not self.use_odom:
            rospy.Timer(rospy.Duration(1 / 30.), self.timer_cb)
            self.cmd = Twist()

        self.sub_goal = rospy.Subscriber("pursue_point",
                                         PoseStamped,
                                         self.goal_cb,
                                         queue_size=1)
        self.pub_arrive = rospy.Publisher("arrive", Bool, queue_size=1)

        if self.use_odom:
            rospy.Subscriber('odometry/ground_truth',
                             Odometry,
                             self.odom_cb,
                             queue_size=1,
                             buff_size=2**24)
        self.pub_cmd = rospy.Publisher("cmd_vel", Twist, queue_size=1)
        self.pub_goal = rospy.Publisher("goal_point", Marker, queue_size=1)
        self.pub_estop = rospy.Publisher("/racecar/stop_mode",
                                         Bool,
                                         queue_size=1)
        self.emergency_stop_srv = rospy.Service("emergency_stop", SetBool,
                                                self.emergency_stop_cb)

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

        self.pos_srv = Server(pos_PIDConfig, self.pos_pid_cb, "Position")
        self.ang_srv = Server(ang_PIDConfig, self.ang_pid_cb, "Angular")

        self.initialize_PID()

        # sub joy to switch on or off
        self.auto = False
        self.sub_joy = rospy.Subscriber('joy', Joy, self.cb_joy, queue_size=1)

    def odom_cb(self, msg):
        self.frame_id = msg.header.frame_id
        robot_position = [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

        #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.arrived_dis or self.emergency_stop:
            print "Goal: ", self.goal, ", robot at: ", robot_position
            print goal_distance
            rospy.loginfo("Stop in odom_cb!!!")
            self.pub_arrive.publish(Bool(True))
            print goal_distance, " ", self.emergency_stop, " "
            pos_output, ang_output = (0, 0)
            self.goal = None
        else:
            pos_output, ang_output = self.control(goal_distance, goal_angle)

        cmd_msg = Twist()
        cmd_msg.linear.x = pos_output * 2
        cmd_msg.angular.z = ang_output
        if not self.emergency_stop:
            self.pub_cmd.publish(cmd_msg)
        if self.goal is not None:
            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 * 3 / 180.)
        if abs(self.ang_control.output) > self.turn_threshold:
            if pos_output > 0.1:
                pos_output = self.slow_speed
        return pos_output, ang_output

    def goal_cb(self, p):
        print('goal_cb')
        self.final_goal = [p.pose.position.x, p.pose.position.y]
        self.goal = self.final_goal
        # New goal, publish arrive to false
        self.pub_arrive.publish(Bool(False))
        if not self.use_odom:
            robot_position = [0.0, 0.0]
            yaw = 0
            goal_distance = self.get_distance(robot_position, self.goal)
            goal_angle = self.get_goal_angle(yaw, robot_position, self.goal)
            estop = Bool()
            if goal_distance < self.arrived_dis:
                rospy.loginfo("Arrived!!!")
                self.pub_arrive.publish(Bool(True))
                print goal_distance
                pos_output, ang_output = (0, 0)
                estop.data = True
                self.pub_estop.publish(estop)
                self.goal = None
            elif self.emergency_stop:
                rospy.loginfo("JOYSTICK: Stop!!!")
                self.pub_arrive.publish(Bool(True))
                pos_output, ang_output = (0, 0)
                estop.data = True
                self.pub_estop.publish(estop)
                self.publish_goal(self.goal)
                return
            else:
                estop.data = False
                self.pub_estop.publish(estop)
                pos_output, ang_output = self.control(goal_distance,
                                                      goal_angle)

                self.cmd = Twist()

                self.cmd.linear.x = pos_output * 2
                self.cmd.angular.z = ang_output
                if self.auto:
                    self.pub_cmd.publish(self.cmd)
                    print('Pub cmd')
            if self.goal is not None:
                self.publish_goal(self.goal)

    def emergency_stop_cb(self, req):
        if req.data == True:
            self.emergency_stop = True
            rospy.loginfo("Mode: Joystick")
        else:
            self.emergency_stop = False
            rospy.loginfo("Mode: Autonomous")
        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 initialize_PID(self):
        self.pos_control.setSampleTime(1)
        self.ang_control.setSampleTime(1)

        self.pos_control.SetPoint = 0.0
        self.ang_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()
        if self.use_odom: marker.header.frame_id = self.frame_id
        else: marker.header.frame_id = "laser_frame"
        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 timer_cb(self, event):
        if not self.emergency_stop:
            self.pub_cmd.publish(self.cmd)

    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 cb_joy(self, joy_msg):
        # MODE D
        start_button = 1
        back_button = 3
        # Start button
        if joy_msg.buttons[start_button] == 1:
            self.auto = True
            rospy.loginfo('go auto')
        elif joy_msg.buttons[back_button] == 1:
            self.auto = False
            rospy.loginfo('go manual')
Ejemplo n.º 4
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)
Ejemplo n.º 5
0
class Tracking():
    def __init__(self):
        self.node_name = rospy.get_name()
        rospy.loginfo("[%s] Initializing " % (self.node_name))
        self.ROBOT_NUM = 3
        self.wavm_labels = ["wamv", ""]
        #rospy.Subscriber('/pcl_points_img', PoseArray, self.call_back, queue_size = 1, buff_size = 2**24)
        self.net = build_ssd('test', 300, 3)  # initialize SSD
        self.net.load_weights(
            '/home/arg_ws3/argbot/catkin_ws/src/tracking/src/ssd300_wamv_5000.pth'
        )
        if torch.cuda.is_available():
            self.net = self.net.cuda()

        # Image definition
        self.width = 1280
        self.height = 960
        self.h_w = 10.
        self.const_SA = 0.17
        self.bridge = CvBridge()
        self.predict_prob = 0.5

        self.pos_ctrl_max = 1
        self.pos_ctrl_min = -1
        self.cmd_ctrl_max = 0.95
        self.cmd_ctrl_min = -0.95
        self.station_keeping_dis = 3.5  # meters
        self.frame_id = 'odom'
        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

        rospy.loginfo("[%s] Initializing " % (self.node_name))
        #self.image_sub = rospy.Subscriber("/BRIAN/camera_node/image/compressed", Image, self.img_cb, queue_size=1)
        self.image_sub = rospy.Subscriber("/detecter/predictions",
                                          Boxlist,
                                          self.box_cb,
                                          queue_size=1,
                                          buff_size=2**24)
        self.pub_cmd = rospy.Publisher("/MONICA/cmd_drive",
                                       UsvDrive,
                                       queue_size=1)
        self.pub_goal = rospy.Publisher("/goal_point", Marker, queue_size=1)
        self.image_pub = rospy.Publisher("/predict_img", Image, queue_size=1)
        self.station_keeping_srv = rospy.Service("/station_keeping", SetBool,
                                                 self.station_keeping_cb)

        self.pos_control = PID_control("Position_tracking")
        self.ang_control = PID_control("Angular_tracking")

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

        self.pos_srv = Server(pos_PIDConfig, self.pos_pid_cb,
                              "Position_tracking")
        self.ang_srv = Server(ang_PIDConfig, self.ang_pid_cb,
                              "Angular_tracking")
        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 box_cb(self, msg):
        self.width = msg.image_width
        self.height = msg.image_height
        try:
            np_arr = np.fromstring(msg.image.data, np.uint8)
            cv_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
            #cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
        except CvBridgeError as e:
            print(e)
        #(rows, cols, channels) = cv_image.shape
        boxes = msg.list
        predict = get_control_info(boxes, cv_image)
        if predict is None:
            return
        angle, dis = predict[0], predict[1]
        self.tracking_control(angle, dis)

    def tracking_control(self, goal_angle, goal_distance):
        if 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 = UsvDrive()
        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)
        #self.publish_goal(self.goal)

    def get_control_info(self, boxes, img):
        # Image Preprocessing (vgg use BGR image as training input)
        confidence = 0
        bbox = None
        for box in boxes:
            if box.confidence > confidence:
                bbox = box
        angle, dis, center = self.BBx2AngDis(bbox)
        cv2.circle(img, (int(center[0]), int(center[1])), 10, (0, 0, 255), -1)
        cv2.rectangle(img, (int(coords[0][0]), int(coords[0][1])),\
             (int(coords[0][0] + coords[1]), int(coords[0][1] + coords[2])),(0,0,255),5)
        try:
            img = self.draw_cmd(img, dis, angle)
            self.image_pub.publish(self.bridge.cv2_to_imgmsg(img, "bgr8"))
        except CvBridgeError as e:
            print(e)
        return angle, dis

    def draw_cmd(self, img, dis, angle):
        v = dis
        omega = angle
        rad = omega * math.pi / 2.  # [-1.57~1.57]
        rad = rad - math.pi / 2.  # rotae for correct direction
        radius = 120
        alpha = 0.3
        v_length = radius * math.sqrt(v**2 + v**2) / math.sqrt(1**2 + 1**2)
        if v_length > radius:
            v_length = radius
        x = v_length * math.cos(rad)
        y = v_length * math.sin(rad)
        x_max = radius * math.cos(rad)
        y_max = radius * math.sin(rad)
        center = (int(self.width / 2.), int(self.height))
        draw_img = img.copy()
        cv2.circle(draw_img, center, radius, (255, 255, 255), 10)
        cv2.addWeighted(draw_img, alpha, img, 1 - alpha, 0, img)
        #cv2.circle(img, (int(center[0]+x_max), int(center[1]+y_max)), 15, (255, 255, 255), -1)
        cv2.arrowedLine(img, center, (int(center[0] + x), int(center[1] + y)),
                        (255, 255, 255), 7)
        return img

    def BBx2AngDis(self, bbox):
        x = bbox.x
        y = bbox.y
        w = bbox.w
        h = bbox.h
        center = (x + w / 2., y + h / 2.)
        angle = (center[0] - self.width / 2.) / (self.width / 2.)
        dis = (self.height - h) / (self.height)
        return angle, dis, center

    def control(self, goal_distance, goal_angle):
        self.pos_control.update(10 * (goal_distance - self.const_SA))
        self.ang_control.update(goal_angle)

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

        # -1 = -180/180 < output/180 < 180/180 = 1
        ang_output = self.ang_control.output / (self.width / 2.)
        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 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
Ejemplo n.º 6
0
class Robot_PID():
    def __init__(self):
        self.node_name = rospy.get_name()
        self.dis4constV = 5.0  # Distance for constant velocity
        self.max_dis_ratiao = 0.8
        self.pos_ctrl_max = 1
        self.pos_ctrl_min = -1
        self.pos_station_max = 0.8
        self.pos_station_min = -0.8
        self.cmd_ctrl_max = 1
        self.cmd_ctrl_min = -1
        self.station_keeping_dis = 3.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.heading = 0
        rospy.loginfo("[%s] Initializing " % (self.node_name))

        # Param
        self.sim = rospy.get_param('sim', False)
        self.tune = rospy.get_param('tune', True)

        self.sub_goal = rospy.Subscriber("/move_base_simple/goal",
                                         PoseStamped,
                                         self.goal_cb,
                                         queue_size=1)
        rospy.Subscriber('localization_gps_imu/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",
                                           VelocityVector,
                                           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)

        if self.tune:
            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.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()
        else:
            print 'no working...'

    def odom_cb(self, msg):
        self.frame_id = msg.header.frame_id
        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

        #yaw = yaw + np.pi/2
        goal_vector = self.get_distance(robot_position, self.goal)
        goal_angle = self.get_goal_angle(yaw, robot_position, self.goal)
        goal_distance = math.sqrt(goal_vector[0]**2 + goal_vector[1]**2)

        if goal_distance < self.station_keeping_dis or self.is_station_keeping:
            rospy.loginfo("Station Keeping")
            head_angle = self.angle_range(np.degrees(yaw) - self.heading)
            pos_x_output, pos_y_output, ang_output = self.station_keeping(
                head_angle, goal_distance, goal_angle)
        else:
            rospy.loginfo("Navigating")
            head_angle = goal_angle
            pos_x_output, pos_y_output, ang_output = self.control(
                head_angle, goal_distance, goal_angle)

        if self.sim:
            cmd_msg = UsvDrive()
        else:
            cmd_msg = VelocityVector()

        cmd_msg.x = self.cmd_constarin(pos_x_output)
        cmd_msg.y = self.cmd_constarin(pos_y_output)
        cmd_msg.angular = self.cmd_constarin(ang_output)
        print("heading %f" % self.heading)
        print("angular %f" % cmd_msg.angular)
        print("compare %f" % np.degrees(yaw))
        self.pub_cmd.publish(cmd_msg)
        self.publish_goal(self.goal)

    def control(self, head_angle, goal_distance, goal_angle):
        self.pos_control.update(goal_distance)
        dis_ratiao = -self.pos_control.output / self.dis4constV
        dis_ratiao = max(min(dis_ratiao, self.max_dis_ratiao), 0)
        print("dis ratiao%f" % dis_ratiao)

        pos_x_output = math.sin(math.radians(goal_angle))
        pos_y_output = math.cos(math.radians(goal_angle))
        pos_x_output = self.pos_constrain(pos_x_output * dis_ratiao)
        pos_y_output = self.pos_constrain(pos_y_output * dis_ratiao)

        # -1 = -180/180 < output/180 < 180/180 = 1
        self.ang_control.update(head_angle)
        ang_output = self.ang_control.output / 180. * -1
        return pos_x_output, pos_y_output, ang_output

    def station_keeping(self, head_angle, goal_distance, goal_angle):

        self.pos_station_control.update(goal_distance)
        dis_ratiao = -self.pos_station_control.output
        dis_ratiao = max(min(dis_ratiao, self.max_dis_ratiao), 0)

        pos_x_output = math.sin(math.radians(goal_angle))
        pos_y_output = math.cos(math.radians(goal_angle))
        pos_x_output = self.pos_constrain(pos_x_output * dis_ratiao)
        pos_y_output = self.pos_constrain(pos_y_output * dis_ratiao)

        # -1 = -180/180 < output/180 < 180/180 = 1
        self.ang_station_control.update(head_angle)
        ang_output = self.ang_station_control.output / 180. * -1
        return pos_x_output, pos_y_output, ang_output

    def goal_cb(self, p):
        self.final_goal = [p.pose.position.x, p.pose.position.y]
        self.goal = self.final_goal
        quat = (p.pose.orientation.x,\
          p.pose.orientation.y,\
          p.pose.orientation.z,\
          p.pose.orientation.w)
        _, _, yaw = tf.transformations.euler_from_quaternion(quat)
        self.heading = np.degrees(yaw)

    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 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 [p1[0] - p2[0], p1[1] - p2[1]]

    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
class Tracking():
    def __init__(self):
        self.node_name = rospy.get_name()
        rospy.loginfo("[%s] Initializing " % (self.node_name))

        # Image definition
        self.width = 640
        self.height = 480
        self.h_w = 10.
        self.const_SA = 0.4
        self.predict_prob = 0.5

        self.pos_ctrl_max = 1
        self.pos_ctrl_min = -1
        self.cmd_ctrl_max = 1
        self.cmd_ctrl_min = -1

        rospy.loginfo("[%s] Initializing " % (self.node_name))
        self.image_sub = rospy.Subscriber("BoundingBoxes",
                                          BoundingBoxes,
                                          self.box_cb,
                                          queue_size=1,
                                          buff_size=2**24)

        self.pub_cmd = rospy.Publisher('track_vel', Twist, queue_size=1)
        self.cmd_msg = Twist()

        self.pos_control = PID_control("Position_tracking")
        self.ang_control = PID_control("Angular_tracking")

        self.pos_srv = Server(pos_PIDConfig, self.pos_pid_cb,
                              "Position_tracking")
        self.ang_srv = Server(ang_PIDConfig, self.ang_pid_cb,
                              "Angular_tracking")

        self.initialize_PID()

        self.timer = rospy.Timer(rospy.Duration(0.2), self.cb_publish)

    def cb_publish(self, event):
        self.pub_cmd.publish(self.cmd_msg)

    def box_cb(self, msg):
        goal_angle, goal_distance = self.BBx2AngDis(msg.bounding_boxes[0])
        pos_output, ang_output = self.control(goal_distance, goal_angle)

        self.cmd_msg = Twist()
        self.cmd_msg.linear.x = pos_output
        self.cmd_msg.angular.z = ang_output

    def BBx2AngDis(self, bbox):
        center = [(bbox.xmin + bbox.xmax) / 2., (bbox.ymin + bbox.ymax) / 2.]
        angle = (center[0] - self.width / 2.) / (self.width / 2.)
        dis = float(self.height - (bbox.ymax - bbox.ymin)) / (self.height)
        return angle, dis

    def control(self, goal_distance, goal_angle):
        self.pos_control.update(5 * (goal_distance - self.const_SA))
        self.ang_control.update(goal_angle)

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

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

    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 initialize_PID(self):
        self.pos_control.setSampleTime(1)
        self.ang_control.setSampleTime(1)

        self.pos_control.SetPoint = 0.0
        self.ang_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)

    # limit the angle to the range of [-180, 180]
    def angle_range(self, angle):
        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 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
Ejemplo n.º 8
0
class BoatHRVO(object):
    def __init__(self):
        self.node_name = rospy.get_name()
        rospy.loginfo("[%s] Initializing" % self.node_name)

        # initiallize PID
        self.dis_pid = PID_control("distance_control")
        self.angu_pid = PID_control("angular_control")
        self.dis_server = Server(dis_PIDConfig, self.cb_dis_pid,
                                 "distance_control")
        self.ang_server = Server(ang_PIDConfig, self.cb_ang_pid,
                                 "angular_control")
        self.dis_pid.setSampleTime(0.1)
        self.angu_pid.setSampleTime(0.1)
        self.dis_pid.SetPoint = 0
        self.angu_pid.SetPoint = 0

        # setup publisher
        # self.pub_v1 = rospy.Publisher("cmd_vel", Twist, queue_size=1)
        # self.sub_p3d1 = rospy.Subscriber(
        #     "p3d_odom", Odometry, self.cb_boat1_odom, queue_size=1)

        # initiallize boat status
        self.boat_odom = Odometry()
        self.yaw = 0

        # initiallize HRVO environment
        self.ws_model = dict()
        # robot radius
        self.ws_model['robot_radius'] = 1

        print os.path.dirname(__file__)
        data = pd.read_csv(
            "/home/developer/vrx/catkin_ws/src/vrx/vrx_gazebo/worlds/block_position.csv"
        )
        objs = []
        for idx, item in data.iterrows():
            objs.append([item['x'], item['y'], objs_dict[item['object']]])
        self.ws_model['circular_obstacles'] = objs

        # rectangular boundary, format [x,y,width/2,heigth/2]
        self.ws_model['boundary'] = []

        self.position = [[0, 0]]
        self.goal = self.random_goal()
        # print(self.position)
        # print(self.goal)
        self.velocity = [[0, 0]]
        self.v_max = [1]

        # timer
        # self.timer = rospy.Timer(rospy.Duration(0.2), self.cb_hrvo)

    def random_goal(self):
        # random angle
        alpha = 2 * math.pi * random.random()
        # random radius
        r = 50 * math.sqrt(2)
        # calculating coordinates
        x = r * math.cos(alpha)
        y = r * math.sin(alpha)

        return [[x, y]]

    def start_hrvo(self):
        env = gym_env.SubtCaveNobackEnv()

        epoch = 0
        iteration = 60
        record = np.zeros([iteration, 2])

        for i in range(iteration):
            start_time = time.time()
            ep_reward = 0
            step = 0
            distance = 0
            while True:

                v_des = compute_V_des(self.position, self.goal, self.v_max)
                self.velocity = RVO_update(self.position, v_des, self.velocity,
                                           self.ws_model)

                dis, angle = self.process_ang_dis(self.velocity[0][0],
                                                  self.velocity[0][1],
                                                  self.yaw)

                self.position, self.yaw, out_bound, r, done, info = env.step(
                    [dis * 0.4, angle * 0.4])

                if out_bound or done:
                    self.goal = self.random_goal()

                if done or env.total_dis > 600:
                    record[epoch][0] = env.total_dis
                    record[epoch][1] = time.time() - start_time
                    s = env.reset()
                    break
            print epoch, record[epoch]

            epoch += 1

        folder = os.getcwd()
        record_name = 'hrvo.pkl'
        fileObject = open(folder + "/" + record_name, 'wb')

        pkl.dump(record, fileObject)
        fileObject.close()

    def process_ang_dis(self, vx, vy, yaw):
        dest_yaw = math.atan2(vy, vx)

        angle = dest_yaw - yaw
        if angle > np.pi:
            angle = angle - 2 * np.pi

        if angle < -np.pi:
            angle = angle + 2 * np.pi

        angle = angle / np.pi

        dis = math.sqrt(vx**2 + vy**2)

        # print "pos      %2.2f, %2.2f" % (self.position[0][0], self.position[0][1])
        # print "goal     %2.2f, %2.2f" % (self.goal[0][0], self.goal[0][1])
        # print "dest_yaw %2.2f" % dest_yaw
        # print "yaw      %2.2f" % yaw
        # print "angle    %2.2f" % angle
        # print "dis      %2.2f\n" % dis

        dis = max(min(dis, 1), -1)
        angle = max(min(angle, 1), -1)
        return dis, angle

    def update_all(self):
        self.position = []

        # update position
        pos = [
            self.boat_odom.pose.pose.position.x,
            self.boat_odom.pose.pose.position.y
        ]
        self.position.append(pos)

        # update orientation
        quaternion = (self.boat_odom.pose.pose.orientation.x,
                      self.boat_odom.pose.pose.orientation.y,
                      self.boat_odom.pose.pose.orientation.z,
                      self.boat_odom.pose.pose.orientation.w)
        euler = tf.transformations.euler_from_quaternion(quaternion)
        self.yaw = euler[2]

    def cb_boat1_odom(self, msg):
        self.boat_odom = msg

    def cb_dis_pid(self, config, level):
        print("distance: [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.dis_pid.setKp(Kp)
        self.dis_pid.setKi(Ki)
        self.dis_pid.setKd(Kd)
        return config

    def cb_ang_pid(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.angu_pid.setKp(Kp)
        self.angu_pid.setKi(Ki)
        self.angu_pid.setKd(Kd)
        return config
Ejemplo n.º 9
0
class Robot_PID():
    def __init__(self):

        cmd_ratio = rospy.get_param("~cmd_ratio", 1)

        self.dis4constV = 1.  # Distance for constant velocity
        self.pos_ctrl_max = 1
        self.pos_ctrl_min = 0.0
        self.cmd_ctrl_max = 1 * cmd_ratio
        self.cmd_ctrl_min = -1 * cmd_ratio
        self.goal = None
        self.robot_pos = None
        self.robot_yaw = None

        rospy.loginfo("[%s] Initializing " % rospy.get_name())

        self.pub_cmd = rospy.Publisher("cmd_vel", Twist, queue_size=1)

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

        self.pos_srv = Server(pos_PIDConfig, self.pos_pid_cb, "Position")
        self.ang_srv = Server(ang_PIDConfig, self.ang_pid_cb, "Angular")

        self.initialize_PID()

        self.sub_goal = rospy.Subscriber("pid_control/goal",
                                         PoseStamped,
                                         self.cb_goal,
                                         queue_size=1)

        # sub joy to switch on or off
        self.auto = False
        self.sub_joy = rospy.Subscriber('joy_teleop/joy',
                                        Joy,
                                        self.cb_joy,
                                        queue_size=1)

        # tf v.s. Odometry => depend on user and scenario
        self.use_odom = rospy.get_param("~use_odom", True)

        self.use_tf = rospy.get_param("~use_tf", False)

        if self.use_odom:
            self.sub_pose = rospy.Subscriber('pid_control/pose',
                                             Odometry,
                                             self.cb_pose,
                                             queue_size=1)

        if self.use_tf:
            self.map_frame = rospy.get_param("~map_frame", 'map')
            self.robot_frame = rospy.get_param("~robot_frame", 'base_link')
            self.listener = tf.TransformListener()

    def cb_pose(self, msg):

        self.robot_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_yaw = yaw

    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 cb_goal(self, p):
        rospy.logwarn("PID_control_cb_goal")
        self.goal = [p.pose.position.x, p.pose.position.y]

        if self.use_tf:
            try:
                (trans_c, rot_c) = self.listener.lookupTransform(
                    self.map_frame, self.robot_frame, rospy.Time(0))
                self.robot_pos = [trans_c[0], trans_c[1]]
                _, _, self.robot_yaw = tf.transformations.euler_from_quaternion(
                    rot_c)
            except (tf.Exception, tf.LookupException, tf.ConnectivityException,
                    tf.ExtrapolationException):
                rospy.logerr("faile to catch tf %s 2 %s" %
                             (target_frame, source_frame))
                return

        if self.robot_pos is None:
            rospy.logwarn("%s : no robot pose" % rospy.get_name())
            return

        # self.robot_yaw = self.robot_yaw + np.pi/2
        goal_distance = self.get_distance(self.robot_pos, self.goal)
        goal_angle = self.get_goal_angle(self.robot_yaw, self.robot_pos,
                                         self.goal)

        pos_output, ang_output = self.control(goal_distance, goal_angle)

        if self.auto:
            cmd_msg = Twist()
            cmd_msg.linear.x = self.cmd_constarin(pos_output)
            cmd_msg.angular.z = self.cmd_constarin(ang_output)
            self.pub_cmd.publish(cmd_msg)

    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 initialize_PID(self):
        self.pos_control.setSampleTime(1)
        self.ang_control.setSampleTime(1)

        self.pos_control.SetPoint = 0.0
        self.ang_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)

    # limit the angle to the range of [-180, 180]
    def angle_range(self, angle):
        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 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 cb_joy(self, joy_msg):
        # MODE D
        start_button = 7
        back_button = 6
        # Start button
        if (joy_msg.buttons[start_button] == 1) and not self.auto:
            self.auto = True
            rospy.loginfo('go auto')
        elif joy_msg.buttons[back_button] == 1 and self.auto:
            self.auto = False
            rospy.loginfo('go manual')
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
class Robot_PID():
    def __init__(self):
        self.dis4constV = 1.  # Distance for constant velocity
        self.pos_ctrl_max = 1
        self.pos_ctrl_min = 0.0
        self.cmd_ctrl_max = 1
        self.cmd_ctrl_min = -1
        self.goal = None
        self.robot_pos = None
        self.robot_yaw = None

        rospy.loginfo("[%s] Initializing " % rospy.get_name())

        self.pub_cmd = rospy.Publisher("cmd_vel", Twist, queue_size=1)

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

        self.pos_srv = Server(pos_PIDConfig, self.pos_pid_cb, "Position")
        self.ang_srv = Server(ang_PIDConfig, self.ang_pid_cb, "Angular")

        self.initialize_PID()

        self.sub_goal = rospy.Subscriber("pid_control/goal",
                                         PoseStamped,
                                         self.cb_goal,
                                         queue_size=1)
        self.sub_pose = rospy.Subscriber('pose',
                                         PoseStamped,
                                         self.cb_pose,
                                         queue_size=1)

    def cb_pose(self, msg):
        self.robot_pos = [msg.pose.position.x, msg.pose.position.y]
        quat = (msg.pose.orientation.x, msg.pose.orientation.y,
                msg.pose.orientation.z, msg.pose.orientation.w)
        _, _, yaw = tf.transformations.euler_from_quaternion(quat)
        self.robot_yaw = yaw

    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 cb_goal(self, p):
        self.goal = [p.pose.position.x, p.pose.position.y]

        if self.robot_pos is None:
            rospy.logwarn("%s : no robot pose" % rospy.get_name())
            return

        # self.robot_yaw = self.robot_yaw + np.pi/2
        goal_distance = self.get_distance(self.robot_pos, self.goal)
        goal_angle = self.get_goal_angle(self.robot_yaw, self.robot_pos,
                                         self.goal)

        pos_output, ang_output = self.control(goal_distance, goal_angle)

        cmd_msg = Twist()
        cmd_msg.linear.x = self.cmd_constarin(pos_output)
        cmd_msg.angular.z = self.cmd_constarin(ang_output)
        self.pub_cmd.publish(cmd_msg)

    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 initialize_PID(self):
        self.pos_control.setSampleTime(1)
        self.ang_control.setSampleTime(1)

        self.pos_control.SetPoint = 0.0
        self.ang_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)

    # limit the angle to the range of [-180, 180]
    def angle_range(self, angle):
        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 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
Ejemplo n.º 12
0
class Robot_PID():
    def __init__(self):
        self.node_name = rospy.get_name()
        self.motor_mode = rospy.get_param("~motor_mode", 0)
        rospy.loginfo("Motor Mode: " + str(self.motor_mode))
        self.small_angle_thres = 0.35
        self.angle_mag = 1 / 2.5
        self.dis4constV = 5.  # Distance for constant velocity
        self.alpha_p = 1.2
        self.alpha_a = 1.3
        self.pos_ctrl_max = 3
        self.pos_ctrl_min = 0.0
        # self.alpha_v = 1.0
        # self.pos_station_max = 0.8
        # self.pos_station_min = -0.8
        self.cmd_ctrl_max = 2.5
        self.cmd_ctrl_min = -2.5
        self.station_keeping_dis = 3.5  # meters
        self.frame_id = 'map'
        self.goal = None
        self.pid_parent = ["pos", "ang", "pos_bridge", "ang_bridge"]
        self.pid_child = ["kp", "ki", "kd"]
        rospack = rospkg.RosPack()
        self.pid_param_path = os.path.join(rospack.get_path('asv_config'),
                                           "pid/pid.yaml")
        # if self.motor_mode == 0:
        # 	self.pid_param_path = os.path.join(rospack.get_path('asv_config'), "pid/pid_0.yaml")
        # elif self.motor_mode == 1:
        # 	self.pid_param_path = os.path.join(rospack.get_path('asv_config'), "pid/pid_1.yaml")
        self.pid_param = None

        with open(self.pid_param_path, 'r') as file:
            self.pid_param = yaml.safe_load(file)

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

        # self.alpha_srv = rospy.Service("alphaV", SetValue, self.alpha_cb)
        self.param_srv = rospy.Service("param", SetString, self.param_cb)

        self.pub_cmd = rospy.Publisher("cmd_drive", MotorCmd, queue_size=1)
        rospy.Subscriber('robot_goal',
                         RobotGoal,
                         self.robot_goal_cb,
                         queue_size=1,
                         buff_size=2**24)

        self.pub_goal = rospy.Publisher("goal_point", Marker, queue_size=1)

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

        self.pos_bridge_control = PID_control("Position_Bridge")
        self.ang_bridge_control = PID_control("Angular_Bridge")

        self.set_pid_param()

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

        # 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 robot_goal_cb(self, msg):
        self.goal = [msg.goal.position.x, msg.goal.position.y]
        self.robot_position = [msg.robot.position.x, msg.robot.position.y]

        if msg.goal.position.x == msg.robot.position.x and msg.goal.position.y == msg.robot.position.y:
            # if station keeping
            cmd_msg = MotorCmd()
            cmd_msg.right = 0
            cmd_msg.left = 0
            cmd_msg.horizontal = 0
            self.pub_cmd.publish(cmd_msg)
            self.publish_goal(self.goal)
            return

        quat = (msg.robot.orientation.x,\
          msg.robot.orientation.y,\
          msg.robot.orientation.z,\
          msg.robot.orientation.w)
        _, _, yaw = tf.transformations.euler_from_quaternion(quat)

        if len(self.goal) == 0 or len(
                self.robot_position
        ) == 0:  # if the robot hasn't recieve any goal
            return

        #yaw = yaw + np.pi/2
        goal_distance = self.get_distance(self.robot_position, self.goal)
        goal_angle = self.get_goal_angle(yaw, self.robot_position, self.goal)

        pos_output, ang_output = 0, 0
        if goal_distance < self.station_keeping_dis:
            # rospy.loginfo("Station Keeping")
            # pos_output, ang_output = self.station_keeping(goal_distance, goal_angle)
            if (msg.mode.data == "bridge"):
                if (abs(goal_angle) < self.small_angle_thres):
                    neg = (goal_angle < 0)
                    goal_angle = self.small_angle_thres * (
                        (abs(goal_angle) / self.small_angle_thres)**
                        self.angle_mag)
                    if neg:
                        goal_angle = -goal_angle
                pos_output, ang_output = self.bridge_control(
                    goal_distance, goal_angle)
            else:
                pos_output, ang_output = self.control(goal_distance,
                                                      goal_angle)
        else:
            if (msg.mode.data == "bridge"):
                if (abs(goal_angle) < self.small_angle_thres):
                    neg = (goal_angle < 0)
                    goal_angle = self.small_angle_thres * (
                        (abs(goal_angle) / self.small_angle_thres) *
                        self.angle_mag)
                    if neg:
                        goal_angle = -goal_angle
                pos_output, ang_output = self.bridge_control(
                    goal_distance, goal_angle)
            else:
                pos_output, ang_output = self.control(goal_distance,
                                                      goal_angle)

        cmd_msg = MotorCmd()
        if self.motor_mode == 0:  # three motors mode
            if not msg.only_angle.data:  # for navigation
                cmd_msg.right = self.cmd_constarin(pos_output + ang_output)
                cmd_msg.left = self.cmd_constarin(pos_output - ang_output)
            else:  # if only for rotation, instead of moving forward
                cmd_msg.right = self.cmd_constarin(ang_output)
                cmd_msg.left = self.cmd_constarin(ang_output)
        elif self.motor_mode == 1:  # four motors mode
            if not msg.only_angle.data:  # for navigation
                cmd_msg.right = self.cmd_constarin(pos_output)
                cmd_msg.left = self.cmd_constarin(pos_output)
                cmd_msg.horizontal = self.cmd_constarin(ang_output)
            else:  # if only for rotation, instead of moving forward
                cmd_msg.horizontal = self.cmd_constarin(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.alpha_p * (-self.pos_control.output / self.dis4constV))

        # -1 = -180/180 < output/180 < 180/180 = 1
        ang_output = self.alpha_a * (self.ang_control.output / 180.)
        return pos_output, ang_output

    def bridge_control(self, goal_distance, goal_angle):
        self.pos_bridge_control.update(goal_distance)
        self.ang_bridge_control.update(goal_angle)

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

        # -1 = -180/180 < output/180 < 180/180 = 1
        ang_output = self.alpha_a * (self.ang_bridge_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.alpha_p *
            (-self.pos_station_control.output / self.dis4constV))

        # -1 = -180/180 < output/180 < 180/180 = 1
        ang_output = self.alpha_a * (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 param_cb(self, req):
        s = req.str
        ss = s.split('/')
        string_valid = False
        if len(ss) == 3:
            if ss[0] in self.pid_parent:
                if ss[1] in self.pid_child:
                    try:
                        string_valid = True
                        self.pid_param[ss[0]][ss[1]] = float(ss[2])
                        with open(self.pid_param_path, "w") as file:
                            yaml.dump(self.pid_param, file)
                        self.set_pid_param()
                    except:
                        pass
        res = SetStringResponse()
        if string_valid:
            res.success = True
        else:
            res.success = False
        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

        self.pos_bridge_control.setSampleTime(1)
        self.ang_bridge_control.setSampleTime(1)
        # self.pos_station_control.setSampleTime(1)
        # self.ang_station_control.setSampleTime(1)

        self.pos_bridge_control.SetPoint = 0.0
        self.ang_bridge_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 = 1.5
        marker.scale.y = 1.5
        marker.scale.z = 1.5
        marker.color.a = 1.0
        marker.color.g = 1.0
        marker.color.r = 1.0
        self.pub_goal.publish(marker)

    def set_pid_param(self):
        self.pos_control.setKp(self.pid_param['pos']['kp'])
        self.pos_control.setKi(self.pid_param['pos']['ki'])
        self.pos_control.setKd(self.pid_param['pos']['kd'])
        self.ang_control.setKp(self.pid_param['ang']['kp'])
        self.ang_control.setKi(self.pid_param['ang']['ki'])
        self.ang_control.setKd(self.pid_param['ang']['kd'])

        self.pos_bridge_control.setKp(self.pid_param['pos_bridge']['kp'])
        self.pos_bridge_control.setKi(self.pid_param['pos_bridge']['ki'])
        self.pos_bridge_control.setKd(self.pid_param['pos_bridge']['kd'])
        self.ang_bridge_control.setKp(self.pid_param['ang_bridge']['kp'])
        self.ang_bridge_control.setKi(self.pid_param['ang_bridge']['ki'])
        self.ang_bridge_control.setKd(self.pid_param['ang_bridge']['kd'])
        rospy.loginfo("PID parameters:")
        print("pos: ", self.pid_param['pos'])
        print("ang: ", self.pid_param['ang'])
        print("pos_bridge: ", self.pid_param['pos_bridge'])
        print("ang_bridge: ", self.pid_param['ang_bridge'])

    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
Ejemplo n.º 13
0
class JoyMapper(object):
    def __init__(self):
        self.node_name = rospy.get_name()
        rospy.loginfo("[%s] Initializing " % (self.node_name))

        # Publications
        self.pub_motor_cmd = rospy.Publisher("motor_cmd",
                                             Motor4Cmd,
                                             queue_size=1)

        # Subscriptions
        self.sub_cmd_drive = rospy.Subscriber("cmd_drive",
                                              VelocityVector,
                                              self.cbCmd,
                                              queue_size=1)
        self.sub_joy = rospy.Subscriber("joy", Joy, self.cbJoy, queue_size=1)
        self.sub_imu = rospy.Subscriber("imu/data",
                                        Imu,
                                        self.cb_imu,
                                        queue_size=1)

        #PID
        self.ang_control = PID_control("joy_stick_Angular")
        self.ang_srv = Server(ang_PIDConfig, self.ang_pid_cb,
                              "joy_stick_Angular")
        self.ang_control.setSampleTime(0.1)
        self.ang_control.SetPoint = 0.0

        #parameter
        self.differential_constrain = rospy.get_param("differential_constrain",
                                                      0.2)

        #varibles
        self.emergencyStop = False
        self.autoMode = False
        self.rotate = 0
        self.motor_msg = Motor4Cmd()
        self.last_msg = Motor4Cmd()
        self.vector_msg = VelocityVector()
        self.imu_msg = Imu()
        self.boat_angle = 0
        self.dest_angle = 0
        self.last_msg.lf = 0
        self.last_msg.lr = 0
        self.last_msg.rf = 0
        self.last_msg.rr = 0
        self.motor_stop()

        #timer
        self.timer = rospy.Timer(rospy.Duration(0.05), self.cb_publish)

    def cb_publish(self, event):
        if self.emergencyStop:
            self.motor_stop()
            self.pub_motor_cmd.publish(self.motor_msg)

        else:  #low pass filter
            if not self.autoMode:
                if self.rotate > 0:
                    self.dest_angle += 5
                elif self.rotate < 0:
                    self.dest_angle -= 5
                self.dest_angle = self.angle_range(self.dest_angle)
                angular_input = self.angle_range(self.boat_angle -
                                                 self.dest_angle)
                angular_output = self.control(angular_input)
                print("angular out %f" % angular_output)
                self.vector_msg.angular = angular_output
                self.motor_msg = self.vector_to_cmd(self.vector_msg)
                self.motor_msg = self.msg_constrain(self.motor_msg)

            self.last_msg.lf = self.low_pass_filter(self.motor_msg.lf,
                                                    self.last_msg.lf)
            self.last_msg.lr = self.low_pass_filter(self.motor_msg.lr,
                                                    self.last_msg.lr)
            self.last_msg.rf = self.low_pass_filter(self.motor_msg.rf,
                                                    self.last_msg.rf)
            self.last_msg.rr = self.low_pass_filter(self.motor_msg.rr,
                                                    self.last_msg.rr)
            self.pub_motor_cmd.publish(self.last_msg)

    def low_pass_filter(self, current, last):
        current = last + max(min(current - last, self.differential_constrain),
                             -self.differential_constrain)
        return current

    def control(self, head_angle):
        self.ang_control.update(head_angle)
        ang_output = self.ang_control.output / (-180.)
        return ang_output

    def cbCmd(self, cmd_msg):
        if not self.emergencyStop and self.autoMode:
            msg = self.vector_to_cmd(cmd_msg)
            self.motor_msg = self.msg_constrain(msg)

    def cbJoy(self, joy_msg):
        self.processButtons(joy_msg)
        if not self.emergencyStop and not self.autoMode:
            self.joy = joy_msg
            self.vector_msg = VelocityVector()
            self.vector_msg.x = self.joy.axes[0] * -1
            self.vector_msg.y = self.joy.axes[1]
            if self.joy.axes[3] > 0:
                self.rotate = 1
            elif self.joy.axes[3] < 0:
                self.rotate = -1
            else:
                self.rotate = 0
            self.vector_msg.angular = 0

    def cb_imu(self, msg):
        self.imu_msg = msg
        quat = (msg.orientation.x, msg.orientation.y, msg.orientation.z,
                msg.orientation.w)
        _, _, yaw = tf.transformations.euler_from_quaternion(quat)
        self.boat_angle = np.degrees(yaw)

    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 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 motor_stop(self):
        self.motor_msg.lf = 0
        self.motor_msg.lr = 0
        self.motor_msg.rf = 0
        self.motor_msg.rr = 0

    def vector_to_cmd(self, vec):
        motor4msg = Motor4Cmd()
        motor4msg.lf = max(min(vec.y + vec.x, 1), -1) + vec.angular
        motor4msg.lr = max(min(vec.y - vec.x, 1), -1) + vec.angular
        motor4msg.rf = max(min(vec.y - vec.x, 1), -1) - vec.angular
        motor4msg.rr = max(min(vec.y + vec.x, 1), -1) - vec.angular
        return motor4msg

    def msg_constrain(self, msg):
        new_msg = Motor4Cmd()
        new_msg.lf = max(min(msg.lf, 1), -1)
        new_msg.lr = max(min(msg.lr, 1), -1)
        new_msg.rf = max(min(msg.rf, 1), -1)
        new_msg.rr = max(min(msg.rr, 1), -1)
        return new_msg

    def processButtons(self, joy_msg):
        # Button A
        if (joy_msg.buttons[0] == 1):
            rospy.loginfo('A button')

        # Y button
        elif (joy_msg.buttons[3] == 1):
            rospy.loginfo('Y button')

        # Left back button
        elif (joy_msg.buttons[4] == 1):
            rospy.loginfo('left back button')

        # Right back button
        elif (joy_msg.buttons[5] == 1):
            rospy.loginfo('right back button')

        # Back button
        elif (joy_msg.buttons[6] == 1):
            rospy.loginfo('back button')

        # Start button
        elif (joy_msg.buttons[7] == 1):
            self.autoMode = not self.autoMode
            if self.autoMode:
                rospy.loginfo('going auto')
            else:
                rospy.loginfo('going manual')

        # Power/middle button
        elif (joy_msg.buttons[8] == 1):
            self.emergencyStop = not self.emergencyStop
            if self.emergencyStop:
                rospy.loginfo('emergency stop activate')
                self.motor_stop()
            else:
                rospy.loginfo('emergency stop release')
        # Left joystick button
        elif (joy_msg.buttons[9] == 1):
            rospy.loginfo('left joystick button')

        else:
            some_active = sum(joy_msg.buttons) > 0
            if some_active:
                rospy.loginfo('No binding for joy_msg.buttons = %s' %
                              str(joy_msg.buttons))

    def on_shutdown(self):
        self.motor_stop()
        self.pub_motor_cmd.publish(self.motor_msg)
        rospy.loginfo("shutting down [%s]" % (self.node_name))