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)
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")