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):
# 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 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 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 __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 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 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 __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)
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