Esempio n. 1
0
    def test(self):
        self.position = Vector(-8, 4)
        self.heading = Vector(0, 1)
        self.plot.addPoint(self.position)

        for i in range(8):
            (next, next_next) = self.addRightTurn(self.position, self.heading)
            self.plot.addLine(self.position, next)
            self.plot.addLine(next, next_next)
            self.plot.addPoint(next)
            self.plot.addPoint(next_next)

            self.heading = -self.heading
            self.position = next_next

            (next, next_next) = self.addLeftTurn(self.position, self.heading)
            self.plot.addLine(self.position, next)
            self.plot.addLine(next, next_next)
            self.plot.addPoint(next)
            self.plot.addPoint(next_next)

            self.heading = -self.heading
            self.position = next_next


#        self.plot.addVector(next_point,next_heading)

        self.plot.show()
Esempio n. 2
0
    def __init__(self):
        # Task parameters
        self.running = True

        # Class variables
        self.origin = Vector(0, 0)
        self.current = Vector(0, 0)
        self.position_list = []

        # init plot
        self.fig = pyplot.figure(num=None,
                                 figsize=(8, 8),
                                 dpi=80,
                                 facecolor='w',
                                 edgecolor='k')
        self.area = 2
        pyplot.axis('equal')
        pyplot.grid()
        ion()

        # Init transform
        self.tf = tf.TransformListener()
        self.quaternion = np.empty((4, ), dtype=np.float64)

        # Init node
        self.rate = rospy.Rate(10)
Esempio n. 3
0
    def __init__(self, file):
        # Task parameters
        self.running = True

        # Class variables
        self.origin = Vector(0, 0)
        self.position = Vector(0, 0)
        self.position_list = []

        # init plot
        self.fig = pyplot.figure(num=None,
                                 figsize=(8, 8),
                                 dpi=80,
                                 facecolor='w',
                                 edgecolor='k')
        self.area = 2
        ion()

        # Init transform
        self.tf = tf.TransformListener()
        self.br = tf.TransformBroadcaster()
        self.quaternion = np.empty((4, ), dtype=np.float64)

        # Init node
        self.rate = rospy.Rate(10)

        # Init subscriber
        self.imu_sub = rospy.Subscriber('/fmInformation/imu', Imu, self.onImu)

        # Init stat
        self.file = file
        self.deviations = []
Esempio n. 4
0
    def __init__(self):
        # Init line
        self.rabbit_factor = 0.2
        self.line_begin = Vector(0, 0)
        self.line_end = Vector(0, 5)
        self.line = Vector(self.line_end[0] - self.line_begin[0], self.line_end[1] - self.line_begin[1])
        self.yaw = 0.0

        # Init control methods
        self.isNewGoalAvailable = self.empty_method()
        self.isPreemptRequested = self.empty_method()
        self.setPreempted = self.empty_method()

        # Get parameters from parameter server
        self.getParameters()

        # Set up markers for rviz
        self.point_marker = MarkerUtility("/point_marker", self.odom_frame)
        self.line_marker = MarkerUtility("/line_marker", self.odom_frame)
        self.pos_marker = MarkerUtility("/pos_marker", self.odom_frame)

        # Init velocity control
        self.controller = Controller()
        self.distance_to_goal = 0
        self.angle_error = 0
        self.goal_angle_error = 0
        self.sp_linear = 0
        self.sp_angular = 0
        self.distance_to_line = 0

        # Init TF listener
        self.__listen = TransformListener()

        # Init planner
        self.corrected = False
        self.rate = rospy.Rate(1 / self.period)
        self.twist = TwistStamped()
        self.quaternion = np.empty((4,), dtype=np.float64)

        # Init vectors
        self.destination = Vector(1, 0)
        self.position = Vector(1, 0)  # Vector from origo to current position of the robot
        self.heading = Vector(1, 0)  # Vector in the current direction of the robot
        self.rabbit = Vector(1, 0)  # Vector from origo to the aiming point on the line
        self.rabbit_path = Vector(1, 0)  # Vector from current position to aiming point on the line
        self.perpendicular = Vector(1, 0)  # Vector from current position to the line perpendicular to the line
        self.projection = Vector(1, 0)  # Projection of the position vector on the line

        # Set up circular buffer for filter
        self.zone_filter = [self.z3_value] * self.z_filter_size
        self.zone = 2
        self.z_ptr = 0

        # Setup Publishers and subscribers
        if not self.use_tf:
            self.odom_sub = rospy.Subscriber(self.odometry_topic, Odometry, self.onOdometry)
        self.twist_pub = rospy.Publisher(self.cmd_vel_topic, TwistStamped)
Esempio n. 5
0
 def updateFeedback(self):
     try:
         (position,
          heading) = self.__listen.lookupTransform(self.frame_id,
                                                   self.world_frame,
                                                   rospy.Time(0))
         (roll, pitch,
          yaw) = tf.transformations.euler_from_quaternion(heading)
         self.controller.setFeedback(Vector(position[0], position[1]),
                                     Vector(math.cos(yaw), math.sin(yaw)))
     except (tf.LookupException, tf.ConnectivityException), err:
         rospy.loginfo("could not locate vehicle")
Esempio n. 6
0
 def show(self):
     #        print(sum(self.lin_vel)/len(self.lin_vel))
     #        print(sum(self.ang_vel)/len(self.ang_vel))
     self.plot.addVector(
         self.position,
         Vector(math.cos(self.heading), math.sin(self.heading)))
     self.plot.addPoint(self.position)
     self.plot.show()
Esempio n. 7
0
 def spin(self):
     self.plot.addVector(
         self.position,
         Vector(math.cos(self.heading), math.sin(self.heading)))
     while not self.targetReached() and self.steps < self.max_steps:
         self.plot.addPose(self.position)
         self.step()
         self.updatePos()
     self.plot.addPose(self.position)
Esempio n. 8
0
 def update(self):
     try:
         (position,
          orientation) = self.tf.lookupTransform("world_frame",
                                                 "mast_bottom",
                                                 rospy.Time(0))
         self.position = Vector(position[0], position[1])
     except (tf.LookupException, tf.ConnectivityException,
             tf.Exception), err:
         rospy.loginfo("Transform error: %s", err)
Esempio n. 9
0
    def positionPlanner(self, goal):
        self.destination[0] = goal[0]
        self.destination[1] = goal[1]

        # Construct desired path vector and calculate distance error
        path = self.destination - Vector(self.position[0], self.position[1])

        # Calculate distance error
        self.distance_error = path.length()

        # Construct heading vector
        head = Vector(math.cos(self.heading), math.sin(self.heading))

        # Calculate angle between heading vector and path vector
        self.angle_error = head.angle(path)

        # Rotate the heading vector according to the calculated angle and test correspondence
        # with the path vector. If not zero sign must be flipped. This is to avoid the sine trap.
        t1 = head.rotate(self.angle_error)
        if path.angle(t1) != 0:
            self.angle_error = -self.angle_error

        # Generate twist from distance and angle errors (For now simply 1:1)
        self.linear_velocity = self.distance_error * self.linear_scale_factor
        self.angular_velocity = self.angle_error * self.angular_scale_factor

        if math.fabs(self.angle_error) > self.max_angle_error:
            self.linear_velocity *= self.retarder

        # Implement maximum linear_velocity velocity and maximum angular_velocity velocity
        if self.linear_velocity > self.max_linear_velocity:
            self.linear_velocity = self.max_linear_velocity
        if self.linear_velocity < -self.max_linear_velocity:
            self.linear_velocity = -self.max_linear_velocity
        if self.angular_velocity > self.max_angular_velocity:
            self.angular_velocity = self.max_angular_velocity
        if self.angular_velocity < -self.max_angular_velocity:
            self.angular_velocity = -self.max_angular_velocity
Esempio n. 10
0
    def positionPlanner(self,goal):
        self.destination[0] = goal[0]
        self.destination[1] = goal[1]
        
        # Construct desired path vector and calculate distance error
        path = self.destination - Vector(self.position[0], self.position[1])
        
        # Calculate distance error     
        self.distance_error = path.length()

        # Construct heading vector
        head = Vector(math.cos(self.heading), math.sin(self.heading))
                
        # Calculate angle between heading vector and path vector
        self.angle_error = head.angle(path)

        # Rotate the heading vector according to the calculated angle and test correspondence
        # with the path vector. If not zero sign must be flipped. This is to avoid the sine trap.
        t1 = head.rotate(self.angle_error)
        if path.angle(t1) != 0 :
            self.angle_error = -self.angle_error          
        
        # Generate twist from distance and angle errors (For now simply 1:1)
        self.linear_velocity = self.distance_error * self.linear_scale_factor
        self.angular_velocity = self.angle_error * self.angular_scale_factor
        
        if math.fabs(self.angle_error) > self.max_angle_error :
            self.linear_velocity *= self.retarder
            
        # Implement maximum linear_velocity velocity and maximum angular_velocity velocity
        if self.linear_velocity > self.max_linear_velocity:
            self.linear_velocity = self.max_linear_velocity
        if self.linear_velocity < -self.max_linear_velocity:
            self.linear_velocity = -self.max_linear_velocity
        if self.angular_velocity > self.max_angular_velocity:
            self.angular_velocity = self.max_angular_velocity
        if self.angular_velocity < -self.max_angular_velocity:
            self.angular_velocity = -self.max_angular_velocity
Esempio n. 11
0
    def __init__(self):
        # Init control loop
        self.twist = TwistStamped()
        self.lin_err = 0.0
        self.ang_err = 0.0
        self.lin_prev_err = 0.0
        self.ang_prev_err = 0.0
        self.lin_int = 0.0
        self.ang_int = 0.0
        self.lin_diff = 0.0
        self.ang_diff = 0.0
        self.fb_linear = 0.0
        self.fb_angular = 0.0

        # Get parameters
        self.period = rospy.get_param("~period", 0.1)
        self.lin_p = rospy.get_param("~lin_p", 0.4)
        self.lin_i = rospy.get_param("~lin_i", 0.6)
        self.lin_d = rospy.get_param("~lin_d", 0.0)
        self.ang_p = rospy.get_param("~ang_p", 0.8)
        self.ang_i = rospy.get_param("~ang_i", 0.1)
        self.ang_d = rospy.get_param("~ang_d", 0.05)
        self.int_max = rospy.get_param("~integrator_max", 0.1)
        self.filter_size = rospy.get_param("~filter_size", 10)
        self.max_linear_velocity = rospy.get_param("~max_linear_velocity", 2)
        self.max_angular_velocity = rospy.get_param("~max_angular_velocity", 1)
        self.use_dynamic_reconfigure = rospy.get_param(
            "~use_dynamic_reconfigure", False)
        self.linear_vel = [0.0] * self.filter_size
        self.angular_vel = [0.0] * self.filter_size
        self.time = 0.0
        self.last_entry = rospy.Time.now()
        self.last_cl_entry = rospy.Time.now()
        self.distance = 0.0
        self.last_position = Vector(1, 0)
        self.angle = 0.0
        self.last_heading = Vector(1, 0)
        self.ptr = 0
Esempio n. 12
0
    def __init__(self):
        # Task parameters
        self.line_begin = Vector(-2, -4)
        self.line_end = Vector(2, 5)
        self.line = Vector(self.line_end[0] - self.line_begin[0],
                           self.line_end[1] - self.line_begin[1])

        self.position = Vector(0, -6)
        self.heading = math.pi / 6

        # Robot parameters
        self.linear_velocity = 0.0
        self.angular_velocity = 0.0
        self.max_linear_velocity = 2  #m/s
        self.max_angular_velocity = 2  #rad/s

        # General planner parameters
        self.target_radius = 0.1
        self.max_angle_error = 0.1
        self.retarder = 0.1

        # Line planner parameters
        self.rabbit_factor = 0.2

        # Position planner parameters
        self.linear_scale_factor = 1
        self.angular_scale_factor = 2

        # Simulation parameters
        self.max_steps = 1000
        self.stepsize = 0.1  # sec/step

        # Init simulation
        self.steps = 0
        self.lin_vel = []
        self.ang_vel = []

        # init plot
        self.plot = Vectorplot(-10, 10, -10, 10)
        #        self.plot.addPoint(self.line_begin)
        #        self.plot.addPoint(self.line_end)
        #        self.plot.addLine(self.line_begin,self.line_end)

        # init position planner
        self.destination = Vector(self.line_end[0], self.line_end[1])
        path = self.destination - Vector(self.position[0], self.position[1])
        self.distance_error = path.length()
        self.angle_error = 0.0
Esempio n. 13
0
 def __init__(self):      
     # Init control loop
     self.twist = TwistStamped()
     self.lin_err = 0.0
     self.ang_err = 0.0
     self.lin_prev_err = 0.0
     self.ang_prev_err = 0.0
     self.lin_int = 0.0
     self.ang_int = 0.0
     self.lin_diff = 0.0
     self.ang_diff = 0.0
     self.fb_linear = 0.0
     self.fb_angular = 0.0   
     
     # Get parameters
     self.period = rospy.get_param("~period",0.1)
     self.lin_p = rospy.get_param("~lin_p",0.4)
     self.lin_i = rospy.get_param("~lin_i",0.6)
     self.lin_d = rospy.get_param("~lin_d",0.0)
     self.ang_p = rospy.get_param("~ang_p",0.8)
     self.ang_i = rospy.get_param("~ang_i",0.1)
     self.ang_d = rospy.get_param("~ang_d",0.05)
     self.int_max = rospy.get_param("~integrator_max",0.1)   
     self.filter_size = rospy.get_param("~filter_size",10) 
     self.max_linear_velocity = rospy.get_param("~max_linear_velocity",2)
     self.max_angular_velocity = rospy.get_param("~max_angular_velocity",1)
     self.use_dynamic_reconfigure = rospy.get_param("~use_dynamic_reconfigure",False)
     self.linear_vel = [0.0] * self.filter_size
     self.angular_vel = [0.0] * self.filter_size
     self.time = 0.0
     self.last_entry = rospy.Time.now()
     self.last_cl_entry = rospy.Time.now()
     self.distance = 0.0
     self.last_position = Vector(1,0)
     self.angle = 0.0
     self.last_heading = Vector(1,0)
     self.ptr = 0
Esempio n. 14
0
    def update(self):
        # Get current pose and send it to controller
        self.get_current_position()
        self.heading = Vector(math.cos(self.yaw), math.sin(self.yaw))
        self.controller.setFeedback(self.position, self.heading)

        # Construct projection vector, perpendicular vector path vectors and rabbit vector
        self.projection = (self.position - self.line_begin).projectedOn(self.line)
        if self.projection.angle(self.line) > 0.1 or self.projection.angle(self.line) < -0.1:
            self.projection = self.projection.scale(-1)
        self.perpendicular = self.projection - self.position + self.line_begin
        self.goal_path = self.line_end - self.position
        self.rabbit = self.line - self.projection
        self.rabbit = self.rabbit.scale(self.rabbit_factor)
        self.rabbit += self.line_begin
        self.rabbit += self.projection
        self.rabbit_path = self.rabbit - Vector(self.position[0], self.position[1])

        # Calculate distances
        self.distance_to_goal = self.goal_path.length()
        self.distance_to_line = self.perpendicular.length()

        # Calculate angle between heading vector and goal/rabbit path vector
        self.angle_error = self.heading.angle(self.rabbit_path)
        # Rotate the heading vector according to the calculated angle and test correspondence
        # with the path vector. If not zero sign must be flipped. This is to avoid the sine trap.
        t1 = self.heading.rotate(self.angle_error)
        if self.rabbit_path.angle(t1) != 0:
            self.angle_error = -self.angle_error

        self.goal_angle_error = self.heading.angle(self.goal_path)
        # Avoid the sine trap.
        t1 = self.heading.rotate(self.goal_angle_error)
        if self.goal_path.angle(t1) != 0:
            self.goal_angle_error = -self.goal_angle_error

        # Determine zone
        if self.distance_to_line < self.z1_max_distance and math.fabs(self.angle_error) < self.z1_max_angle:
            self.zone_filter[self.z_ptr] = self.z1_value
        elif self.distance_to_line < self.z2_max_distance and math.fabs(self.angle_error) < self.z2_max_angle:
            self.zone_filter[self.z_ptr] = self.z2_value
        else:
            self.zone_filter[self.z_ptr] = self.z3_value

        self.z_ptr = self.z_ptr + 1
        if self.z_ptr >= self.z_filter_size:
            self.z_ptr = 0

        self.zone = sum(self.zone_filter) / len(self.zone_filter)
        if self.zone < (self.z1_value + ((self.z2_value - self.z1_value) / 2)):
            self.corrected = True
            self.rabbit_factor = self.z1_rabbit
            self.max_linear_velocity = self.z1_lin_vel
            self.max_angular_velocity = self.z1_ang_vel
        elif self.zone < (self.z2_value + ((self.z3_value - self.z2_value) / 2)):
            self.rabbit_factor = self.z2_rabbit
            self.max_linear_velocity = self.z2_lin_vel
            self.max_angular_velocity = self.z2_ang_vel
        else:
            self.rabbit_factor = self.z3_rabbit
            self.max_linear_velocity = self.z3_lin_vel
            self.max_angular_velocity = self.z3_ang_vel

        if self.distance_to_goal < 3 * self.max_distance_error:
            self.max_linear_velocity *= self.distance_to_goal
        elif self.distance_to_goal < self.z4_distance_to_target:
            self.rabbit_factor = 1
            self.max_linear_velocity = self.max_linear_velocity / (
                self.max_linear_velocity + (self.z4_distance_to_target - self.distance_to_goal) ** 2
            )
            self.max_angular_velocity = self.z3_ang_vel
        elif not self.corrected:
            self.max_linear_velocity *= self.retarder
            self.rabbit_factor = self.z3_rabbit
Esempio n. 15
0
    def __init__(self):
        # Init line
        self.rabbit_factor = 0.2
        self.line_begin = Vector(0, 0)
        self.line_end = Vector(0, 5)
        self.line = Vector(self.line_end[0] - self.line_begin[0],
                           self.line_end[1] - self.line_begin[1])
        self.yaw = 0.0

        # Init control methods
        self.isNewGoalAvailable = self.empty_method()
        self.isPreemptRequested = self.empty_method()
        self.setPreempted = self.empty_method()

        # Init velocity control
        self.controller = Controller()
        self.distance_to_goal = 0
        self.angle_error = 0
        self.goal_angle_error = 0
        self.sp_linear = 0
        self.sp_angular = 0
        self.distance_to_line = 0

        # Get parameters from parameter server
        self.getParameters()

        # Set up markers for rviz
        self.point_marker = MarkerUtility("/point_marker", self.odom_frame)
        self.line_marker = MarkerUtility("/line_marker", self.odom_frame)
        self.pos_marker = MarkerUtility("/pos_marker", self.odom_frame)

        # Init TF listener
        self.__listen = TransformListener()

        # Init planner
        self.corrected = False
        self.rate = rospy.Rate(1 / self.period)
        self.twist = TwistStamped()
        self.quaternion = np.empty((4, ), dtype=np.float64)

        # Init vectors
        self.destination = Vector(1, 0)
        self.position = Vector(
            1, 0)  # Vector from origo to current position of the robot
        self.heading = Vector(
            1, 0)  # Vector in the current direction of the robot
        self.rabbit = Vector(
            1, 0)  # Vector from origo to the aiming point on the line
        self.rabbit_path = Vector(
            1, 0)  # Vector from current position to aiming point on the line
        self.perpendicular = Vector(
            1, 0
        )  # Vector from current position to the line perpendicular to the line
        self.projection = Vector(
            1, 0)  # Projection of the position vector on the line

        # Set up circular buffer for filter
        self.zone_filter = [self.z3_value] * int(self.z_filter_size)
        self.zone = 2
        self.z_ptr = 0

        # Setup Publishers and subscribers
        if not self.use_tf:
            self.odom_sub = rospy.Subscriber(self.odometry_topic, Odometry,
                                             self.onOdometry)
        self.twist_pub = rospy.Publisher(self.cmd_vel_topic, TwistStamped)

        # Setup dynamic reconfigure
        if self.use_dynamic_reconfigure:
            self.reconfigure_server = Server(ParametersConfig,
                                             self.reconfigure_cb)
Esempio n. 16
0
class LinePlanner():
    """
        Controller class taking line goals and generating twist messages.
        The planner is based on the concept of a rabbit, an aiming point on the line between the robot and the target. 
        If the rabbit is close to the robot, it will follow the line close, but regulate much and if the rabbit is far 
        away the robot will drive steady, but might be off the line. 
        
        The planner sets up the following zones and acts accordingly:

        zone 1:    Low distance to line and low angle error
                   Everything is good so high linear velocity, low angular velocity and a rabbit far away
        zone 2:    Low distance to line, but higher angle error or low angle error but higher distance to line
                   Trying to correct heading so low linear velocity, normal angular velocity and medium rabbit
        zone 3:    High distance to line
                   This is going bad so low linear velocity, high angular velocity and a close rabbit
        zone 4:    Close to target
                   The line matters no more. Low linear velocity, high angular velocity and rabbit fixed on target.

        The filter controls when to change between the zones. The value assigned to each zone is not important, but the
        ratios between them is a measure of how conservative the planner is. Think of it as a measure of the transition states.
        Small transitions happens faster and large transitions happens slower.
        
        Examples:
        1,2,3 :    The planner will change quickly between zones making it very dynamic but less accurate
        1,5,25 :   The planner will change slowly between zones making it more accurate, but slow 
        1,15,25 :  The planner will be even more accurate and even slower
        1,2,25 :   The planner will be faster, but if the robot is slow, it will go slalom
        1,10,100 : Slower transitions is good for a slowly reacting robot
        
        The filter size acts as a low pass filter so higher filter size means slower reaction, but more robust to noisy sensors
        """
    def __init__(self):
        # Init line
        self.rabbit_factor = 0.2
        self.line_begin = Vector(0, 0)
        self.line_end = Vector(0, 5)
        self.line = Vector(self.line_end[0] - self.line_begin[0],
                           self.line_end[1] - self.line_begin[1])
        self.yaw = 0.0

        # Init control methods
        self.isNewGoalAvailable = self.empty_method()
        self.isPreemptRequested = self.empty_method()
        self.setPreempted = self.empty_method()

        # Init velocity control
        self.controller = Controller()
        self.distance_to_goal = 0
        self.angle_error = 0
        self.goal_angle_error = 0
        self.sp_linear = 0
        self.sp_angular = 0
        self.distance_to_line = 0

        # Get parameters from parameter server
        self.getParameters()

        # Set up markers for rviz
        self.point_marker = MarkerUtility("/point_marker", self.odom_frame)
        self.line_marker = MarkerUtility("/line_marker", self.odom_frame)
        self.pos_marker = MarkerUtility("/pos_marker", self.odom_frame)

        # Init TF listener
        self.__listen = TransformListener()

        # Init planner
        self.corrected = False
        self.rate = rospy.Rate(1 / self.period)
        self.twist = TwistStamped()
        self.quaternion = np.empty((4, ), dtype=np.float64)

        # Init vectors
        self.destination = Vector(1, 0)
        self.position = Vector(
            1, 0)  # Vector from origo to current position of the robot
        self.heading = Vector(
            1, 0)  # Vector in the current direction of the robot
        self.rabbit = Vector(
            1, 0)  # Vector from origo to the aiming point on the line
        self.rabbit_path = Vector(
            1, 0)  # Vector from current position to aiming point on the line
        self.perpendicular = Vector(
            1, 0
        )  # Vector from current position to the line perpendicular to the line
        self.projection = Vector(
            1, 0)  # Projection of the position vector on the line

        # Set up circular buffer for filter
        self.zone_filter = [self.z3_value] * int(self.z_filter_size)
        self.zone = 2
        self.z_ptr = 0

        # Setup Publishers and subscribers
        if not self.use_tf:
            self.odom_sub = rospy.Subscriber(self.odometry_topic, Odometry,
                                             self.onOdometry)
        self.twist_pub = rospy.Publisher(self.cmd_vel_topic, TwistStamped)

        # Setup dynamic reconfigure
        if self.use_dynamic_reconfigure:
            self.reconfigure_server = Server(ParametersConfig,
                                             self.reconfigure_cb)

    def getParameters(self):
        # Get topics and transforms
        self.cmd_vel_topic = rospy.get_param("~cmd_vel_topic",
                                             "/fmSignals/cmd_vel")
        self.odom_frame = rospy.get_param("~odom_frame", "/world")
        self.odometry_topic = rospy.get_param("~odometry_topic",
                                              "/fmKnowledge/odom")
        self.base_frame = rospy.get_param("~base_frame", "/base_footprint")
        self.use_tf = rospy.get_param("~use_tf", True)
        self.use_dynamic_reconfigure = rospy.get_param(
            "~use_dynamic_reconfigure", False)

        # Get general parameters
        self.period = rospy.get_param("~period", 0.1)
        self.max_linear_velocity = rospy.get_param("~max_linear_velocity", 2)
        self.max_angular_velocity = rospy.get_param("~max_angular_velocity", 1)
        self.max_distance_error = rospy.get_param("~max_distance_error", 0.05)

        # Get control parameters
        self.retarder = rospy.get_param("~retarder", 0.8)

        # Get zone parameters
        self.z1_value = rospy.get_param("~zone1_value", 1)
        self.z1_lin_vel = rospy.get_param("~zone1_linear_velocity", 0.1)
        self.z1_ang_vel = rospy.get_param("~zone1_angular_velocity", 0.2)
        self.z1_rabbit = rospy.get_param("~zone1_rabbit_factor", 0.8)
        self.z1_max_distance = rospy.get_param("~zone1_max_distance_to_line",
                                               0.05)
        self.z1_max_angle = rospy.get_param("~zone1_max_angle_error",
                                            math.pi / 36)

        self.z2_value = rospy.get_param("~zone2_value", 5)
        self.z2_lin_vel = rospy.get_param("~zone2_linear_velocity", 0.1)
        self.z2_ang_vel = rospy.get_param("~zone2_angular_velocity", 0.8)
        self.z2_rabbit = rospy.get_param("~zone2_rabbit_factor", 0.7)
        self.z2_max_distance = rospy.get_param("~zone2_max_distance_to_line",
                                               0.25)
        self.z2_max_angle = rospy.get_param("~zone2_max_angle_error",
                                            math.pi / 6)

        self.z3_value = rospy.get_param("~zone3_value", 10)
        self.z3_lin_vel = rospy.get_param("~zone3_linear_velocity", 0.1)
        self.z3_ang_vel = rospy.get_param("~zone3_angular_velocity", 1.2)
        self.z3_rabbit = rospy.get_param("~zone3_rabbit_factor", 0.6)

        self.z4_distance_to_target = rospy.get_param(
            "~zone4_distance_to_target", 0.4)
        self.z_filter_size = rospy.get_param("~transition_filter_size", 10)

    def reconfigure_cb(self, config, level):
        self.max_distance_error = config['max_distance_error']
        self.retarder = config['retarder']
        self.z1_value = config['zone1_value']
        self.z1_lin_vel = config['zone1_linear_velocity']
        self.z1_ang_vel = config['zone1_angular_velocity']
        self.z1_rabbit = config['zone1_rabbit_factor']
        self.z1_max_distance = config['zone1_max_distance_to_line']
        self.z1_max_angle = config['zone1_max_angle_error']
        self.z2_value = config['zone2_value']
        self.z2_lin_vel = config['zone2_linear_velocity']
        self.z2_ang_vel = config['zone2_angular_velocity']
        self.z2_rabbit = config['zone2_rabbit_factor']
        self.z2_max_distance = config['zone2_max_distance_to_line']
        self.z2_max_angle = config['zone2_max_angle_error']
        self.z3_value = config['zone3_value']
        self.z3_lin_vel = config['zone3_linear_velocity']
        self.z3_ang_vel = config['zone3_angular_velocity']
        self.z3_rabbit = config['zone3_rabbit_factor']
        self.z4_distance_to_target = config['zone4_distance_to_target']

        self.controller.reconfigure_cb(config, level)
        return config

    def stop(self):
        # Publish a zero twist to stop the robot
        self.twist.header.stamp = rospy.Time.now()
        self.sp_angular = 0
        self.twist.twist.angular.z = 0
        self.twist_pub.publish(self.twist)

    def execute(self, goal):
        # Construct a vector from line goal
        self.line_begin[0] = goal.a_x
        self.line_begin[1] = goal.a_y
        self.line_end[0] = goal.b_x
        self.line_end[1] = goal.b_y
        self.line = Vector(self.line_end[0] - self.line_begin[0],
                           self.line_end[1] - self.line_begin[1])
        rospy.loginfo(rospy.get_name() + " Received goal: (%f,%f) to (%f,%f) ",
                      goal.a_x, goal.a_y, goal.b_x, goal.b_y)

        # Clear filter
        self.zone_filter = [self.z3_value] * self.z_filter_size
        self.corrected = False
        self.target_area = False

        while not rospy.is_shutdown():

            # Check for new goal
            if self.isNewGoalAvailable():
                rospy.loginfo(rospy.get_name() + "New goal is available")
                break

            # Preemption check
            if self.isPreemptRequested():
                rospy.loginfo(rospy.get_name() + "Preempt requested")
                break

            # Update vectors
            self.update()

            # Publish markers for rviz
            self.line_marker.updateLine([
                Point(self.line_begin[0], self.line_begin[1], 0),
                Point(self.line_end[0], self.line_end[1], 0)
            ])
            self.point_marker.updatePoint(
                [Point(self.rabbit[0], self.rabbit[1], 0)])
            self.pos_marker.updatePoint(
                [Point(self.position[0], self.position[1], 0)])

            # If the goal is unreached
            if self.distance_to_goal > self.max_distance_error:
                # Spin the loop
                self.control_loop()

                # Block
                try:
                    self.rate.sleep()
                except rospy.ROSInterruptException:
                    print("Interrupted during sleep")
                    return 'preempted'
            else:
                # Success - position has been reached
                rospy.loginfo(
                    rospy.get_name() + " Goal reached in distance: %f m",
                    self.distance_to_goal)
                self.stop()
                break

        # Return statement
        if self.isPreemptRequested():
            self.setPreempted()
            print("Returning due to preemption")
            return 'preempted'
        elif rospy.is_shutdown():
            self.setPreempted()
            print("Returning due to abort")
            return 'aborted'
        else:
            self.setSucceeded()
            print("Returning due to success")
            return 'succeeded'

    def update(self):
        # Get current pose and send it to controller
        self.get_current_position()
        self.heading = Vector(math.cos(self.yaw), math.sin(self.yaw))
        self.controller.setFeedback(self.position, self.heading)

        # Construct projection vector, perpendicular vector path vectors and rabbit vector
        self.projection = (self.position - self.line_begin).projectedOn(
            self.line)
        if self.projection.angle(self.line) > 0.1 or self.projection.angle(
                self.line) < -0.1:
            self.projection = self.projection.scale(-1)
        self.perpendicular = self.projection - self.position + self.line_begin
        self.goal_path = self.line_end - self.position
        self.rabbit = self.line - self.projection
        self.rabbit = self.rabbit.scale(self.rabbit_factor)
        self.rabbit += self.line_begin
        self.rabbit += self.projection
        self.rabbit_path = self.rabbit - Vector(self.position[0],
                                                self.position[1])

        # Calculate distances
        self.distance_to_goal = self.goal_path.length()
        self.distance_to_line = self.perpendicular.length()

        # Calculate angle between heading vector and goal/rabbit path vector
        self.angle_error = self.heading.angle(self.rabbit_path)
        # Rotate the heading vector according to the calculated angle and test correspondence
        # with the path vector. If not zero sign must be flipped. This is to avoid the sine trap.
        t1 = self.heading.rotate(self.angle_error)
        if self.rabbit_path.angle(t1) != 0:
            self.angle_error = -self.angle_error

        self.goal_angle_error = self.heading.angle(self.goal_path)
        # Avoid the sine trap.
        t1 = self.heading.rotate(self.goal_angle_error)
        if self.goal_path.angle(t1) != 0:
            self.goal_angle_error = -self.goal_angle_error
        print("Errors (to goal, to line, angle) : (" +
              str(self.distance_to_goal) + " , " + str(self.distance_to_line) +
              " , " + str(self.angle_error) + ")")
        # Determine zone
        if self.distance_to_line < self.z1_max_distance and math.fabs(
                self.angle_error) < self.z1_max_angle:
            self.zone_filter[self.z_ptr] = self.z1_value
        elif self.distance_to_line < self.z2_max_distance and math.fabs(
                self.angle_error) < self.z2_max_angle:
            self.zone_filter[self.z_ptr] = self.z2_value
        else:
            self.zone_filter[self.z_ptr] = self.z3_value

        self.z_ptr = self.z_ptr + 1
        if self.z_ptr >= self.z_filter_size:
            self.z_ptr = 0

        self.zone = (sum(self.zone_filter) / len(self.zone_filter))

        if self.zone < (self.z1_value + ((self.z2_value - self.z1_value) / 2)):
            self.corrected = True
            self.rabbit_factor = self.z1_rabbit
            self.max_linear_velocity = self.z1_lin_vel
            self.max_angular_velocity = self.z1_ang_vel
        elif self.zone < (self.z2_value +
                          ((self.z3_value - self.z2_value) / 2)):
            self.rabbit_factor = self.z2_rabbit
            self.max_linear_velocity = self.z2_lin_vel
            self.max_angular_velocity = self.z2_ang_vel
        else:
            self.rabbit_factor = self.z3_rabbit
            self.max_linear_velocity = self.z3_lin_vel
            self.max_angular_velocity = self.z3_ang_vel

#        if self.distance_to_goal < 3*self.max_distance_error :
#            self.max_linear_velocity *= self.distance_to_goal
        if self.distance_to_goal < self.z4_distance_to_target:
            self.rabbit_factor = 1
            #self.max_linear_velocity = self.max_linear_velocity / ( self.max_linear_velocity + (self.z4_distance_to_target - self.distance_to_goal)**2)
            self.max_linear_velocity = (
                self.max_linear_velocity /
                self.z4_distance_to_target) * self.distance_to_goal
            self.max_angular_velocity = self.z3_ang_vel
        elif not self.corrected:
            self.rabbit_factor = self.z3_rabbit * self.retarder
            if math.fabs(self.angle_error) < self.z2_max_angle:
                self.max_linear_velocity = self.z2_lin_vel
            else:
                self.max_linear_velocity = self.z3_lin_vel * self.retarder
            self.max_angular_velocity = self.z3_ang_vel
#            self.max_linear_velocity *=  self.retarder
#            self.rabbit_factor = self.z3_rabbit

    def control_loop(self):
        """
            Method running the control loop. Distinguishes between target and goal to 
            adapt to other planners. For position planning the two will be the same.
        """
        self.sp_linear = self.max_linear_velocity
        self.sp_angular = self.angle_error

        # Implement maximum linear velocity and maximum angular velocity
        if self.sp_linear > self.max_linear_velocity:
            self.sp_linear = self.max_linear_velocity
        if self.sp_linear < -self.max_linear_velocity:
            self.sp_linear = -self.max_linear_velocity
        if self.twist.twist.angular.z > self.max_angular_velocity:
            self.sp_angular = self.max_angular_velocity
        if self.sp_angular < -self.max_angular_velocity:
            self.sp_angular = -self.max_angular_velocity

        # Prohibit reverse driving
        if self.sp_linear < 0:
            self.sp_linear = 0

        # If not preempted, add a time stamp and publish the twist
        if not self.isPreemptRequested():
            self.twist = self.controller.generateTwist(self.sp_linear,
                                                       self.sp_angular)
            self.twist_pub.publish(self.twist)

    def onOdometry(self, msg):
        """
            Callback method for handling odometry messages
        """
        # Extract the orientation quaternion
        self.quaternion[0] = msg.pose.pose.orientation.x
        self.quaternion[1] = msg.pose.pose.orientation.y
        self.quaternion[2] = msg.pose.pose.orientation.z
        self.quaternion[3] = msg.pose.pose.orientation.w
        (roll, pitch,
         self.yaw) = tf.transformations.euler_from_quaternion(self.quaternion)

        # Extract the position vector
        self.position[0] = msg.pose.pose.position.x
        self.position[1] = msg.pose.pose.position.y

    def empty_method(self):
        """
            Empty method pointer
        """
        return False

    def get_current_position(self):
        """
            Get current position from tf
        """
        if self.use_tf:
            try:
                (position, head) = self.__listen.lookupTransform(
                    self.odom_frame, self.base_frame, rospy.Time(0)
                )  # The transform is returned as position (x,y,z) and an orientation quaternion (x,y,z,w).
                self.position[0] = position[0]
                self.position[1] = position[1]
                (roll, pitch,
                 self.yaw) = tf.transformations.euler_from_quaternion(head)
                print("State: (" + str(position[0]) + " , " +
                      str(position[1]) + " , " + str(self.yaw) + ")")
            except (tf.LookupException, tf.ConnectivityException), err:
                rospy.loginfo("could not locate vehicle")
Esempio n. 17
0
    def update(self):
        # Get current pose and send it to controller
        self.get_current_position()
        self.heading = Vector(math.cos(self.yaw), math.sin(self.yaw))
        self.controller.setFeedback(self.position, self.heading)

        # Construct projection vector, perpendicular vector path vectors and rabbit vector
        self.projection = (self.position - self.line_begin).projectedOn(
            self.line)
        if self.projection.angle(self.line) > 0.1 or self.projection.angle(
                self.line) < -0.1:
            self.projection = self.projection.scale(-1)
        self.perpendicular = self.projection - self.position + self.line_begin
        self.goal_path = self.line_end - self.position
        self.rabbit = self.line - self.projection
        self.rabbit = self.rabbit.scale(self.rabbit_factor)
        self.rabbit += self.line_begin
        self.rabbit += self.projection
        self.rabbit_path = self.rabbit - Vector(self.position[0],
                                                self.position[1])

        # Calculate distances
        self.distance_to_goal = self.goal_path.length()
        self.distance_to_line = self.perpendicular.length()

        # Calculate angle between heading vector and goal/rabbit path vector
        self.angle_error = self.heading.angle(self.rabbit_path)
        # Rotate the heading vector according to the calculated angle and test correspondence
        # with the path vector. If not zero sign must be flipped. This is to avoid the sine trap.
        t1 = self.heading.rotate(self.angle_error)
        if self.rabbit_path.angle(t1) != 0:
            self.angle_error = -self.angle_error

        self.goal_angle_error = self.heading.angle(self.goal_path)
        # Avoid the sine trap.
        t1 = self.heading.rotate(self.goal_angle_error)
        if self.goal_path.angle(t1) != 0:
            self.goal_angle_error = -self.goal_angle_error
        print("Errors (to goal, to line, angle) : (" +
              str(self.distance_to_goal) + " , " + str(self.distance_to_line) +
              " , " + str(self.angle_error) + ")")
        # Determine zone
        if self.distance_to_line < self.z1_max_distance and math.fabs(
                self.angle_error) < self.z1_max_angle:
            self.zone_filter[self.z_ptr] = self.z1_value
        elif self.distance_to_line < self.z2_max_distance and math.fabs(
                self.angle_error) < self.z2_max_angle:
            self.zone_filter[self.z_ptr] = self.z2_value
        else:
            self.zone_filter[self.z_ptr] = self.z3_value

        self.z_ptr = self.z_ptr + 1
        if self.z_ptr >= self.z_filter_size:
            self.z_ptr = 0

        self.zone = (sum(self.zone_filter) / len(self.zone_filter))

        if self.zone < (self.z1_value + ((self.z2_value - self.z1_value) / 2)):
            self.corrected = True
            self.rabbit_factor = self.z1_rabbit
            self.max_linear_velocity = self.z1_lin_vel
            self.max_angular_velocity = self.z1_ang_vel
        elif self.zone < (self.z2_value +
                          ((self.z3_value - self.z2_value) / 2)):
            self.rabbit_factor = self.z2_rabbit
            self.max_linear_velocity = self.z2_lin_vel
            self.max_angular_velocity = self.z2_ang_vel
        else:
            self.rabbit_factor = self.z3_rabbit
            self.max_linear_velocity = self.z3_lin_vel
            self.max_angular_velocity = self.z3_ang_vel

#        if self.distance_to_goal < 3*self.max_distance_error :
#            self.max_linear_velocity *= self.distance_to_goal
        if self.distance_to_goal < self.z4_distance_to_target:
            self.rabbit_factor = 1
            #self.max_linear_velocity = self.max_linear_velocity / ( self.max_linear_velocity + (self.z4_distance_to_target - self.distance_to_goal)**2)
            self.max_linear_velocity = (
                self.max_linear_velocity /
                self.z4_distance_to_target) * self.distance_to_goal
            self.max_angular_velocity = self.z3_ang_vel
        elif not self.corrected:
            self.rabbit_factor = self.z3_rabbit * self.retarder
            if math.fabs(self.angle_error) < self.z2_max_angle:
                self.max_linear_velocity = self.z2_lin_vel
            else:
                self.max_linear_velocity = self.z3_lin_vel * self.retarder
            self.max_angular_velocity = self.z3_ang_vel
Esempio n. 18
0
    def execute(self, goal):
        # Construct a vector from line goal
        self.line_begin[0] = goal.a_x
        self.line_begin[1] = goal.a_y
        self.line_end[0] = goal.b_x
        self.line_end[1] = goal.b_y
        self.line = Vector(self.line_end[0] - self.line_begin[0],
                           self.line_end[1] - self.line_begin[1])
        rospy.loginfo(rospy.get_name() + " Received goal: (%f,%f) to (%f,%f) ",
                      goal.a_x, goal.a_y, goal.b_x, goal.b_y)

        # Clear filter
        self.zone_filter = [self.z3_value] * self.z_filter_size
        self.corrected = False
        self.target_area = False

        while not rospy.is_shutdown():

            # Check for new goal
            if self.isNewGoalAvailable():
                rospy.loginfo(rospy.get_name() + "New goal is available")
                break

            # Preemption check
            if self.isPreemptRequested():
                rospy.loginfo(rospy.get_name() + "Preempt requested")
                break

            # Update vectors
            self.update()

            # Publish markers for rviz
            self.line_marker.updateLine([
                Point(self.line_begin[0], self.line_begin[1], 0),
                Point(self.line_end[0], self.line_end[1], 0)
            ])
            self.point_marker.updatePoint(
                [Point(self.rabbit[0], self.rabbit[1], 0)])
            self.pos_marker.updatePoint(
                [Point(self.position[0], self.position[1], 0)])

            # If the goal is unreached
            if self.distance_to_goal > self.max_distance_error:
                # Spin the loop
                self.control_loop()

                # Block
                try:
                    self.rate.sleep()
                except rospy.ROSInterruptException:
                    print("Interrupted during sleep")
                    return 'preempted'
            else:
                # Success - position has been reached
                rospy.loginfo(
                    rospy.get_name() + " Goal reached in distance: %f m",
                    self.distance_to_goal)
                self.stop()
                break

        # Return statement
        if self.isPreemptRequested():
            self.setPreempted()
            print("Returning due to preemption")
            return 'preempted'
        elif rospy.is_shutdown():
            self.setPreempted()
            print("Returning due to abort")
            return 'aborted'
        else:
            self.setSucceeded()
            print("Returning due to success")
            return 'succeeded'
Esempio n. 19
0
    def publish_twist(self, target, goal):
        """
            Method running the control loop. Distinguishes between target and goal to 
            adapt to other planners. For position planning the two will be the same.
        """
        # Get current position
        self.get_current_position()

        # Construct goal path vector
        goal_path = goal - Vector(self.position[0], self.position[1])

        # Calculate distance to goal
        self.distance_error = goal_path.length()

        # If the goal is unreached
        if self.distance_error > self.max_distance_error:
            # Construct target path vector
            target_path = target - Vector(self.position[0], self.position[1])

            # Calculate yaw
            if self.use_tf:
                (roll, pitch,
                 yaw) = tf.transformations.euler_from_quaternion(self.heading)
            else:
                (roll, pitch, yaw) = tf.transformations.euler_from_quaternion(
                    self.quaternion)

            # Construct heading vector
            head = Vector(math.cos(yaw), math.sin(yaw))

            # Calculate angle between heading vector and target path vector
            self.angle_error = head.angle(target_path)

            # Rotate the heading vector according to the calculated angle and test correspondence
            # with the path vector. If not zero sign must be flipped. This is to avoid the sine trap.
            t1 = head.rotate(self.angle_error)
            if target_path.angle(t1) != 0:
                self.angle_error = -self.angle_error

            # Calculate angle between heading vector and goal path vector
            goal_angle_error = head.angle(goal_path)

            # Avoid the sine trap.
            t1 = head.rotate(goal_angle_error)
            if goal_path.angle(t1) != 0:
                goal_angle_error = -goal_angle_error

            # Check if large initial errors have been corrected
            if math.fabs(self.angle_error) < self.max_initial_error:
                self.corrected = True

            # Generate velocity setpoints from distance and angle errors
            self.sp_linear = self.distance_error
            self.sp_angular = self.angle_error

            # Calculate velocity errors for control loop
            self.lin_err = self.sp_linear - self.fb_linear
            self.ang_err = self.sp_angular - self.fb_angular

            # Calculate integrators and implement max
            self.lin_int += self.lin_err * self.period
            if self.lin_int > self.int_max:
                self.lin_int = self.int_max
            if self.lin_int < -self.int_max:
                self.lin_int = -self.int_max
            self.ang_int += self.ang_err * self.period
            if self.ang_int > self.int_max:
                self.ang_int = self.int_max
            if self.ang_int < -self.int_max:
                self.ang_int = -self.int_max

            # Calculate differentiators and save value
            self.lin_diff = (self.lin_prev_err - self.lin_err) / self.period
            self.ang_diff = (self.ang_prev_err - self.ang_err) / self.period
            self.lin_prev_err = self.lin_err
            self.ang_prev_err = self.ang_err

            # Update twist message with control velocities
            self.twist.twist.linear.x = (self.lin_err * self.lin_p) + (
                self.lin_int * self.lin_i) + (self.lin_diff * self.lin_d)
            self.twist.twist.angular.z = (self.ang_err * self.ang_p) + (
                self.ang_int * self.ang_i) + (self.ang_diff * self.ang_d)

            # Implement retarder to reduce linear velocity if angle error is too big
            if math.fabs(goal_angle_error) > self.max_angle_error:
                self.twist.twist.linear.x *= self.retarder

            # Implement initial correction speed for large angle errors
            if not self.corrected:
                self.twist.twist.linear.x *= self.retarder**2

            # Implement maximum linear velocity and maximum angular velocity
            if self.twist.twist.linear.x > self.max_linear_velocity:
                self.twist.twist.linear.x = self.max_linear_velocity
            if self.twist.twist.linear.x < -self.max_linear_velocity:
                self.twist.twist.linear.x = -self.max_linear_velocity
            if self.twist.twist.angular.z > self.max_angular_velocity:
                self.twist.twist.angular.z = self.max_angular_velocity
            if self.twist.twist.angular.z < -self.max_angular_velocity:
                self.twist.twist.angular.z = -self.max_angular_velocity

            # Prohibit reverse driving
            if self.twist.twist.linear.x < 0:
                self.twist.twist.linear.x = 0

            # If not preempted, add a time stamp and publish the twist
            if not self.isPreemptRequested():
                self.twist.header.stamp = rospy.Time.now()
                self.twist_pub.publish(self.twist)

            return True
        else:
            return False
Esempio n. 20
0
    def __init__(self):
        # Init control methods
        self.isNewGoalAvailable = self.empty_method()
        self.isPreemptRequested = self.empty_method()
        self.setPreempted = self.empty_method()

        # Get topics and transforms
        self.cmd_vel_topic = rospy.get_param("~cmd_vel_topic",
                                             "/fmSignals/cmd_vel")
        self.odom_frame = rospy.get_param("~odom_frame", "/odom")
        self.odometry_topic = rospy.get_param("~odometry_topic",
                                              "/fmKnowledge/odom")
        self.base_frame = rospy.get_param("~base_frame", "/base_footprint")
        self.use_tf = rospy.get_param("~use_tf", False)

        # Get general parameters
        self.max_linear_velocity = rospy.get_param("~max_linear_velocity", 2)
        self.max_angular_velocity = rospy.get_param("~max_angular_velocity", 1)
        self.max_initial_error = rospy.get_param("~max_initial_angle_error", 1)

        self.max_distance_error = rospy.get_param("~max_distance_error", 0.2)
        self.use_tf = rospy.get_param("~use_tf", False)
        self.max_angle_error = rospy.get_param("~max_angle_error", math.pi / 4)
        self.retarder = rospy.get_param("~retarder", 0.8)

        # Control loop
        self.lin_p = rospy.get_param("~lin_p", 0.4)
        self.lin_i = rospy.get_param("~lin_i", 0.6)
        self.lin_d = rospy.get_param("~lin_d", 0.0)
        self.ang_p = rospy.get_param("~ang_p", 0.8)
        self.ang_i = rospy.get_param("~ang_i", 0.1)
        self.ang_d = rospy.get_param("~ang_d", 0.05)
        self.int_max = rospy.get_param("~int_max", 0.1)

        # Setup Publishers and subscribers
        if not self.use_tf:
            self.odometry_topic = rospy.get_param("~odometry_topic",
                                                  "/fmKnowledge/odom")
            self.odom_sub = rospy.Subscriber(self.odometry_topic, Odometry,
                                             self.onOdometry)
        self.twist_pub = rospy.Publisher(self.cmd_vel_topic, TwistStamped)

        # Parameters for action server
        self.period = 0.1
        self.retarder_point = 0.3  #distance to target when speed should start declining

        # Init control loop
        self.lin_err = 0.0
        self.ang_err = 0.0
        self.lin_prev_err = 0.0
        self.ang_prev_err = 0.0
        self.lin_int = 0.0
        self.ang_int = 0.0
        self.lin_diff = 0.0
        self.ang_diff = 0.0
        self.distance_error = 0
        self.angle_error = 0
        self.fb_linear = 0.0
        self.fb_angular = 0.0
        self.sp_linear = 0.0
        self.sp_angular = 0.0

        # Init TF listener
        self.__listen = TransformListener()

        # Init controller
        self.corrected = False
        self.rate = rospy.Rate(1 / self.period)
        self.twist = TwistStamped()
        self.destination = Vector(0, 0)
        self.position = Vector(0, 0)
        self.heading = Vector(0, 0)
        self.quaternion = np.empty((4, ), dtype=np.float64)

        # Setup Publishers and subscribers
        self.twist_pub = rospy.Publisher(self.cmd_vel_topic, TwistStamped)
Esempio n. 21
0
class LinePlanner:
    """
        Controller class taking line goals and generating twist messages.
        The planner is based on the concept of a rabbit, an aiming point on the line between the robot and the target. 
        If the rabbit is close to the robot, it will follow the line close, but regulate much and if the rabbit is far 
        away the robot will drive steady, but might be off the line. 
        
        The planner sets up the following zones and acts accordingly:

        zone 1:    Low distance to line and low angle error
                   Everything is good so high linear velocity, low angular velocity and a rabbit far away
        zone 2:    Low distance to line, but higher angle error or low angle error but higher distance to line
                   Trying to correct heading so low linear velocity, normal angular velocity and medium rabbit
        zone 3:    High distance to line
                   This is going bad so low linear velocity, high angular velocity and a close rabbit
        zone 4:    Close to target
                   The line matters no more. Low linear velocity, high angular velocity and rabbit fixed on target.

        The filter controls when to change between the zones. The value assigned to each zone is not important, but the
        ratios between them is a measure of how conservative the planner is. Think of it as a measure of the transition states.
        Small transitions happens faster and large transitions happens slower.
        
        Examples:
        1,2,3 :    The planner will change quickly between zones making it very dynamic but less accurate
        1,5,25 :   The planner will change slowly between zones making it more accurate, but slow 
        1,15,25 :  The planner will be even more accurate and even slower
        1,2,25 :   The planner will be faster, but if the robot is slow, it will go slalom
        1,10,100 : Slower transitions is good for a slowly reacting robot
        
        The filter size acts as a low pass filter so higher filter size means slower reaction, but more robust to noisy sensors
        """

    def __init__(self):
        # Init line
        self.rabbit_factor = 0.2
        self.line_begin = Vector(0, 0)
        self.line_end = Vector(0, 5)
        self.line = Vector(self.line_end[0] - self.line_begin[0], self.line_end[1] - self.line_begin[1])
        self.yaw = 0.0

        # Init control methods
        self.isNewGoalAvailable = self.empty_method()
        self.isPreemptRequested = self.empty_method()
        self.setPreempted = self.empty_method()

        # Get parameters from parameter server
        self.getParameters()

        # Set up markers for rviz
        self.point_marker = MarkerUtility("/point_marker", self.odom_frame)
        self.line_marker = MarkerUtility("/line_marker", self.odom_frame)
        self.pos_marker = MarkerUtility("/pos_marker", self.odom_frame)

        # Init velocity control
        self.controller = Controller()
        self.distance_to_goal = 0
        self.angle_error = 0
        self.goal_angle_error = 0
        self.sp_linear = 0
        self.sp_angular = 0
        self.distance_to_line = 0

        # Init TF listener
        self.__listen = TransformListener()

        # Init planner
        self.corrected = False
        self.rate = rospy.Rate(1 / self.period)
        self.twist = TwistStamped()
        self.quaternion = np.empty((4,), dtype=np.float64)

        # Init vectors
        self.destination = Vector(1, 0)
        self.position = Vector(1, 0)  # Vector from origo to current position of the robot
        self.heading = Vector(1, 0)  # Vector in the current direction of the robot
        self.rabbit = Vector(1, 0)  # Vector from origo to the aiming point on the line
        self.rabbit_path = Vector(1, 0)  # Vector from current position to aiming point on the line
        self.perpendicular = Vector(1, 0)  # Vector from current position to the line perpendicular to the line
        self.projection = Vector(1, 0)  # Projection of the position vector on the line

        # Set up circular buffer for filter
        self.zone_filter = [self.z3_value] * self.z_filter_size
        self.zone = 2
        self.z_ptr = 0

        # Setup Publishers and subscribers
        if not self.use_tf:
            self.odom_sub = rospy.Subscriber(self.odometry_topic, Odometry, self.onOdometry)
        self.twist_pub = rospy.Publisher(self.cmd_vel_topic, TwistStamped)

    def getParameters(self):
        # Get topics and transforms
        self.cmd_vel_topic = rospy.get_param("~cmd_vel_topic", "/fmSignals/cmd_vel")
        self.odom_frame = rospy.get_param("~odom_frame", "/odom")
        self.odometry_topic = rospy.get_param("~odometry_topic", "/fmKnowledge/odom")
        self.base_frame = rospy.get_param("~base_frame", "/base_footprint")
        self.use_tf = rospy.get_param("~use_tf", True)

        # Get general parameters
        self.period = rospy.get_param("~period", 0.1)
        self.max_linear_velocity = rospy.get_param("~max_linear_velocity", 2)
        self.max_angular_velocity = rospy.get_param("~max_angular_velocity", 1)
        self.max_distance_error = rospy.get_param("~max_distance_error", 0.05)

        # Get control parameters
        self.retarder = rospy.get_param("~retarder", 0.8)

        # Get zone parameters
        self.z1_value = rospy.get_param("~zone1_value", 1)
        self.z1_lin_vel = rospy.get_param("~zone1_linear_velocity", 0.1)
        self.z1_ang_vel = rospy.get_param("~zone1_angular_velocity", 0.2)
        self.z1_rabbit = rospy.get_param("~zone1_rabbit_factor", 0.8)
        self.z1_max_distance = rospy.get_param("~zone1_max_distance_to_line", 0.05)
        self.z1_max_angle = rospy.get_param("~zone1_max_angle_error", math.pi / 36)

        self.z2_value = rospy.get_param("~zone2_value", 5)
        self.z2_lin_vel = rospy.get_param("~zone2_linear_velocity", 0.1)
        self.z2_ang_vel = rospy.get_param("~zone2_angular_velocity", 0.8)
        self.z2_rabbit = rospy.get_param("~zone2_rabbit_factor", 0.7)
        self.z2_max_distance = rospy.get_param("~zone2_max_distance_to_line", 0.25)
        self.z2_max_angle = rospy.get_param("~zone2_max_angle_error", math.pi / 6)

        self.z3_value = rospy.get_param("~zone3_value", 10)
        self.z3_lin_vel = rospy.get_param("~zone3_linear_velocity", 0.1)
        self.z3_ang_vel = rospy.get_param("~zone3_angular_velocity", 1.2)
        self.z3_rabbit = rospy.get_param("~zone3_rabbit_factor", 0.6)

        self.z4_distance_to_target = rospy.get_param("~zone4_distance_to_target", 0.4)
        self.z_filter_size = rospy.get_param("~transition_filter_size", 10)

    def stop(self):
        # Publish a zero twist to stop the robot
        self.twist.header.stamp = rospy.Time.now()
        self.sp_angular = 0
        self.twist.twist.angular.z = 0
        self.twist_pub.publish(self.twist)

    def execute(self, goal):
        # Construct a vector from line goal
        self.line_begin[0] = goal.a_x
        self.line_begin[1] = goal.a_y
        self.line_end[0] = goal.b_x
        self.line_end[1] = goal.b_y
        self.line = Vector(self.line_end[0] - self.line_begin[0], self.line_end[1] - self.line_begin[1])
        rospy.loginfo(rospy.get_name() + " Received goal: (%f,%f) to (%f,%f) ", goal.a_x, goal.a_y, goal.b_x, goal.b_y)

        # Clear filter
        self.zone_filter = [self.z3_value] * self.z_filter_size
        self.corrected = False
        self.target_area = False

        while not rospy.is_shutdown():

            # Check for new goal
            if self.isNewGoalAvailable():
                rospy.loginfo(rospy.get_name() + "New goal is available")
                break

            # Preemption check
            if self.isPreemptRequested():
                rospy.loginfo(rospy.get_name() + "Preempt requested")
                break

            # Update vectors
            self.update()

            # Publish markers for rviz
            self.line_marker.updateLine(
                [Point(self.line_begin[0], self.line_begin[1], 0), Point(self.line_end[0], self.line_end[1], 0)]
            )
            self.point_marker.updatePoint([Point(self.rabbit[0], self.rabbit[1], 0)])
            self.pos_marker.updatePoint([Point(self.position[0], self.position[1], 0)])

            # If the goal is unreached
            if self.distance_to_goal > self.max_distance_error:
                # Spin the loop
                self.control_loop()

                # Block
                try:
                    self.rate.sleep()
                except rospy.ROSInterruptException:
                    print("Interrupted during sleep")
                    return "preempted"
            else:
                # Success - position has been reached
                rospy.loginfo(rospy.get_name() + " Goal reached in distance: %f m", self.distance_to_goal)
                self.stop()
                break

        # Return statement
        if self.isPreemptRequested():
            self.setPreempted()
            print("Returning due to preemption")
            return "preempted"
        elif rospy.is_shutdown():
            self.setPreempted()
            print("Returning due to abort")
            return "aborted"
        else:
            self.setSucceeded()
            print("Returning due to success")
            return "succeeded"

    def update(self):
        # Get current pose and send it to controller
        self.get_current_position()
        self.heading = Vector(math.cos(self.yaw), math.sin(self.yaw))
        self.controller.setFeedback(self.position, self.heading)

        # Construct projection vector, perpendicular vector path vectors and rabbit vector
        self.projection = (self.position - self.line_begin).projectedOn(self.line)
        if self.projection.angle(self.line) > 0.1 or self.projection.angle(self.line) < -0.1:
            self.projection = self.projection.scale(-1)
        self.perpendicular = self.projection - self.position + self.line_begin
        self.goal_path = self.line_end - self.position
        self.rabbit = self.line - self.projection
        self.rabbit = self.rabbit.scale(self.rabbit_factor)
        self.rabbit += self.line_begin
        self.rabbit += self.projection
        self.rabbit_path = self.rabbit - Vector(self.position[0], self.position[1])

        # Calculate distances
        self.distance_to_goal = self.goal_path.length()
        self.distance_to_line = self.perpendicular.length()

        # Calculate angle between heading vector and goal/rabbit path vector
        self.angle_error = self.heading.angle(self.rabbit_path)
        # Rotate the heading vector according to the calculated angle and test correspondence
        # with the path vector. If not zero sign must be flipped. This is to avoid the sine trap.
        t1 = self.heading.rotate(self.angle_error)
        if self.rabbit_path.angle(t1) != 0:
            self.angle_error = -self.angle_error

        self.goal_angle_error = self.heading.angle(self.goal_path)
        # Avoid the sine trap.
        t1 = self.heading.rotate(self.goal_angle_error)
        if self.goal_path.angle(t1) != 0:
            self.goal_angle_error = -self.goal_angle_error

        # Determine zone
        if self.distance_to_line < self.z1_max_distance and math.fabs(self.angle_error) < self.z1_max_angle:
            self.zone_filter[self.z_ptr] = self.z1_value
        elif self.distance_to_line < self.z2_max_distance and math.fabs(self.angle_error) < self.z2_max_angle:
            self.zone_filter[self.z_ptr] = self.z2_value
        else:
            self.zone_filter[self.z_ptr] = self.z3_value

        self.z_ptr = self.z_ptr + 1
        if self.z_ptr >= self.z_filter_size:
            self.z_ptr = 0

        self.zone = sum(self.zone_filter) / len(self.zone_filter)
        if self.zone < (self.z1_value + ((self.z2_value - self.z1_value) / 2)):
            self.corrected = True
            self.rabbit_factor = self.z1_rabbit
            self.max_linear_velocity = self.z1_lin_vel
            self.max_angular_velocity = self.z1_ang_vel
        elif self.zone < (self.z2_value + ((self.z3_value - self.z2_value) / 2)):
            self.rabbit_factor = self.z2_rabbit
            self.max_linear_velocity = self.z2_lin_vel
            self.max_angular_velocity = self.z2_ang_vel
        else:
            self.rabbit_factor = self.z3_rabbit
            self.max_linear_velocity = self.z3_lin_vel
            self.max_angular_velocity = self.z3_ang_vel

        if self.distance_to_goal < 3 * self.max_distance_error:
            self.max_linear_velocity *= self.distance_to_goal
        elif self.distance_to_goal < self.z4_distance_to_target:
            self.rabbit_factor = 1
            self.max_linear_velocity = self.max_linear_velocity / (
                self.max_linear_velocity + (self.z4_distance_to_target - self.distance_to_goal) ** 2
            )
            self.max_angular_velocity = self.z3_ang_vel
        elif not self.corrected:
            self.max_linear_velocity *= self.retarder
            self.rabbit_factor = self.z3_rabbit

    def control_loop(self):
        """
            Method running the control loop. Distinguishes between target and goal to 
            adapt to other planners. For position planning the two will be the same.
        """
        self.sp_linear = self.max_linear_velocity
        self.sp_angular = self.angle_error

        # Implement maximum linear velocity and maximum angular velocity
        if self.sp_linear > self.max_linear_velocity:
            self.sp_linear = self.max_linear_velocity
        if self.sp_linear < -self.max_linear_velocity:
            self.sp_linear = -self.max_linear_velocity
        if self.twist.twist.angular.z > self.max_angular_velocity:
            self.sp_angular = self.max_angular_velocity
        if self.sp_angular < -self.max_angular_velocity:
            self.sp_angular = -self.max_angular_velocity

        # Prohibit reverse driving
        if self.sp_linear < 0:
            self.sp_linear = 0

        # If not preempted, add a time stamp and publish the twist
        if not self.isPreemptRequested():
            self.twist = self.controller.generateTwist(self.sp_linear, self.sp_angular)
            self.twist_pub.publish(self.twist)

    def onOdometry(self, msg):
        """
            Callback method for handling odometry messages
        """
        # Extract the orientation quaternion
        self.quaternion[0] = msg.pose.pose.orientation.x
        self.quaternion[1] = msg.pose.pose.orientation.y
        self.quaternion[2] = msg.pose.pose.orientation.z
        self.quaternion[3] = msg.pose.pose.orientation.w
        (roll, pitch, self.yaw) = tf.transformations.euler_from_quaternion(self.quaternion)

        # Extract the position vector
        self.position[0] = msg.pose.pose.position.x
        self.position[1] = msg.pose.pose.position.y

    def empty_method(self):
        """
            Empty method pointer
        """
        return False

    def get_current_position(self):
        """
            Get current position from tf
        """
        if self.use_tf:
            try:
                (position, head) = self.__listen.lookupTransform(
                    self.odom_frame, self.base_frame, rospy.Time(0)
                )  # The transform is returned as position (x,y,z) and an orientation quaternion (x,y,z,w).
                self.position[0] = position[0]
                self.position[1] = position[1]
                (roll, pitch, self.yaw) = tf.transformations.euler_from_quaternion(head)
            except (tf.LookupException, tf.ConnectivityException), err:
                rospy.loginfo("could not locate vehicle")
Esempio n. 22
0
class Controller():
    def __init__(self):      
        # Init control loop
        self.twist = TwistStamped()
        self.lin_err = 0.0
        self.ang_err = 0.0
        self.lin_prev_err = 0.0
        self.ang_prev_err = 0.0
        self.lin_int = 0.0
        self.ang_int = 0.0
        self.lin_diff = 0.0
        self.ang_diff = 0.0
        self.fb_linear = 0.0
        self.fb_angular = 0.0   
        
        # Get parameters
        self.period = rospy.get_param("~period",0.1)
        self.lin_p = rospy.get_param("~lin_p",0.4)
        self.lin_i = rospy.get_param("~lin_i",0.6)
        self.lin_d = rospy.get_param("~lin_d",0.0)
        self.ang_p = rospy.get_param("~ang_p",0.8)
        self.ang_i = rospy.get_param("~ang_i",0.1)
        self.ang_d = rospy.get_param("~ang_d",0.05)
        self.int_max = rospy.get_param("~integrator_max",0.1)   
        self.filter_size = rospy.get_param("~filter_size",10) 
        self.max_linear_velocity = rospy.get_param("~max_linear_velocity",2)
        self.max_angular_velocity = rospy.get_param("~max_angular_velocity",1)
        self.linear_vel = [0.0] * self.filter_size
        self.angular_vel = [0.0] * self.filter_size
        self.time = 0.0
        self.last_entry = rospy.Time.now()
        self.last_cl_entry = rospy.Time.now()
        self.distance = 0.0
        self.last_position = Vector(1,0)
        self.angle = 0.0
        self.last_heading = Vector(1,0)
        self.ptr = 0
        
    def generateTwist(self,sp_linear,sp_angular):
        # Calculate time since last entry
        self.period = (rospy.Time.now() - self.last_cl_entry).to_sec()
        self.last_cl_entry = rospy.Time.now()
        
        # Estimate feedback velocities
        self.fb_linear = (sum(self.linear_vel)/len(self.linear_vel))
        self.fb_angular = -(sum(self.angular_vel)/len(self.angular_vel))
        
        # Calculate velocity errors for control loop
        self.lin_err = sp_linear - self.fb_linear
        self.ang_err = sp_angular - self.fb_angular
        
        # Calculate integrators and implement max
        self.lin_int += self.lin_err * self.period
        if self.lin_int > self.int_max :
            self.lin_int = self.int_max
        if self.lin_int < -self.int_max :
            self.lin_int = -self.int_max                
        self.ang_int += self.ang_err * self.period    
        if self.ang_int > self.int_max :
            self.ang_int = self.int_max
        if self.ang_int < -self.int_max :
            self.ang_int = -self.int_max
            
        # Calculate differentiators and save value
        if self.period :
            self.lin_diff = (self.lin_prev_err - self.lin_err) / self.period
            self.ang_diff = (self.ang_prev_err - self.ang_err) / self.period
        self.lin_prev_err = self.lin_err
        self.ang_prev_err = self.ang_err
        
        # Update twist message with control velocities
        self.twist.header.stamp = rospy.Time.now() 
        self.twist.twist.linear.x = sp_linear + (self.lin_err * self.lin_p) + (self.lin_int * self.lin_i) + (self.lin_diff * self.lin_d)
        self.twist.twist.angular.z = sp_angular + (self.ang_err * self.ang_p) + (self.ang_int * self.ang_i) + (self.ang_diff * self.ang_d)
#        print("Fb:",(self.fb_linear,self.fb_angular)," Sp:",(sp_linear,sp_angular)," New:",(self.twist.twist.linear.x,self.twist.twist.angular.z))
        
        # Implement maximum linear velocity and maximum angular velocity
        if self.twist.twist.linear.x > self.max_linear_velocity:
            self.twist.twist.linear.x = self.max_linear_velocity
        if self.twist.twist.linear.x < -self.max_linear_velocity:
            self.twist.twist.linear.x = -self.max_linear_velocity
        if self.twist.twist.angular.z > self.max_angular_velocity:
            self.twist.twist.angular.z = self.max_angular_velocity
        if self.twist.twist.angular.z < -self.max_angular_velocity:
            self.twist.twist.angular.z = -self.max_angular_velocity
            
        return self.twist

    def setFeedback(self,position,heading):
        # Calculate time since last entry
        self.time = (rospy.Time.now() - self.last_entry).to_sec()
        self.last_entry = rospy.Time.now()
        
        # Calculate distance travelled since last entry
        self.distance = (self.last_position - position).length()
        self.last_position = position
        
        # Calculate change in orientation since last entry
        self.angle = heading.angle(self.last_heading)
        t1 = heading.rotate(self.angle)
        if self.last_heading.angle(t1) != 0 :
            self.angle = -self.angle
            
        self.last_heading = heading
        
        if self.time :
            self.linear_vel[self.ptr] = self.distance / self.time
            self.angular_vel[self.ptr] = self.angle / self.time
            self.ptr = self.ptr + 1
            if self.ptr >= self.filter_size :
                self.ptr = 0  
Esempio n. 23
0
    def publish_twist(self, target, goal):
        """
            Method running the control loop. Distinguishes between target and goal to 
            adapt to other planners. For position planning the two will be the same.
        """
        # Get current position
        self.get_current_position()

        # Construct goal path vector
        goal_path = goal - Vector(self.position[0], self.position[1])

        # Calculate distance to goal
        self.distance_error = goal_path.length()

        # If the goal is unreached
        if self.distance_error > self.max_distance_error:
            # Construct target path vector
            target_path = target - Vector(self.position[0], self.position[1])

            # Calculate yaw
            if self.use_tf:
                (roll, pitch, yaw) = tf.transformations.euler_from_quaternion(self.heading)
            else:
                (roll, pitch, yaw) = tf.transformations.euler_from_quaternion(self.quaternion)

            # Construct heading vector
            head = Vector(math.cos(yaw), math.sin(yaw))

            # Calculate angle between heading vector and target path vector
            self.angle_error = head.angle(target_path)

            # Rotate the heading vector according to the calculated angle and test correspondence
            # with the path vector. If not zero sign must be flipped. This is to avoid the sine trap.
            t1 = head.rotate(self.angle_error)
            if target_path.angle(t1) != 0:
                self.angle_error = -self.angle_error

            # Calculate angle between heading vector and goal path vector
            goal_angle_error = head.angle(goal_path)

            # Avoid the sine trap.
            t1 = head.rotate(goal_angle_error)
            if goal_path.angle(t1) != 0:
                goal_angle_error = -goal_angle_error

            # Check if large initial errors have been corrected
            if math.fabs(self.angle_error) < self.max_initial_error:
                self.corrected = True

            # Generate velocity setpoints from distance and angle errors
            self.sp_linear = self.distance_error
            self.sp_angular = self.angle_error

            # Calculate velocity errors for control loop
            self.lin_err = self.sp_linear - self.fb_linear
            self.ang_err = self.sp_angular - self.fb_angular

            # Calculate integrators and implement max
            self.lin_int += self.lin_err * self.period
            if self.lin_int > self.int_max:
                self.lin_int = self.int_max
            if self.lin_int < -self.int_max:
                self.lin_int = -self.int_max
            self.ang_int += self.ang_err * self.period
            if self.ang_int > self.int_max:
                self.ang_int = self.int_max
            if self.ang_int < -self.int_max:
                self.ang_int = -self.int_max

            # Calculate differentiators and save value
            self.lin_diff = (self.lin_prev_err - self.lin_err) / self.period
            self.ang_diff = (self.ang_prev_err - self.ang_err) / self.period
            self.lin_prev_err = self.lin_err
            self.ang_prev_err = self.ang_err

            # Update twist message with control velocities
            self.twist.twist.linear.x = (
                (self.lin_err * self.lin_p) + (self.lin_int * self.lin_i) + (self.lin_diff * self.lin_d)
            )
            self.twist.twist.angular.z = (
                (self.ang_err * self.ang_p) + (self.ang_int * self.ang_i) + (self.ang_diff * self.ang_d)
            )

            # Implement retarder to reduce linear velocity if angle error is too big
            if math.fabs(goal_angle_error) > self.max_angle_error:
                self.twist.twist.linear.x *= self.retarder

            # Implement initial correction speed for large angle errors
            if not self.corrected:
                self.twist.twist.linear.x *= self.retarder ** 2

            # Implement maximum linear velocity and maximum angular velocity
            if self.twist.twist.linear.x > self.max_linear_velocity:
                self.twist.twist.linear.x = self.max_linear_velocity
            if self.twist.twist.linear.x < -self.max_linear_velocity:
                self.twist.twist.linear.x = -self.max_linear_velocity
            if self.twist.twist.angular.z > self.max_angular_velocity:
                self.twist.twist.angular.z = self.max_angular_velocity
            if self.twist.twist.angular.z < -self.max_angular_velocity:
                self.twist.twist.angular.z = -self.max_angular_velocity

            # Prohibit reverse driving
            if self.twist.twist.linear.x < 0:
                self.twist.twist.linear.x = 0

            # If not preempted, add a time stamp and publish the twist
            if not self.isPreemptRequested():
                self.twist.header.stamp = rospy.Time.now()
                self.twist_pub.publish(self.twist)

            return True
        else:
            return False
Esempio n. 24
0
class Controller():
    def __init__(self):
        # Init control loop
        self.twist = TwistStamped()
        self.lin_err = 0.0
        self.ang_err = 0.0
        self.lin_prev_err = 0.0
        self.ang_prev_err = 0.0
        self.lin_int = 0.0
        self.ang_int = 0.0
        self.lin_diff = 0.0
        self.ang_diff = 0.0
        self.fb_linear = 0.0
        self.fb_angular = 0.0

        # Get parameters
        self.period = rospy.get_param("~period", 0.1)
        self.lin_p = rospy.get_param("~lin_p", 0.4)
        self.lin_i = rospy.get_param("~lin_i", 0.6)
        self.lin_d = rospy.get_param("~lin_d", 0.0)
        self.ang_p = rospy.get_param("~ang_p", 0.8)
        self.ang_i = rospy.get_param("~ang_i", 0.1)
        self.ang_d = rospy.get_param("~ang_d", 0.05)
        self.int_max = rospy.get_param("~integrator_max", 0.1)
        self.filter_size = rospy.get_param("~filter_size", 10)
        self.max_linear_velocity = rospy.get_param("~max_linear_velocity", 2)
        self.max_angular_velocity = rospy.get_param("~max_angular_velocity", 1)
        self.use_dynamic_reconfigure = rospy.get_param(
            "~use_dynamic_reconfigure", False)
        self.linear_vel = [0.0] * self.filter_size
        self.angular_vel = [0.0] * self.filter_size
        self.time = 0.0
        self.last_entry = rospy.Time.now()
        self.last_cl_entry = rospy.Time.now()
        self.distance = 0.0
        self.last_position = Vector(1, 0)
        self.angle = 0.0
        self.last_heading = Vector(1, 0)
        self.ptr = 0

    def reconfigure_cb(self, config, level):
        self.lin_p = config['lin_p']
        self.lin_i = config['lin_i']
        self.lin_d = config['lin_d']
        self.ang_p = config['ang_p']
        self.ang_i = config['ang_i']
        self.ang_d = config['ang_d']
        self.int_max = config['integrator_max']
        return config

    def generateTwist(self, sp_linear, sp_angular):
        # Calculate time since last entry
        self.period = (rospy.Time.now() - self.last_cl_entry).to_sec()
        self.last_cl_entry = rospy.Time.now()

        # Estimate feedback velocities
        self.fb_linear = (sum(self.linear_vel) / len(self.linear_vel))
        self.fb_angular = -(sum(self.angular_vel) / len(self.angular_vel))

        # Calculate velocity errors for control loop
        self.lin_err = sp_linear - self.fb_linear
        self.ang_err = sp_angular - self.fb_angular

        # Calculate integrators and implement max
        self.lin_int += self.lin_err * self.period
        if self.lin_int > self.int_max:
            self.lin_int = self.int_max
        if self.lin_int < -self.int_max:
            self.lin_int = -self.int_max
        self.ang_int += self.ang_err * self.period
        if self.ang_int > self.int_max:
            self.ang_int = self.int_max
        if self.ang_int < -self.int_max:
            self.ang_int = -self.int_max

        # Calculate differentiators and save value
        if self.period:
            self.lin_diff = (self.lin_prev_err - self.lin_err) / self.period
            self.ang_diff = (self.ang_prev_err - self.ang_err) / self.period
        self.lin_prev_err = self.lin_err
        self.ang_prev_err = self.ang_err

        # Update twist message with control velocities
        self.twist.header.stamp = rospy.Time.now()
        self.twist.twist.linear.x = sp_linear + (self.lin_err * self.lin_p) + (
            self.lin_int * self.lin_i) + (self.lin_diff * self.lin_d)
        self.twist.twist.angular.z = sp_angular + (
            self.ang_err * self.ang_p) + (self.ang_int * self.ang_i) + (
                self.ang_diff * self.ang_d)
        #        print("Fb:",(self.fb_linear,self.fb_angular)," Sp:",(sp_linear,sp_angular)," New:",(self.twist.twist.linear.x,self.twist.twist.angular.z))

        # Implement maximum linear velocity and maximum angular velocity
        if self.twist.twist.linear.x > self.max_linear_velocity:
            self.twist.twist.linear.x = self.max_linear_velocity
        if self.twist.twist.linear.x < -self.max_linear_velocity:
            self.twist.twist.linear.x = -self.max_linear_velocity
        if self.twist.twist.angular.z > self.max_angular_velocity:
            self.twist.twist.angular.z = self.max_angular_velocity
        if self.twist.twist.angular.z < -self.max_angular_velocity:
            self.twist.twist.angular.z = -self.max_angular_velocity

        return self.twist

    def setFeedback(self, position, heading):
        # Calculate time since last entry
        self.time = (rospy.Time.now() - self.last_entry).to_sec()
        self.last_entry = rospy.Time.now()

        # Calculate distance travelled since last entry
        self.distance = (self.last_position - position).length()
        self.last_position = position

        # Calculate change in orientation since last entry
        self.angle = heading.angle(self.last_heading)
        t1 = heading.rotate(self.angle)
        if self.last_heading.angle(t1) != 0:
            self.angle = -self.angle

        self.last_heading = heading

        if self.time:
            self.linear_vel[self.ptr] = self.distance / self.time
            self.angular_vel[self.ptr] = self.angle / self.time
            self.ptr = self.ptr + 1
            if self.ptr >= self.filter_size:
                self.ptr = 0