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()
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)
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 = []
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 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")
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()
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)
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)
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
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
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 __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
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 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 __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)
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")
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
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 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
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)
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")
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
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
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