def setup(self):
self.first_img = None
self.prev_img = None
self.first = True
self.alpha = 0.7
self.lr_err = ERR()
self.fb_err = ERR()
self.prev_time = None
self.pospub = rospy.Publisher('/pidrone/set_mode', Mode, queue_size=1)
self.pos = [0, 0]
self.lr_pid = PIDaxis(-0.00001,
-0.,
-0.0,
midpoint=0,
control_range=(-15., 15.))
self.fb_pid = PIDaxis(0.00001,
0.,
0.0,
midpoint=0,
control_range=(-15., 15.))
self.index_params = dict(algorithm=6,
table_number=6,
key_size=12,
multi_probe_level=1)
self.search_params = dict(checks=50)
self.min_match_count = 10
self.camera_matrix = np.array([[250.0, 0., 160.0], [0., 250.0, 120.0],
[0., 0., 1.]])
self.z = 7.5
self.est_RT = np.identity(4)
self.threshold = 0.5
def setup(self):
self.first_kp = None
self.first_des = None
self.prev_kp = None
self.prev_des = None
self.first = True
self.orb = cv2.ORB_create(nfeatures=500, nlevels=8, scaleFactor=1.1)
self.alpha = 0.7
self.lr_err = ERR()
self.fb_err = ERR()
self.prev_time = None
self.pospub = rospy.Publisher('/pidrone/set_mode', Mode, queue_size=1)
self.smoothed_pos = PoseStamped()
self.lr_pid = PIDaxis(-0.01,
-0.,
-0.005,
midpoint=0,
control_range=(-15., 15.))
self.fb_pid = PIDaxis(0.01,
0.,
0.005,
midpoint=0,
control_range=(-15., 15.))
self.index_params = dict(algorithm=6,
table_number=6,
key_size=12,
multi_probe_level=1)
self.search_params = dict(checks=50)
self.min_match_count = 10
self.camera_matrix = np.array([[250.0, 0., 160.0], [0., 250.0, 120.0],
[0., 0., 1.]])
self.z = 7.5
self.est_RT = np.identity(4)
def __init__(self, camera, bridge):
picamera.array.PiMotionAnalysis.__init__(self, camera)
self.bridge = bridge
self.br = tf.TransformBroadcaster()
rospy.Subscriber("/pidrone/set_mode", Mode, self.mode_callback)
rospy.Subscriber("/pidrone/reset_transform", Empty, self.reset_callback)
rospy.Subscriber("/pidrone/toggle_transform", Empty, self.toggle_callback)
rospy.Subscriber("/pidrone/infrared", Range, self.range_callback)
self.pospub = rospy.Publisher('/pidrone/set_mode_vel', Mode, queue_size=1)
self.first_image_pub = rospy.Publisher("/pidrone/picamera/first_image", Image, queue_size=1, latch=True)
self.lr_pid = PIDaxis(4.00, -0.00000, 0.000, midpoint=0, control_range=(-10.0, 10.0))
self.fb_pid = PIDaxis(4.00, 0.0000, -0.000, midpoint=0, control_range=(-10.0, 10.0))
self.prev_img = None
self.detector = cv2.ORB(nfeatures=120, scoreType=cv2.ORB_FAST_SCORE)
self.index_params = dict(algorithm=6, table_number=6, key_size=12, multi_probe_level=1)
self.search_params = dict(checks=50)
self.matcher = cv2.FlannBasedMatcher(self.index_params, self.search_params)
self.first_kp = None
self.first_des = None
self.prev_kp = None
self.prev_des = None
self.first = True
self.lr_err = ERR()
self.fb_err = ERR()
self.prev_time = None
self.prev_rostime = None
self.pos = [0, 0]
self.z = 0.075
self.smoothed_yaw = 0.0
self.yaw_observed = 0.0
self.iacc_yaw = 0.0
self.transforming = False
self.last_first_time = None
self.target_x = 0
self.target_y = 0
self.first_counter = 0
self.max_first_counter = 0
self.mode = Mode()
# constant
self.kp_yaw = 80.0
self.ki_yaw = 0.1
self.kpi_yaw = 15.0
self.kpi_max_yaw = 0.01
self.alpha_yaw = 0.1 # perceived yaw smoothing alpha
self.hybrid_alpha = 0.1 # blend position with first frame and int
def setup(self):
self.first_img = None
self.prev_img = None
self.first = True
self.lr_err = ERR()
self.fb_err = ERR()
self.prev_time = None
self.pospub = rospy.Publisher('/pidrone/set_mode_vel', Mode, queue_size=1)
self.pos = [0, 0, 0]
# -, -, 0.1
#self.lr_pid = PIDaxis(0.100, -0.000100, 0.0050, midpoint=0, control_range=(-10.0, 10.0))
self.lr_pid = PIDaxis(0.0500, -0.00000, 0.000, midpoint=0, control_range=(-10.0, 10.0))
self.fb_pid = PIDaxis(-0.0500, 0.0000, -0.000, midpoint=0, control_range=(-10.0, 10.0))
#self.lr_pid = PIDaxis(0.0500, 0.001, 0.04, midpoint=0, control_range=(-15., 15.))
#self.fb_pid = PIDaxis(-0.0500, -0.0010, -0.04, midpoint=0, control_range=(-15., 15.))
#self.lr_pid = PIDaxis(0.05, 0., 0.001, midpoint=0, control_range=(-15., 15.))
#self.fb_pid = PIDaxis(-0.05, 0., -0.001, midpoint=0, control_range=(-15., 15.))
self.index_params = dict(algorithm = 6, table_number = 6,
key_size = 12, multi_probe_level = 1)
self.search_params = dict(checks = 50)
self.min_match_count = 10
self.camera_matrix = np.array([[ 250.0, 0., 160.0],
[ 0., 250.0, 120.0],
[ 0., 0., 1.]])
self.z = 7.5
self.est_RT = np.identity(4)
self.threshold = 0.5
self.kp_yaw = 100.0
self.ki_yaw = 0.1
self.kpi_yaw = 20.0
self.kpi_max_yaw = 0.01
self.alpha_yaw = 0.1
self.smoothed_yaw = 0.0
self.yaw_observed = 0.0
self.iacc_yaw = 0.0
self.transforming = False
self.i = 0
self.last_first_time = None
self.target_x = 0
self.target_y = 0
def __init__(self, camera, bridge):
picamera.array.PiMotionAnalysis.__init__(self, camera)
self.bridge = bridge
self.br = tf.TransformBroadcaster()
# bind method calls to subscribed topics
rospy.Subscriber("/pidrone/set_mode", Mode, self.mode_callback)
rospy.Subscriber("/pidrone/reset_transform", Empty,
self.reset_callback)
rospy.Subscriber("/pidrone/toggle_transform", Empty,
self.toggle_callback)
rospy.Subscriber("/pidrone/infrared", Range, self.range_callback)
rospy.Subscriber('/pidrone/angle', TwistStamped, self.angle_callback)
self.pospub = rospy.Publisher('/pidrone/set_mode_vel',
Mode,
queue_size=1)
self.first_image_pub = rospy.Publisher("/pidrone/picamera/first_image",
Image,
queue_size=1,
latch=True)
self.lr_pid = PIDaxis(10.0,
0.000,
0.0,
midpoint=0,
control_range=(-5.0, 5.0))
self.fb_pid = PIDaxis(10.0,
0.000,
0.0,
midpoint=0,
control_range=(-5.0, 5.0))
self.detector = cv2.ORB(nfeatures=NUM_FEATURES,
scoreType=cv2.ORB_FAST_SCORE
) # FAST_SCORE is a little faster to compute
map_grid_kp, map_grid_des = create_map('map.jpg')
self.estimator = LocalizationParticleFilter(map_grid_kp, map_grid_des)
self.first_locate = True
self.first_hold = True
self.prev_img = None
self.prev_kp = None
self.prev_des = None
self.locate_position = False
self.prev_time = None
self.prev_rostime = None
self.pos = [0, 0, 0]
self.yaw = 0.0
self.z = 0.075
self.iacc_yaw = 0.0
self.hold_position = False
self.target_pos = [0, 0, 0, 0]
self.target_yaw = 0.0
self.map_counter = 0
self.max_map_counter = 0
self.mode = Mode()
self.mode.mode = 5
# constant
self.kp_yaw = 50.0
self.ki_yaw = 0.1
self.alpha_yaw = 0.1 # perceived yaw smoothing alpha
self.hybrid_alpha = 0.3 # blend position with first frame and int
# angle
self.angle_x = 0.0 # the hz of state_controller is different
self.angle_y = 0.0
class AnalyzePhase(picamera.array.PiMotionAnalysis):
def __init__(self, camera, bridge):
picamera.array.PiMotionAnalysis.__init__(self, camera)
self.bridge = bridge
self.br = tf.TransformBroadcaster()
# bind method calls to subscribed topics
rospy.Subscriber("/pidrone/set_mode", Mode, self.mode_callback)
rospy.Subscriber("/pidrone/reset_transform", Empty,
self.reset_callback)
rospy.Subscriber("/pidrone/toggle_transform", Empty,
self.toggle_callback)
rospy.Subscriber("/pidrone/infrared", Range, self.range_callback)
rospy.Subscriber('/pidrone/angle', TwistStamped, self.angle_callback)
self.pospub = rospy.Publisher('/pidrone/set_mode_vel',
Mode,
queue_size=1)
self.first_image_pub = rospy.Publisher("/pidrone/picamera/first_image",
Image,
queue_size=1,
latch=True)
self.lr_pid = PIDaxis(10.0,
0.000,
0.0,
midpoint=0,
control_range=(-5.0, 5.0))
self.fb_pid = PIDaxis(10.0,
0.000,
0.0,
midpoint=0,
control_range=(-5.0, 5.0))
self.detector = cv2.ORB(nfeatures=NUM_FEATURES,
scoreType=cv2.ORB_FAST_SCORE
) # FAST_SCORE is a little faster to compute
map_grid_kp, map_grid_des = create_map('map.jpg')
self.estimator = LocalizationParticleFilter(map_grid_kp, map_grid_des)
self.first_locate = True
self.first_hold = True
self.prev_img = None
self.prev_kp = None
self.prev_des = None
self.locate_position = False
self.prev_time = None
self.prev_rostime = None
self.pos = [0, 0, 0]
self.yaw = 0.0
self.z = 0.075
self.iacc_yaw = 0.0
self.hold_position = False
self.target_pos = [0, 0, 0, 0]
self.target_yaw = 0.0
self.map_counter = 0
self.max_map_counter = 0
self.mode = Mode()
self.mode.mode = 5
# constant
self.kp_yaw = 50.0
self.ki_yaw = 0.1
self.alpha_yaw = 0.1 # perceived yaw smoothing alpha
self.hybrid_alpha = 0.3 # blend position with first frame and int
# angle
self.angle_x = 0.0 # the hz of state_controller is different
self.angle_y = 0.0
def write(self, data):
curr_img = np.reshape(np.fromstring(data, dtype=np.uint8),
(CAMERA_HEIGHT, CAMERA_WIDTH, 3))
curr_rostime = rospy.Time.now()
curr_time = curr_rostime.to_sec()
# start MCL localization
if self.locate_position:
curr_kp, curr_des = self.detector.detectAndCompute(curr_img, None)
if curr_kp is not None and curr_kp is not None:
# generate particles for the first time
if self.first_locate:
particle = self.estimator.initialize_particles(
NUM_PARTICLE, curr_kp, curr_des)
self.first_locate = False
self.pos = [
particle.x(),
particle.y(),
particle.z(),
particle.yaw()
]
self.yaw = particle.yaw()
print 'first', particle
else:
# get a estimate velocity over time
particle = self.estimator.update(self.z, self.angle_x,
self.angle_y,
self.prev_kp,
self.prev_des, curr_kp,
curr_des)
# update position
self.pos = [
self.hybrid_alpha * particle.x() +
(1.0 - self.hybrid_alpha) * self.pos[0],
self.hybrid_alpha * particle.y() +
(1.0 - self.hybrid_alpha) * self.pos[1], self.z
]
self.yaw = self.alpha_yaw * particle.yaw() + (
1.0 - self.alpha_yaw) * self.yaw
# print 'particle', particle
print '--pose', self.pos[0], self.pos[1], self.yaw
# if average particle weight is close to initial weight
if is_almost_equal(particle.weight(), PROB_THRESHOLD):
self.map_counter = self.map_counter - 1
elif self.map_counter <= 0:
self.map_counter = 1
else:
self.map_counter = min(self.map_counter + 1,
-MAX_BAD_COUNT)
# if it's been a while without a high average particle weight
if self.map_counter < MAX_BAD_COUNT:
self.first_locate = True
self.fb_pid._i = 0
self.lr_pid._i = 0
self.iacc_yaw = 0.0
self.map_counter = 0
self.mode.x_velocity = 0
self.mode.y_velocity = 0
self.mode.yaw_velocity = 0
self.pospub.publish(self.mode)
print 'Restart localization'
else:
if self.hold_position:
if self.first_hold:
self.target_pos = self.pos
self.target_yaw = 0 # rotate is not implement
self.first_hold = False
image_message = self.bridge.cv2_to_imgmsg(
curr_img, encoding="bgr8")
self.first_image_pub.publish(image_message)
else:
err_x = self.target_pos[0] - self.pos[0]
err_y = self.target_pos[1] - self.pos[1]
self.mode.x_velocity = self.lr_pid.step(
err_x, curr_time - self.prev_time)
self.mode.y_velocity = self.fb_pid.step(
err_y, curr_time - self.prev_time)
err_yaw = self.target_yaw - self.yaw
self.iacc_yaw += err_yaw * self.ki_yaw
self.mode.yaw_velocity = err_yaw * self.kp_yaw + self.iacc_yaw
self.pospub.publish(self.mode)
print '--target', self.target_pos[
0], self.target_pos[1], self.target_yaw
print 'count', self.map_counter
else:
print "CANNOT FIND ANY FEATURES !!!!!"
self.prev_kp = curr_kp
self.prev_des = curr_des
self.prev_img = curr_img
self.prev_time = curr_time
self.prev_rostime = curr_rostime
self.br.sendTransform(
(self.pos[0], self.pos[1], self.z),
tf.transformations.quaternion_from_euler(0, 0, self.yaw),
rospy.Time.now(), "base", "world")
# update angle data when '/pidrone/angle' is published to
def angle_callback(self, data):
self.angle_x = data.twist.angular.x
self.angle_y = data.twist.angular.y
# update z when '/pidrone/infrared' is published to
def range_callback(self, data):
if data.range != -1:
self.z = data.range
# start localization when '/pidrone/reset_transform' is published to (press 'r')
def reset_callback(self, data):
print "Start localization"
self.locate_position = True
self.first_locate = True
self.hold_position = False
self.map_counter = 0
self.max_map_counter = 0
# toggle position hold when '/pidrone/toggle_transform' is published to (press 'p')
def toggle_callback(self, data):
self.hold_position = not self.hold_position
self.first_hold = True
self.fb_pid._i = 0
self.lr_pid._i = 0
self.iacc_yaw = 0.0
print "Position hold", "enabled." if self.hold_position else "disabled."
# called whenever '/pidrone/set_mode' is published to, velocity mode is default
def mode_callback(self, data):
self.mode.mode = data.mode
if not self.hold_position or data.mode == 4 or data.mode == 3:
print "VELOCITY"
# TODO scale is not consistent, check index.html and pid_class.py
data.z_velocity = data.z_velocity * 100
self.pospub.publish(data)
else:
self.target_pos[0] += data.x_velocity / 100.
self.target_pos[1] += data.y_velocity / 100.
print "Target position", self.target_pos
def __init__(self):
# Initialize the current and desired modes
self.current_mode = Mode('DISARMED')
self.desired_mode = Mode('DISARMED')
# Initialize in velocity control
self.position_control = False
# Initialize the current and desired positions
self.current_position = Position()
self.desired_position = Position(z=0.3)
self.last_desired_position = Position(z=0.3)
# Initialize the position error
self.position_error = Error()
# Initialize the current and desired velocities
self.current_velocity = Velocity()
self.desired_velocity = Velocity()
# Initialize the velocity error
self.velocity_error = Error()
# Set the distance that a velocity command will move the drone (m)
self.desired_velocity_travel_distance = 0.1
# Set a static duration that a velocity command will be held
self.desired_velocity_travel_time = 0.1
# Set a static duration that a yaw velocity command will be held
self.desired_yaw_velocity_travel_time = 0.25
# Store the start time of the desired velocities
self.desired_velocity_start_time = None
self.desired_yaw_velocity_start_time = None
# Initialize the primary PID
self.pid = PID()
# Initialize the error used for the PID which is vx, vy, z where vx and
# vy are velocities, and z is the error in the altitude of the drone
self.pid_error = Error()
# Initialize the 'position error to velocity error' PIDs:
# left/right (roll) pid
self.lr_pid = PIDaxis(kp=10.0,
ki=0.5,
kd=5.0,
midpoint=0,
control_range=(-10.0, 10.0))
# front/back (pitch) pid
self.fb_pid = PIDaxis(kp=10.0,
ki=0.5,
kd=5.0,
midpoint=0,
control_range=(-10.0, 10.0))
# Initialize the pose callback time
self.last_pose_time = None
# Initialize the desired yaw velocity
self.desired_yaw_velocity = 0
# Initialize the current and previous roll, pitch, yaw values
self.current_rpy = RPY()
self.previous_rpy = RPY()
# initialize the current and previous states
self.current_state = State()
self.previous_state = State()
# Store the command publisher
self.cmdpub = None
# a variable used to determine if the drone is moving between disired
# positions
self.moving = False
# a variable that determines the maximum magnitude of the position error
# Any greater position error will overide the drone into velocity
# control
self.safety_threshold = 1.5
# determines if the position of the drone is known
self.lost = False
# determines if the desired poses are aboslute or relative to the drone
self.absolute_desired_position = False
# determines whether to use open loop velocity path planning which is
# accomplished by calculate_travel_time
self.path_planning = True
class PIDController(object):
''' Controls the flight of the drone by running a PID controller on the
error calculated by the desired and current velocity and position of the drone
'''
def __init__(self):
# Initialize the current and desired modes
self.current_mode = Mode('DISARMED')
self.desired_mode = Mode('DISARMED')
# Initialize in velocity control
self.position_control = False
# Initialize the current and desired positions
self.current_position = Position()
self.desired_position = Position(z=0.3)
self.last_desired_position = Position(z=0.3)
# Initialize the position error
self.position_error = Error()
# Initialize the current and desired velocities
self.current_velocity = Velocity()
self.desired_velocity = Velocity()
# Initialize the velocity error
self.velocity_error = Error()
# Set the distance that a velocity command will move the drone (m)
self.desired_velocity_travel_distance = 0.1
# Set a static duration that a velocity command will be held
self.desired_velocity_travel_time = 0.1
# Set a static duration that a yaw velocity command will be held
self.desired_yaw_velocity_travel_time = 0.25
# Store the start time of the desired velocities
self.desired_velocity_start_time = None
self.desired_yaw_velocity_start_time = None
# Initialize the primary PID
self.pid = PID()
# Initialize the error used for the PID which is vx, vy, z where vx and
# vy are velocities, and z is the error in the altitude of the drone
self.pid_error = Error()
# Initialize the 'position error to velocity error' PIDs:
# left/right (roll) pid
self.lr_pid = PIDaxis(kp=10.0,
ki=0.5,
kd=5.0,
midpoint=0,
control_range=(-10.0, 10.0))
# front/back (pitch) pid
self.fb_pid = PIDaxis(kp=10.0,
ki=0.5,
kd=5.0,
midpoint=0,
control_range=(-10.0, 10.0))
# Initialize the pose callback time
self.last_pose_time = None
# Initialize the desired yaw velocity
self.desired_yaw_velocity = 0
# Initialize the current and previous roll, pitch, yaw values
self.current_rpy = RPY()
self.previous_rpy = RPY()
# initialize the current and previous states
self.current_state = State()
self.previous_state = State()
# Store the command publisher
self.cmdpub = None
# a variable used to determine if the drone is moving between disired
# positions
self.moving = False
# a variable that determines the maximum magnitude of the position error
# Any greater position error will overide the drone into velocity
# control
self.safety_threshold = 1.5
# determines if the position of the drone is known
self.lost = False
# determines if the desired poses are aboslute or relative to the drone
self.absolute_desired_position = False
# determines whether to use open loop velocity path planning which is
# accomplished by calculate_travel_time
self.path_planning = True
# ROS SUBSCRIBER CALLBACK METHODS
#################################
def current_state_callback(self, state):
""" Store the drone's current state for calculations """
self.previous_state = self.current_state
self.current_state = state
self.state_to_three_dim_vec_structs()
def desired_pose_callback(self, msg):
""" Update the desired pose """
# store the previous desired position
self.last_desired_position = self.desired_position
# set the desired positions equal to the desired pose message
if self.absolute_desired_position:
self.desired_position.x = msg.position.x
self.desired_position.y = msg.position.y
# the desired z must be above z and below the range of the ir sensor (.55meters)
self.desired_position.z = msg.position.z if 0 <= desired_z <= 0.5 else self.last_desired_position.z
# set the desired positions relative to the current position (except for z to make it more responsive)
else:
self.desired_position.x = self.current_position.x + msg.position.x
self.desired_position.y = self.current_position.y + msg.position.y
# set the disired z position relative to the last desired position (doesn't limit the mag of the error)
# the desired z must be above z and below the range of the ir sensor (.55meters)
desired_z = self.last_desired_position.z + msg.position.z
self.desired_position.z = desired_z if 0 <= desired_z <= 0.5 else self.last_desired_position.z
if self.desired_position != self.last_desired_position:
# the drone is moving between desired positions
self.moving = True
print 'moving'
def desired_twist_callback(self, msg):
""" Update the desired twist """
self.desired_velocity.x = msg.linear.x
self.desired_velocity.y = msg.linear.y
self.desired_velocity.z = msg.linear.z
self.desired_yaw_velocity = msg.angular.z
self.desired_velocity_start_time = None
self.desired_yaw_velocity_start_time = None
if self.path_planning:
self.calculate_travel_time()
def current_mode_callback(self, msg):
""" Update the current mode """
self.current_mode = msg.mode
def desired_mode_callback(self, msg):
""" Update the desired mode """
self.desired_mode = msg.mode
def position_control_callback(self, msg):
""" Set whether or not position control is enabled """
self.position_control = msg.data
if self.position_control:
self.reset_callback(Empty())
print "Position Control", self.position_control
def reset_callback(self, empty):
""" Reset the desired and current poses of the drone and set
desired velocities to zero """
self.current_position = Position(z=self.current_position.z)
self.desired_position = Position(z=self.current_position.z)
self.desired_velocity.x = 0
self.desired_velocity.y = 0
def lost_callback(self, msg):
self.lost = msg.data
# Step Method
#############
def step(self):
""" Returns the commands generated by the pid """
self.calc_error()
if self.position_control:
if self.position_error.planar_magnitude(
) < self.safety_threshold and not self.lost:
if self.moving:
if self.position_error.magnitude() > 0.05:
self.pid_error -= self.velocity_error * 100
else:
self.moving = False
print 'not moving'
else:
self.position_control_pub.publish(False)
if self.desired_velocity.magnitude() > 0 or abs(
self.desired_yaw_velocity) > 0:
self.adjust_desired_velocity()
return self.pid.step(self.pid_error, self.desired_yaw_velocity)
# HELPER METHODS
################
def state_to_three_dim_vec_structs(self):
"""
Convert the values from the state estimator into ThreeDimVec structs to
make calculations concise
"""
# store the positions
pose = self.current_state.pose_with_covariance.pose
self.current_position.x = pose.position.x
self.current_position.y = pose.position.y
self.current_position.z = pose.position.z
# store the linear velocities
twist = self.current_state.twist_with_covariance.twist
self.current_velocity.x = twist.linear.x
self.current_velocity.y = twist.linear.y
self.current_velocity.z = twist.linear.z
# store the orientations
self.previous_rpy = self.current_rpy
quaternion = (pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w)
r, p, y = tf.transformations.euler_from_quaternion(quaternion)
self.current_rpy = RPY(r, p, y)
def adjust_desired_velocity(self):
""" Set the desired velocity back to 0 once the drone has traveled the
amount of time that causes it to move the specified desired velocity
travel distance if path_planning otherwise just set the velocities back
to 0 after the . This is an open loop method meaning that the specified
travel distance cannot be guarenteed. If path planning_planning is false,
just set the velocities back to zero, this allows the user to move the
drone for as long as they are holding down a key
"""
curr_time = rospy.get_time()
# set the desired planar velocities to zero if the duration is up
if self.desired_velocity_start_time is not None:
# the amount of time the set point velocity is not zero
duration = curr_time - self.desired_velocity_start_time
if duration > self.desired_velocity_travel_time:
self.desired_velocity.x = 0
self.desired_velocity.y = 0
self.desired_velocity_start_time = None
else:
self.desired_velocity_start_time = curr_time
# set the desired yaw velocity to zero if the duration is up
if self.desired_yaw_velocity_start_time is not None:
# the amount of time the set point velocity is not zero
duration = curr_time - self.desired_yaw_velocity_start_time
if duration > self.desired_yaw_velocity_travel_time:
self.desired_yaw_velocity = 0
self.desired_yaw_velocity_start_time = None
else:
self.desired_yaw_velocity_start_time = curr_time
def calc_error(self):
""" Calculate the error in velocity, and if in position hold, add the
error from lr_pid and fb_pid to the velocity error to control the
position of the drone
"""
# store the time difference
pose_dt = 0
if self.last_pose_time != None:
pose_dt = rospy.get_time() - self.last_pose_time
self.last_pose_time = rospy.get_time()
# calculate the velocity error
self.velocity_error = self.desired_velocity - self.current_velocity
# calculate the z position error
dz = self.desired_position.z - self.current_position.z
# calculate the pid_error from the above values
self.pid_error.x = self.velocity_error.x
self.pid_error.y = self.velocity_error.y
self.pid_error.z = dz
# multiply by 100 to account for the fact that code was originally written using cm
self.pid_error = self.pid_error * 100
if self.position_control:
# calculate the position error
self.position_error = self.desired_position - self.current_position
# calculate a value to add to the velocity error based based on the
# position error in the x (roll) direction
lr_step = self.lr_pid.step(self.position_error.x, pose_dt)
# calculate a value to add to the velocity error based based on the
# position error in the y (pitch) direction
fb_step = self.fb_pid.step(self.position_error.y, pose_dt)
self.pid_error.x += lr_step
self.pid_error.y += fb_step
def calculate_travel_time(self):
''' return the amount of time that desired velocity should be used to
calculate the error in order to move the drone the specified travel
distance for a desired velocity
'''
if self.desired_velocity.magnitude() > 0:
# tiime = distance / velocity
travel_time = self.desired_velocity_travel_distance / self.desired_velocity.planar_magnitude(
)
else:
travel_time = 0.0
self.desired_velocity_travel_time = travel_time
def reset(self):
''' Set desired_position to be current position, set
filtered_desired_velocity to be zero, and reset both the PositionPID
and VelocityPID
'''
# reset position control variables
self.position_error = Error(0, 0, 0)
self.desired_position = Position(self.current_position.x,
self.current_position.y, 0.3)
# reset velocity control_variables
self.velocity_error = Error(0, 0, 0)
self.desired_velocity = Velocity(0, 0, 0)
# reset the pids
self.pid.reset()
self.lr_pid.reset()
self.fb_pid.reset()
def ctrl_c_handler(self, signal, frame):
""" Gracefully handles ctrl-c """
print 'Caught ctrl-c\n Stopping Controller'
sys.exit()
def publish_cmd(self, cmd):
"""Publish the controls to /pidrone/fly_commands """
msg = RC()
msg.roll = cmd[0]
msg.pitch = cmd[1]
msg.yaw = cmd[2]
msg.throttle = cmd[3]
self.cmdpub.publish(msg)
def __init__(self):
self.bridge = CvBridge()
self.br = tf.TransformBroadcaster()
# self.lr_pid = PIDaxis(11.0, 0.001, 0.001, midpoint=0, control_range=(-5.0, 5.0))
# self.fb_pid = PIDaxis(11.0, 0.001, 0.001, midpoint=0, control_range=(-5.0, 5.0))
self.lr_pid = PIDaxis(9.0, 0.001, 0.001, midpoint=0, control_range=(-4.0, 4.0))
self.fb_pid = PIDaxis(9.0, 0.001, 0.001, midpoint=0, control_range=(-4.0, 4.0))
self.detector = cv2.ORB(nfeatures=200, scoreType=cv2.ORB_FAST_SCORE) # FAST_SCORE is a little faster to compute
map_grid_kp, map_grid_des = create_map('big_map.jpg')
self.estimator = LocalizationParticleFilter(map_grid_kp, map_grid_des)
self.first_locate = True
self.first_hold = True
self.prev_img = None
self.prev_kp = None
self.prev_des = None
self.locate_position = False
self.prev_time = None
self.prev_rostime = None
self.pos = [0, 0, 0]
self.yaw = 0.0
self.z = 0.075
self.set_z = INIZ_Z
self.iacc_yaw = 0.0
self.hold_position = False
self.target_pos = [0, 0, self.set_z]
self.target_point = Point()
self.target_yaw = 0.0
self.map_counter = 0
self.max_map_counter = 0
self.mode = Mode()
self.mode.mode = 5
# constant
self.kp_yaw = 40.0
self.ki_yaw = 0.1
self.alpha_yaw = 0.1 # perceived yaw smoothing alpha
self.hybrid_alpha = 0.75 # blend position with first frame and int
# angle
self.angle_x = 0.0 # the hz of state_controller is different
self.angle_y = 0.0
# TODO: current z position is controlled by state_controller
self.increment_z = 0
# path from MDP
self.path = []
self.path_index = -1
rospy.Subscriber("/pidrone/set_mode", Mode, self.mode_callback)
rospy.Subscriber('/pidrone/path', Float32MultiArray, self.path_callback)
rospy.Subscriber('/hololens/path', String, self.hololens_path_callback)
rospy.Subscriber("/pidrone/reset_transform", Empty, self.reset_callback)
rospy.Subscriber("/pidrone/toggle_transform", Empty, self.toggle_callback)
rospy.Subscriber("/pidrone/capture_photo", Empty, self.capture_callback)
rospy.Subscriber("/pidrone/infrared", Range, self.range_callback)
rospy.Subscriber('/pidrone/angle', TwistStamped, self.angle_callback)
rospy.Subscriber('/pidrone/picamera/image_raw', Image, self.image_callback)
self.pospub = rospy.Publisher('/pidrone/set_mode_vel', Mode, queue_size=1)
self.target_pos_pub = rospy.Publisher('/pidrone/drone_position', Point, queue_size=1)
self.first_image_pub = rospy.Publisher("/pidrone/picamera/first_image", Image, queue_size=1, latch=True)
self.captured_image_pub = rospy.Publisher("/pidrone/picamera/captured_image", CompressedImage, queue_size=1, latch=True)
def __init__(self):
self.bridge = CvBridge()
self.br = tf.TransformBroadcaster()
self.lr_pid = PIDaxis(10.0,
0.000,
0.0,
midpoint=0,
control_range=(-5.0, 5.0))
self.fb_pid = PIDaxis(10.0,
0.000,
0.0,
midpoint=0,
control_range=(-5.0, 5.0))
self.detector = cv2.ORB(nfeatures=NUM_FEATURES,
scoreType=cv2.ORB_FAST_SCORE)
self.estimator = FastSLAM()
self.first_locate = True
self.first_hold = True
self.prev_img = None
self.prev_kp = None
self.prev_des = None
self.locate_position = False
self.prev_time = None
self.prev_rostime = None
self.pos = [0, 0, 0]
self.yaw = 0.0
self.z = 0.075
self.iacc_yaw = 0.0
self.hold_position = False
self.target_pos = [0, 0, 0]
self.target_yaw = 0.0
self.map_counter = 0
self.max_map_counter = 0
self.mode = Mode()
self.mode.mode = 5
# constant
self.kp_yaw = 50.0
self.ki_yaw = 0.1
self.alpha_yaw = 0.1 # perceived yaw smoothing alpha
self.hybrid_alpha = 0.3 # blend position with first frame and int
# angle
self.angle_x = 0.0 # the hz of state_controller is different
self.angle_y = 0.0
rospy.Subscriber("/pidrone/set_mode", Mode, self.mode_callback)
rospy.Subscriber("/pidrone/reset_transform", Empty,
self.reset_callback)
rospy.Subscriber("/pidrone/toggle_transform", Empty,
self.toggle_callback)
rospy.Subscriber("/pidrone/infrared", Range, self.range_callback)
rospy.Subscriber('/pidrone/angle', TwistStamped, self.angle_callback)
rospy.Subscriber('/pidrone/picamera/image_raw', Image,
self.image_callback)
self.pospub = rospy.Publisher('/pidrone/set_mode_vel',
Mode,
queue_size=1)
self.first_image_pub = rospy.Publisher("/pidrone/picamera/first_image",
Image,
queue_size=1,
latch=True)
import copy
from pyquaternion import Quaternion
import time
from copy import deepcopy
from cv_bridge import CvBridge, CvBridgeError
import sys
from pid_class import PIDaxis
# Needed Variables
bridge = CvBridge()
first_img = None
first_kp = None
first_des = None
orb = cv2.ORB_create(nfeatures=500, nlevels=8, scaleFactor=1.1)
pospub = rospy.Publisher('/pidrone/set_mode', Mode, queue_size=1)
lr_pid = PIDaxis(-0.22, -0., -0., midpoint=0, control_range=(-15., 15.))
fb_pid = PIDaxis(0.22, 0., 0., midpoint=0, control_range=(-15., 15.))
lr_err = ERR()
fb_err = ERR()
prev_time = None
frame_count = 0
smoothed_pos = PoseStamped()
# Configuration Params
index_params = dict(algorithm = 6, table_number = 6,
key_size = 12, multi_probe_level = 1)
search_params = dict(checks = 50)
min_match_count = 10
camera_matrix = np.array([[ 250.0, 0., 160.0],
[ 0., 250.0, 120.0],
[ 0., 0., 1.]])
def __init__(self, camera, bridge):
picamera.array.PiMotionAnalysis.__init__(self, camera)
self.bridge = bridge
self.br = tf.TransformBroadcaster()
rospy.Subscriber("/pidrone/set_mode", Mode, self.mode_callback)
rospy.Subscriber("/pidrone/reset_transform", Empty,
self.reset_callback)
rospy.Subscriber("/pidrone/toggle_transform", Empty,
self.toggle_callback)
rospy.Subscriber("/pidrone/infrared", Range, self.range_callback)
rospy.Subscriber('/pidrone/angle_and_velocity', Twist,
self.angle_and_velocity_callback)
self.pospub = rospy.Publisher('/pidrone/set_mode_vel',
Mode,
queue_size=1)
self.first_image_pub = rospy.Publisher("/pidrone/picamera/first_image",
Image,
queue_size=1,
latch=True)
self.lr_pid = PIDaxis(10.0,
0.000,
1.0,
midpoint=0,
control_range=(-10.0, 10.0))
self.fb_pid = PIDaxis(10.0,
0.000,
1.0,
midpoint=0,
control_range=(-10.0, 10.0))
self.detector = cv2.ORB(nfeatures=120, scoreType=cv2.ORB_FAST_SCORE
) # FAST_SCORE is a little faster to compute
map_imgs = [
cv2.imread('map1.jpg'),
cv2.imread('map2.jpg'),
cv2.imread('map3.jpg'),
cv2.imread('map4.jpg')
]
map_detector = cv2.ORB(nfeatures=500, scoreType=cv2.ORB_FAST_SCORE)
# TODO use particle filter, so we can use more features which makes GridAdaptedFeatureDetector work better
# map_detector = cv2.GridAdaptedFeatureDetector(self.detector, maxTotalKeypoints=500) # find features evenly
self.map_index = 0
self.map_features = []
# save features for each map in an array
for map_img in map_imgs:
map_kp = map_detector.detect(map_img, None)
map_kp, map_des = self.detector.compute(map_img, map_kp)
if map_kp is None or map_des is None:
print "Map cannot be detect and compute !"
sys.exit()
else:
self.map_features.append((map_kp, map_des))
self.map_kp, self.map_des = self.map_features[0]
self.prev_img = None
index_params = dict(algorithm=6,
table_number=6,
key_size=12,
multi_probe_level=1)
search_params = dict(checks=50)
self.matcher = cv2.FlannBasedMatcher(index_params, search_params)
self.prev_kp = None
self.prev_des = None
self.first = True
self.prev_time = None
self.prev_rostime = None
self.pos = [0, 0]
self.z = 0.075
self.angle_x = 0.0 # the hz of state_controller is different
self.angle_y = 0.0
self.smoothed_yaw = 0.0
self.yaw_observed = 0.0
self.iacc_yaw = 0.0
self.transforming = False
self.target_pos = [0, 0]
self.map_counter = 0
self.max_map_counter = 0
self.map_missing_counter = 0
self.mode = Mode()
self.mode.mode = 5
# constant
self.kp_yaw = 100.0
self.ki_yaw = 0.1
self.alpha_yaw = 0.1 # perceived yaw smoothing alpha
self.hybrid_alpha = 0.2 # blend position with first frame and int
class AnalyzePhase(picamera.array.PiMotionAnalysis):
def __init__(self, camera, bridge):
picamera.array.PiMotionAnalysis.__init__(self, camera)
self.bridge = bridge
self.br = tf.TransformBroadcaster()
rospy.Subscriber("/pidrone/set_mode", Mode, self.mode_callback)
rospy.Subscriber("/pidrone/reset_transform", Empty,
self.reset_callback)
rospy.Subscriber("/pidrone/toggle_transform", Empty,
self.toggle_callback)
rospy.Subscriber("/pidrone/infrared", Range, self.range_callback)
rospy.Subscriber('/pidrone/angle_and_velocity', Twist,
self.angle_and_velocity_callback)
self.pospub = rospy.Publisher('/pidrone/set_mode_vel',
Mode,
queue_size=1)
self.first_image_pub = rospy.Publisher("/pidrone/picamera/first_image",
Image,
queue_size=1,
latch=True)
self.lr_pid = PIDaxis(10.0,
0.000,
1.0,
midpoint=0,
control_range=(-10.0, 10.0))
self.fb_pid = PIDaxis(10.0,
0.000,
1.0,
midpoint=0,
control_range=(-10.0, 10.0))
self.detector = cv2.ORB(nfeatures=120, scoreType=cv2.ORB_FAST_SCORE
) # FAST_SCORE is a little faster to compute
map_imgs = [
cv2.imread('map1.jpg'),
cv2.imread('map2.jpg'),
cv2.imread('map3.jpg'),
cv2.imread('map4.jpg')
]
map_detector = cv2.ORB(nfeatures=500, scoreType=cv2.ORB_FAST_SCORE)
# TODO use particle filter, so we can use more features which makes GridAdaptedFeatureDetector work better
# map_detector = cv2.GridAdaptedFeatureDetector(self.detector, maxTotalKeypoints=500) # find features evenly
self.map_index = 0
self.map_features = []
# save features for each map in an array
for map_img in map_imgs:
map_kp = map_detector.detect(map_img, None)
map_kp, map_des = self.detector.compute(map_img, map_kp)
if map_kp is None or map_des is None:
print "Map cannot be detect and compute !"
sys.exit()
else:
self.map_features.append((map_kp, map_des))
self.map_kp, self.map_des = self.map_features[0]
self.prev_img = None
index_params = dict(algorithm=6,
table_number=6,
key_size=12,
multi_probe_level=1)
search_params = dict(checks=50)
self.matcher = cv2.FlannBasedMatcher(index_params, search_params)
self.prev_kp = None
self.prev_des = None
self.first = True
self.prev_time = None
self.prev_rostime = None
self.pos = [0, 0]
self.z = 0.075
self.angle_x = 0.0 # the hz of state_controller is different
self.angle_y = 0.0
self.smoothed_yaw = 0.0
self.yaw_observed = 0.0
self.iacc_yaw = 0.0
self.transforming = False
self.target_pos = [0, 0]
self.map_counter = 0
self.max_map_counter = 0
self.map_missing_counter = 0
self.mode = Mode()
self.mode.mode = 5
# constant
self.kp_yaw = 100.0
self.ki_yaw = 0.1
self.alpha_yaw = 0.1 # perceived yaw smoothing alpha
self.hybrid_alpha = 0.2 # blend position with first frame and int
def write(self, data):
curr_img = np.reshape(np.fromstring(data, dtype=np.uint8),
(CAMERA_HEIGHT, CAMERA_WIDTH, 3))
curr_kp, curr_des = self.detector.detectAndCompute(curr_img, None)
curr_rostime = rospy.Time.now()
curr_time = curr_rostime.to_sec()
if self.transforming:
# cannot find matched features in current map, then change map
if self.map_missing_counter > MAX_MISSING_COUNT:
if self.map_index == len(self.map_features) - 1:
self.map_index = 0
else:
self.map_index += 1
self.map_kp, self.map_des = self.map_features[self.map_index]
print 'change to map', self.map_index
if self.first:
pos, yaw = self.feature_locate(curr_kp, curr_des)
if pos is not None and yaw is not None:
self.map_missing_counter = 0
self.pos = pos
self.yaw_observed = -yaw # take the opposite
self.smoothed_yaw = -yaw # take the opposite
self.target_pos = pos
self.first = False
image_message = self.bridge.cv2_to_imgmsg(curr_img,
encoding="bgr8")
self.first_image_pub.publish(image_message)
print "start and set pose", pos
else:
self.map_missing_counter += 1
print 'start or reset failed'
else:
pos, yaw = self.feature_locate(curr_kp, curr_des)
if pos is not None and yaw is not None:
self.map_counter += 1
self.max_map_counter = max(self.map_counter,
self.max_map_counter)
self.map_missing_counter = 0
self.pos = [
self.hybrid_alpha * pos[0] +
(1.0 - self.hybrid_alpha) * self.pos[0],
self.hybrid_alpha * pos[1] +
(1.0 - self.hybrid_alpha) * self.pos[1]
]
err_x = self.target_pos[0] - self.pos[0]
err_y = self.target_pos[1] - self.pos[1]
self.mode.x_velocity = self.lr_pid.step(
err_x, curr_time - self.prev_time)
self.mode.y_velocity = self.fb_pid.step(
err_y, curr_time - self.prev_time)
self.yaw_observed = -yaw # take the opposite
self.smoothed_yaw = (
1.0 - self.alpha_yaw
) * self.smoothed_yaw + self.alpha_yaw * self.yaw_observed
yaw = self.smoothed_yaw
self.iacc_yaw += yaw * self.ki_yaw
self.mode.yaw_velocity = yaw * self.kp_yaw + self.iacc_yaw
self.pospub.publish(self.mode)
print "MAP", self.max_map_counter, self.map_counter, "\nPOS", self.pos, "\nTARGET", self.target_pos
else:
# TODO or hover ?
self.map_counter = 0
self.map_missing_counter += 1
err_x = self.target_pos[0] - self.pos[0]
err_y = self.target_pos[1] - self.pos[1]
self.mode.x_velocity = self.lr_pid.step(
err_x, curr_time - self.prev_time) / 10. # less power
self.mode.y_velocity = self.fb_pid.step(
err_y, curr_time - self.prev_time) / 10. # less power
yaw = self.smoothed_yaw
self.iacc_yaw += yaw * self.ki_yaw
self.mode.yaw_velocity = (
yaw * self.kp_yaw + self.iacc_yaw) / 10. # less power
print "LOST !", "\nPOS", self.pos, "\nTARGET", self.target_pos
self.prev_img = curr_img
self.prev_kp = curr_kp
self.prev_des = curr_des
self.prev_time = curr_time
self.prev_rostime = curr_rostime
self.br.sendTransform(
(self.pos[0], self.pos[1], self.z),
tf.transformations.quaternion_from_euler(0, 0, self.yaw_observed),
rospy.Time.now(), "base", "world")
def feature_transform(self, kp1, des1, kp2, des2):
transform = None
if des1 is not None and des2 is not None:
matches = self.matcher.knnMatch(des1, des2, k=2)
good = []
for match in matches:
if len(match
) > 1 and match[0].distance < 0.7 * match[1].distance:
good.append(match[0])
src_pts = np.float32([kp1[m.queryIdx].pt
for m in good]).reshape(-1, 1, 2)
dst_pts = np.float32([kp2[m.trainIdx].pt
for m in good]).reshape(-1, 1, 2)
# estimateRigidTransform needs at least three pairs
if src_pts is not None and dst_pts is not None and len(
src_pts) > 3 and len(dst_pts) > 3:
transform = cv2.estimateRigidTransform(src_pts, dst_pts, False)
return transform
def feature_locate(self, kp, des):
"""
find features from current image,
match features with map,
compute the location.
:param kp: train kp
:param des: train des
:return: global x, y, and yaw
"""
pos = None
yaw = None
if des is not None:
matches = self.matcher.knnMatch(des, self.map_des, k=2)
# take from opencv tutorial
good = []
for match in matches:
if len(match
) > 1 and match[0].distance < 0.7 * match[1].distance:
good.append(match[0])
if len(good) > MIN_MATCH_COUNT:
src_pts = np.float32([kp[m.queryIdx].pt
for m in good]).reshape(-1, 1, 2)
dst_pts = np.float32([
self.map_kp[m.trainIdx].pt for m in good
]).reshape(-1, 1, 2)
transform = cv2.estimateRigidTransform(src_pts, dst_pts, False)
if transform is not None:
transformed_center = cv2.transform(
CAMERA_CENTER, transform) # get local pixel
transformed_center = [
transformed_center[0][0][0] /
float(METER_TO_PIXEL), # map to local pose
(MAP_HEIGHT - transformed_center[0][0][1]) /
float(METER_TO_PIXEL)
]
transformed_center = self.local_to_global(
transformed_center) # get the global pose
yaw = np.arctan2(transform[1, 0],
transform[0, 0]) # get global heading
# correct the pose if the drone is not level
z = math.sqrt(self.z**2 / (1 + math.tan(self.angle_x)**2 +
math.tan(self.angle_y)**2))
offset_x = np.tan(self.angle_x) * z
offset_y = np.tan(self.angle_y) * z
global_offset_x = math.cos(yaw) * offset_x + math.sin(
yaw) * offset_y
global_offset_y = math.sin(yaw) * offset_x + math.cos(
yaw) * offset_y
pos = [
transformed_center[0] + global_offset_x,
transformed_center[1] + global_offset_y
]
# else:
# print "Not enough matches are found - %d/%d" % (len(good), MIN_MATCH_COUNT)
return pos, yaw
def local_to_global(self, pos):
if self.map_index == 1:
pos[1] += MAP_REAL_HEIGHT
elif self.map_index == 2:
pos[0] += MAP_REAL_WIDTH
pos[1] += MAP_REAL_HEIGHT
elif self.map_index == 3:
pos[0] += MAP_REAL_WIDTH
return pos
def angle_and_velocity_callback(self, data):
self.angle_x = data.angular.x
self.angle_y = data.angular.y
def range_callback(self, data):
if data.range != -1:
self.z = data.range
def reset_callback(self, data):
print "Resetting position"
self.first = True
self.fb_pid._i = 0
self.lr_pid._i = 0
self.iacc_yaw = 0.0
self.map_counter = 0
self.max_map_counter = 0
self.map_missing_counter = 0
self.map_index = 0
self.map_kp, self.map_des = self.map_features[0]
def toggle_callback(self, data):
self.transforming = not self.transforming
print "Position hold", "enabled." if self.transforming else "disabled."
def mode_callback(self, data):
if not self.transforming or data.mode == 4 or data.mode == 3:
print "VELOCITY"
# TODO scale is not consistent, check index.html and pid_class.py
data.z_velocity = data.z_velocity * 100
self.pospub.publish(data)
else:
self.target_pos[0] += data.x_velocity / 100.
self.target_pos[1] += data.y_velocity / 100.
print "POSITION", self.target_pos
class AnalyzePhase:
def __init__(self):
self.bridge = CvBridge()
self.br = tf.TransformBroadcaster()
self.lr_pid = PIDaxis(12.5,
0.001,
0.0023,
midpoint=0,
control_range=(-5.0, 5.0))
self.fb_pid = PIDaxis(12.5,
0.001,
0.0023,
midpoint=0,
control_range=(-5.0, 5.0))
self.detector = cv2.ORB(nfeatures=500, scoreType=cv2.ORB_FAST_SCORE
) # FAST_SCORE is a little faster to compute
map_grid_kp, map_grid_des = create_map('map.jpg')
self.estimator = LocalizationParticleFilter(map_grid_kp, map_grid_des)
self.first_locate = True
self.first_hold = True
self.prev_img = None
self.prev_kp = None
self.prev_des = None
self.locate_position = False
self.prev_time = None
self.prev_rostime = None
self.pos = [0, 0, 0]
self.yaw = 0.0
self.z = 0.075
self.iacc_yaw = 0.0
self.hold_position = False
self.target_pos = [0, 0, 0]
self.target_yaw = 0.0
self.map_counter = 0
self.max_map_counter = 0
self.mode = Mode()
self.mode.mode = 5
# constant
self.kp_yaw = 50.0
self.ki_yaw = 0.1
self.alpha_yaw = 0.1 # perceived yaw smoothing alpha
self.hybrid_alpha = 0.3 # blend position with first frame and int
# angle
self.angle_x = 0.0 # the hz of state_controller is different
self.angle_y = 0.0
rospy.Subscriber("/pidrone/set_mode", Mode, self.mode_callback)
rospy.Subscriber("/pidrone/reset_transform", Empty,
self.reset_callback)
rospy.Subscriber("/pidrone/toggle_transform", Empty,
self.toggle_callback)
rospy.Subscriber("/pidrone/infrared", Range, self.range_callback)
rospy.Subscriber('/pidrone/angle', TwistStamped, self.angle_callback)
rospy.Subscriber('/pidrone/picamera/image_raw', Image,
self.image_callback)
self.pospub = rospy.Publisher('/pidrone/set_mode_vel',
Mode,
queue_size=1)
self.first_image_pub = rospy.Publisher("/pidrone/picamera/first_image",
Image,
queue_size=1,
latch=True)
def image_callback(self, data):
curr_img = self.bridge.imgmsg_to_cv2(data,
desired_encoding="passthrough")
curr_rostime = rospy.Time.now()
curr_time = curr_rostime.to_sec()
# start MC localization
if self.locate_position:
curr_kp, curr_des = self.detector.detectAndCompute(curr_img, None)
if curr_kp is not None and curr_kp is not None:
# generate particles for the first time
if self.first_locate:
particle = self.estimator.initialize_particles(
NUM_PARTICLE, curr_kp, curr_des)
self.first_locate = False
self.pos = particle.position[0:3]
self.yaw = particle.position[3]
print 'first', particle
else:
# get a estimate velocity over time
particle = self.estimator.update(self.z, self.angle_x,
self.angle_y,
self.prev_kp,
self.prev_des, curr_kp,
curr_des)
# update position
self.pos = [
self.hybrid_alpha * particle.position[0] +
(1.0 - self.hybrid_alpha) * self.pos[0],
self.hybrid_alpha * particle.position[1] +
(1.0 - self.hybrid_alpha) * self.pos[1], self.z
]
self.yaw = self.alpha_yaw * particle.position[3] + (
1.0 - self.alpha_yaw) * self.yaw
# print 'particle', particle
print '--pose', self.pos[0], self.pos[1], self.yaw
# if all particles are not good estimations
if is_almost_equal(particle.weight, PROB_THRESHOLD):
self.map_counter = self.map_counter - 1
elif self.map_counter <= 0:
self.map_counter = 1
else:
self.map_counter = min(self.map_counter + 1,
-MAX_BAD_COUNT)
# if no particles are good estimations, we should restart
if self.map_counter < MAX_BAD_COUNT:
self.first_locate = True
self.fb_pid._i = 0
self.lr_pid._i = 0
self.iacc_yaw = 0.0
self.map_counter = 0
self.mode.x_velocity = 0
self.mode.y_velocity = 0
self.mode.yaw_velocity = 0
self.pospub.publish(self.mode)
print 'Restart localization'
else:
if self.hold_position:
if self.first_hold:
self.target_pos = self.pos
self.target_yaw = 0 # rotate is not implement
self.first_hold = False
image_message = self.bridge.cv2_to_imgmsg(
curr_img, encoding="bgr8")
self.first_image_pub.publish(image_message)
else:
err_x = self.target_pos[0] - self.pos[0]
err_y = self.target_pos[1] - self.pos[1]
self.mode.x_velocity = self.lr_pid.step(
err_x, curr_time - self.prev_time)
self.mode.y_velocity = self.fb_pid.step(
err_y, curr_time - self.prev_time)
err_yaw = self.target_yaw - self.yaw
self.iacc_yaw += err_yaw * self.ki_yaw
self.mode.yaw_velocity = err_yaw * self.kp_yaw + self.iacc_yaw
self.pospub.publish(self.mode)
print '--target', self.target_pos[
0], self.target_pos[1], self.target_yaw
print 'count', self.map_counter
else:
print "CANNOT FIND ANY FEATURES !!!!!"
self.prev_kp = curr_kp
self.prev_des = curr_des
self.prev_time = curr_time
self.prev_rostime = curr_rostime
self.br.sendTransform(
(self.pos[0], self.pos[1], self.z),
tf.transformations.quaternion_from_euler(0, 0, self.yaw),
rospy.Time.now(), "base", "world")
# the angle is just estimate
def angle_callback(self, data):
self.angle_x = data.twist.angular.x
self.angle_y = data.twist.angular.y
def range_callback(self, data):
if data.range != -1:
self.z = data.range
def reset_callback(self, data):
print "Start localization"
self.locate_position = True
self.first_locate = True
self.hold_position = False
self.map_counter = 0
self.max_map_counter = 0
def toggle_callback(self, data):
self.hold_position = not self.hold_position
self.first_hold = True
self.fb_pid._i = 0
self.lr_pid._i = 0
self.iacc_yaw = 0.0
print "Position hold", "enabled." if self.hold_position else "disabled."
def mode_callback(self, data):
if not self.hold_position or data.mode == 4 or data.mode == 3:
print "VELOCITY"
# TODO scale is not consistent, check index.html and pid_class.py
data.z_velocity = data.z_velocity * 100
self.pospub.publish(data)
else:
self.target_pos[0] += data.x_velocity / 100.
self.target_pos[1] += data.y_velocity / 100.
print "Target position", self.target_pos
class AnalyzePhase(picamera.array.PiMotionAnalysis):
def __init__(self, camera, bridge):
picamera.array.PiMotionAnalysis.__init__(self, camera)
self.bridge = bridge
self.br = tf.TransformBroadcaster()
rospy.Subscriber("/pidrone/set_mode", Mode, self.mode_callback)
rospy.Subscriber("/pidrone/reset_transform", Empty, self.reset_callback)
rospy.Subscriber("/pidrone/toggle_transform", Empty, self.toggle_callback)
rospy.Subscriber("/pidrone/infrared", Range, self.range_callback)
self.pospub = rospy.Publisher('/pidrone/set_mode_vel', Mode, queue_size=1)
self.first_image_pub = rospy.Publisher("/pidrone/picamera/first_image", Image, queue_size=1, latch=True)
self.lr_pid = PIDaxis(4.00, -0.00000, 0.000, midpoint=0, control_range=(-10.0, 10.0))
self.fb_pid = PIDaxis(4.00, 0.0000, -0.000, midpoint=0, control_range=(-10.0, 10.0))
self.prev_img = None
self.detector = cv2.ORB(nfeatures=120, scoreType=cv2.ORB_FAST_SCORE)
self.index_params = dict(algorithm=6, table_number=6, key_size=12, multi_probe_level=1)
self.search_params = dict(checks=50)
self.matcher = cv2.FlannBasedMatcher(self.index_params, self.search_params)
self.first_kp = None
self.first_des = None
self.prev_kp = None
self.prev_des = None
self.first = True
self.lr_err = ERR()
self.fb_err = ERR()
self.prev_time = None
self.prev_rostime = None
self.pos = [0, 0]
self.z = 0.075
self.smoothed_yaw = 0.0
self.yaw_observed = 0.0
self.iacc_yaw = 0.0
self.transforming = False
self.last_first_time = None
self.target_x = 0
self.target_y = 0
self.first_counter = 0
self.max_first_counter = 0
self.mode = Mode()
# constant
self.kp_yaw = 80.0
self.ki_yaw = 0.1
self.kpi_yaw = 15.0
self.kpi_max_yaw = 0.01
self.alpha_yaw = 0.1 # perceived yaw smoothing alpha
self.hybrid_alpha = 0.1 # blend position with first frame and int
def write(self, data):
curr_img = np.reshape(np.fromstring(data, dtype=np.uint8), (240, 320, 3))
curr_kp, curr_des = self.detector.detectAndCompute(curr_img, None)
curr_rostime = rospy.Time.now()
curr_time = curr_rostime.to_sec()
if self.first:
print "taking new first"
self.first = False
self.first_kp = curr_kp
self.first_des = curr_des
self.last_first_time = curr_time
image_message = self.bridge.cv2_to_imgmsg(curr_img, encoding="bgr8")
self.first_image_pub.publish(image_message)
elif self.transforming:
# move the drone to the right, x is negative.
# move the drone to the front, y is positive.
corr_first = self.feature_transform(self.first_kp, self.first_des, curr_kp, curr_des)
if corr_first is not None:
self.last_first_time = curr_time
self.first_counter += 1
self.max_first_counter = max(self.first_counter, self.max_first_counter)
# X and Y
first_displacement = [-corr_first[0, 2] / 280., corr_first[1, 2] / 280.]
self.pos = [
self.hybrid_alpha * first_displacement[0] * self.z + (1.0 - self.hybrid_alpha) * self.pos[0],
self.hybrid_alpha * first_displacement[1] * self.z + (1.0 - self.hybrid_alpha) * self.pos[1]]
self.lr_err.err = self.target_x - self.pos[0]
self.fb_err.err = self.target_y - self.pos[1]
print "ERR", self.lr_err.err, self.fb_err.err
self.mode.x_i = self.lr_pid.step(self.lr_err.err, curr_time - self.prev_time)
self.mode.y_i = self.fb_pid.step(self.fb_err.err, curr_time - self.prev_time)
cvc_vel = 1.2
self.mode.x_velocity = cvc_vel * self.mode.x_i
self.mode.y_velocity = cvc_vel * self.mode.y_i
# Yaw
scalex = np.linalg.norm(corr_first[:, 0])
scalez = np.linalg.norm(corr_first[:, 1])
corr_first[:, 0] /= scalex
corr_first[:, 1] /= scalez
self.yaw_observed = np.arctan2(corr_first[1, 0], corr_first[0, 0])
self.smoothed_yaw = (1.0 - self.alpha_yaw) * self.smoothed_yaw + self.alpha_yaw * self.yaw_observed
yaw = self.smoothed_yaw
self.iacc_yaw += yaw * self.ki_yaw
yaw_kpi_term = np.sign(yaw) * yaw * yaw * self.kpi_yaw
if abs(yaw_kpi_term) < self.kpi_max_yaw:
self.iacc_yaw += yaw_kpi_term
else:
self.iacc_yaw += np.sign(yaw) * self.kpi_max_yaw
self.mode.yaw_velocity = yaw * self.kp_yaw + self.iacc_yaw
print "yaw iacc: ", self.iacc_yaw
self.pospub.publish(self.mode)
print "first", self.max_first_counter, self.first_counter
else:
self.first_counter = 0
corr_int = self.feature_transform(self.prev_kp, self.prev_des, curr_kp, curr_des)
if corr_int is not None:
time_since_first = curr_time - self.last_first_time
print "integrated", time_since_first
print "max_first_counter: ", self.max_first_counter
# X and Y
int_displacement = [-corr_int[0, 2] / 280., corr_int[1, 2] / 280.]
self.pos[0] += int_displacement[0] * self.z
self.pos[1] += int_displacement[1] * self.z
self.lr_err.err = self.target_x - self.pos[0]
self.fb_err.err = self.target_y - self.pos[1]
print "ERR", self.lr_err.err, self.fb_err.err
self.mode.x_i = self.lr_pid.step(self.lr_err.err, curr_time - self.prev_time)
self.mode.y_i = self.fb_pid.step(self.fb_err.err, curr_time - self.prev_time)
cvc_vel = 1.3 # 0.25 #1.0
self.mode.x_velocity = cvc_vel * self.mode.x_i
self.mode.y_velocity = cvc_vel * self.mode.y_i
# Yaw
self.mode.yaw_velocity = self.iacc_yaw
print "yaw iacc: ", self.iacc_yaw
self.pospub.publish(self.mode)
else:
print "LOST"
else:
corr_first = self.feature_transform(self.first_kp, self.first_des, curr_kp, curr_des)
if corr_first is not None:
self.last_first_time = rospy.get_time()
if curr_time - self.last_first_time > 2:
self.first = True
else:
print "no first", curr_time - self.last_first_time
self.prev_img = curr_img
self.prev_kp = curr_kp
self.prev_des = curr_des
self.prev_time = curr_time
self.prev_rostime = curr_rostime
# TODO should be z instead of pos[2], which has never been updated
self.br.sendTransform((self.pos[0], self.pos[1], self.z),
tf.transformations.quaternion_from_euler(0, 0, self.yaw_observed),
rospy.Time.now(),
"base",
"world")
def feature_transform(self, kp1, des1, kp2, des2):
transform = None
if des1 is not None and des2 is not None:
matches = self.matcher.knnMatch(des1, des2, k=2)
good = []
for match in matches:
if len(match) > 1 and match[0].distance < 0.7 * match[1].distance:
good.append(match[0])
src_pts = np.float32([kp1[m.queryIdx].pt for m in good]).reshape(-1, 1, 2)
dst_pts = np.float32([kp2[m.trainIdx].pt for m in good]).reshape(-1, 1, 2)
# estimateRigidTransform needs at least three pairs
if src_pts is not None and dst_pts is not None and len(src_pts) > 3 and len(dst_pts) > 3:
transform = cv2.estimateRigidTransform(src_pts, dst_pts, False)
return transform
def range_callback(self, data):
if data.range != -1:
self.z = data.range
def reset_callback(self, data):
print "Resetting Phase"
self.first = True
self.pos = [0, 0]
self.fb_pid._i = 0
self.lr_pid._i = 0
self.target_x = 0
self.target_y = 0
self.smoothed_yaw = 0.0
self.iacc_yaw = 0.0
self.first_counter = 0
self.max_first_counter = 0
def toggle_callback(self, data):
self.transforming = not self.transforming
print "Position hold", "enabled." if self.transforming else "disabled."
def mode_callback(self, data):
if not self.transforming or data.mode == 4 or data.mode == 3:
print "VELOCITY"
self.pospub.publish(data)
else:
# TODO 4 is used for what ? Should the target be accumulated ?
self.target_x = data.x_velocity * 4.
self.target_y = data.y_velocity * 4.
print "POSITION", self.target_x, self.target_y
class AnalyzePhase:
def __init__(self):
self.bridge = CvBridge()
self.br = tf.TransformBroadcaster()
# self.lr_pid = PIDaxis(11.0, 0.001, 0.001, midpoint=0, control_range=(-5.0, 5.0))
# self.fb_pid = PIDaxis(11.0, 0.001, 0.001, midpoint=0, control_range=(-5.0, 5.0))
self.lr_pid = PIDaxis(9.0, 0.001, 0.001, midpoint=0, control_range=(-4.0, 4.0))
self.fb_pid = PIDaxis(9.0, 0.001, 0.001, midpoint=0, control_range=(-4.0, 4.0))
self.detector = cv2.ORB(nfeatures=200, scoreType=cv2.ORB_FAST_SCORE) # FAST_SCORE is a little faster to compute
map_grid_kp, map_grid_des = create_map('big_map.jpg')
self.estimator = LocalizationParticleFilter(map_grid_kp, map_grid_des)
self.first_locate = True
self.first_hold = True
self.prev_img = None
self.prev_kp = None
self.prev_des = None
self.locate_position = False
self.prev_time = None
self.prev_rostime = None
self.pos = [0, 0, 0]
self.yaw = 0.0
self.z = 0.075
self.set_z = INIZ_Z
self.iacc_yaw = 0.0
self.hold_position = False
self.target_pos = [0, 0, self.set_z]
self.target_point = Point()
self.target_yaw = 0.0
self.map_counter = 0
self.max_map_counter = 0
self.mode = Mode()
self.mode.mode = 5
# constant
self.kp_yaw = 40.0
self.ki_yaw = 0.1
self.alpha_yaw = 0.1 # perceived yaw smoothing alpha
self.hybrid_alpha = 0.75 # blend position with first frame and int
# angle
self.angle_x = 0.0 # the hz of state_controller is different
self.angle_y = 0.0
# TODO: current z position is controlled by state_controller
self.increment_z = 0
# path from MDP
self.path = []
self.path_index = -1
rospy.Subscriber("/pidrone/set_mode", Mode, self.mode_callback)
rospy.Subscriber('/pidrone/path', Float32MultiArray, self.path_callback)
rospy.Subscriber('/hololens/path', String, self.hololens_path_callback)
rospy.Subscriber("/pidrone/reset_transform", Empty, self.reset_callback)
rospy.Subscriber("/pidrone/toggle_transform", Empty, self.toggle_callback)
rospy.Subscriber("/pidrone/capture_photo", Empty, self.capture_callback)
rospy.Subscriber("/pidrone/infrared", Range, self.range_callback)
rospy.Subscriber('/pidrone/angle', TwistStamped, self.angle_callback)
rospy.Subscriber('/pidrone/picamera/image_raw', Image, self.image_callback)
self.pospub = rospy.Publisher('/pidrone/set_mode_vel', Mode, queue_size=1)
self.target_pos_pub = rospy.Publisher('/pidrone/drone_position', Point, queue_size=1)
self.first_image_pub = rospy.Publisher("/pidrone/picamera/first_image", Image, queue_size=1, latch=True)
self.captured_image_pub = rospy.Publisher("/pidrone/picamera/captured_image", CompressedImage, queue_size=1, latch=True)
def image_callback(self, data):
curr_img = self.bridge.imgmsg_to_cv2(data, desired_encoding="passthrough")
self.prev_img = curr_img
curr_rostime = rospy.Time.now()
curr_time = curr_rostime.to_sec()
# process path from MDP
if len(self.path) != 0 and is_close_to(self.pos, self.target_pos, distance=0.15):
if self.path[self.path_index + 1] == -10: # take photo
if is_close_to(self.pos, self.target_pos, distance=0.1):
# image_message = self.bridge.cv2_to_imgmsg(curr_img, encoding="bgr8")
# self.first_image_pub.publish(image_message)
image_message = self.bridge.cv2_to_compressed_imgmsg(curr_img)
self.captured_image_pub.publish(image_message)
self.path_index += 1
else: # take action
if self.increment_z == 0:
self.target_pos[0] += self.path[self.path_index + 1]
self.target_pos[1] += self.path[self.path_index + 2]
self.increment_z = self.path[self.path_index + 3] * 100.
self.target_pos[2] += self.path[self.path_index + 3]
self.path_index += 3
if self.path_index == len(self.path) - 1: # reset path
self.path = []
self.path_index = -1
# start MC localization
if self.locate_position:
curr_kp, curr_des = self.detector.detectAndCompute(curr_img, None)
if curr_kp is not None and curr_kp is not None:
# generate particles for the first time
if self.first_locate:
particle = self.estimator.initialize_particles(NUM_PARTICLE, curr_kp, curr_des)
self.first_locate = False
self.pos = particle.position[0:3]
self.pos[1] += 0.05 # camera offset
self.yaw = particle.position[3]
print 'first', particle
else:
# get a estimate velocity over time
particle = self.estimator.update(self.z, self.angle_x, self.angle_y, self.prev_kp, self.prev_des,
curr_kp, curr_des)
# update position
self.pos = [self.hybrid_alpha * particle.position[0] + (1.0 - self.hybrid_alpha) * self.pos[0],
self.hybrid_alpha * particle.position[1] + (1.0 - self.hybrid_alpha) * self.pos[1],
self.z]
self.yaw = self.alpha_yaw * particle.position[3] + (1.0 - self.alpha_yaw) * self.yaw
# print 'particle', particle
print '--pose', str(round(self.pos[0], 3)), str(round(self.pos[1], 3)), str(round(self.pos[2], 3)), self.yaw
# if all particles are not good estimations
if is_almost_equal(particle.weight, PROB_THRESHOLD):
self.map_counter = self.map_counter - 1
elif self.map_counter <= 0:
self.map_counter = 1
else:
self.map_counter = min(self.map_counter + 1, -MAX_BAD_COUNT)
# if no particles are good estimations, we should restart
if self.map_counter < MAX_BAD_COUNT:
self.first_locate = True
self.fb_pid._i = 0
self.lr_pid._i = 0
self.iacc_yaw = 0.0
self.map_counter = 0
self.mode.x_velocity = 0
self.mode.y_velocity = 0
self.mode.z_velocity = 0
self.mode.yaw_velocity = 0
self.pospub.publish(self.mode)
print 'Restart localization'
else:
if self.hold_position:
if self.first_hold:
self.target_pos[0] = (int)(self.pos[0] / CELL_DISTANCE) * CELL_DISTANCE + CELL_DISTANCE / 2.
self.target_pos[1] = (int)(self.pos[1] / CELL_DISTANCE) * CELL_DISTANCE + CELL_DISTANCE / 2.
self.target_pos[2] = self.set_z
self.target_yaw = 0 # rotate is not implement
self.first_hold = False
# image_message = self.bridge.cv2_to_imgmsg(curr_img, encoding="bgr8")
# self.first_image_pub.publish(image_message)
else:
err_x = self.target_pos[0] - self.pos[0]
err_y = self.target_pos[1] - self.pos[1]
self.mode.x_velocity = self.lr_pid.step(err_x, curr_time - self.prev_time)
self.mode.y_velocity = self.fb_pid.step(err_y, curr_time - self.prev_time)
self.mode.z_velocity = self.increment_z
self.increment_z = 0
err_yaw = self.target_yaw - self.yaw
self.iacc_yaw += err_yaw * self.ki_yaw
self.mode.yaw_velocity = err_yaw * self.kp_yaw + self.iacc_yaw
self.pospub.publish(self.mode)
self.target_point.x = self.target_pos[0]
self.target_point.y = self.target_pos[1]
self.target_point.z = self.target_pos[2]
self.target_pos_pub.publish(self.target_point)
print '--target', self.target_pos[0], self.target_pos[1], self.target_pos[2], self.target_yaw
print 'count', self.map_counter
else:
print "CANNOT FIND ANY FEATURES !!!!!"
self.prev_kp = curr_kp
self.prev_des = curr_des
self.prev_time = curr_time
self.prev_rostime = curr_rostime
self.br.sendTransform((self.pos[0], self.pos[1], self.z),
tf.transformations.quaternion_from_euler(0, 0, self.yaw),
rospy.Time.now(),
"base",
"world")
# the angle is just estimate
def angle_callback(self, data):
#self.angle_x = data.twist.angular.x
#self.angle_y = data.twist.angular.y
self.angle_x = 0
self.angle_y = 0
def range_callback(self, data):
if data.range != -1:
self.z = data.range
def reset_callback(self, data):
print "Start localization"
self.set_z = self.target_pos[2]
self.path = []
self.path_index = -1
self.locate_position = True
self.first_locate = True
self.hold_position = False
self.first_hold = True
self.fb_pid._i = 0
self.lr_pid._i = 0
self.iacc_yaw = 0.0
self.map_counter = 0
self.max_map_counter = 0
def toggle_callback(self, data):
self.hold_position = not self.hold_position
self.first_hold = True
self.fb_pid._i = 0
self.lr_pid._i = 0
self.iacc_yaw = 0.0
print "Position hold", "enabled." if self.hold_position else "disabled."
def capture_callback(self, data):
image_message = self.bridge.cv2_to_imgmsg(self.prev_img, encoding="bgr8")
self.first_image_pub.publish(image_message)
image_message = self.bridge.cv2_to_compressed_imgmsg(self.prev_img)
self.captured_image_pub.publish(image_message)
def mode_callback(self, data):
if not self.hold_position or data.mode == 4 or data.mode == 3:
print "VELOCITY"
# TODO scale is not consistent, check index.html and pid_class.py
data.z_velocity = data.z_velocity * 100.
self.pospub.publish(data)
else:
self.target_pos[0] += data.x_velocity / 100. * 5
self.target_pos[1] += data.y_velocity / 100. * 5
# TODO scale is not consistent, check index.html and pid_class.py
if self.increment_z == 0:
self.target_pos[2] += data.z_velocity
self.increment_z = data.z_velocity * 100.
print "Target position", self.target_pos
def path_callback(self, data):
self.path = data.data
print(self.path)
def hololens_path_callback(self, data):
self.path = [float(str(round(float(i), 2))) for i in (data.data).split()]
print(self.path)
class AnalyzePhase(picamera.array.PiMotionAnalysis):
def __init__(self, camera, bridge):
picamera.array.PiMotionAnalysis.__init__(self, camera)
self.bridge = bridge
self.br = tf.TransformBroadcaster()
rospy.Subscriber("/pidrone/set_mode", Mode, self.mode_callback)
rospy.Subscriber("/pidrone/reset_transform", Empty,
self.reset_callback)
rospy.Subscriber("/pidrone/toggle_transform", Empty,
self.toggle_callback)
rospy.Subscriber("/pidrone/infrared", Range, self.range_callback)
rospy.Subscriber('/pidrone/angle', TwistStamped, self.angle_callback)
rospy.Subscriber('/pidrone/plane_err', TwistStamped,
self.picam_velocity_callback)
self.pospub = rospy.Publisher('/pidrone/set_mode_vel',
Mode,
queue_size=1)
self.first_image_pub = rospy.Publisher("/pidrone/picamera/first_image",
Image,
queue_size=1,
latch=True)
self.lr_pid = PIDaxis(20.0,
0.000,
2.0,
midpoint=0,
control_range=(-10.0, 10.0))
self.fb_pid = PIDaxis(20.0,
0.000,
2.0,
midpoint=0,
control_range=(-10.0, 10.0))
self.detector = cv2.ORB(nfeatures=100, scoreType=cv2.ORB_FAST_SCORE
) # FAST_SCORE is a little faster to compute
map_grid_kp, map_grid_des = create_map('map.jpg')
self.estimator = LocalizationParticleFilter(map_grid_kp, map_grid_des)
self.first_locate = True
self.first_hold = True
self.prev_img = None
self.locate_position = False
self.prev_velocity_time = None
self.prev_time = None
self.prev_rostime = None
self.pos = [0, 0, 0]
self.yaw = 0.0
self.z = 0.075
self.iacc_yaw = 0.0
self.hold_position = False
self.target_pos = [0, 0, 0]
self.target_yaw = 0.0
self.map_counter = 0
self.max_map_counter = 0
self.mode = Mode()
self.mode.mode = 5
# constant
self.kp_yaw = 70.0
self.ki_yaw = 0.1
self.alpha_yaw = 0.1 # perceived yaw smoothing alpha
self.hybrid_alpha = 0.2 # blend position with first frame and int
# angle, velocity
self.angle_x = 0.0 # the hz of state_controller is different
self.angle_y = 0.0
self.angle_prev_time = None
self.mw_angle_comp_x = 0.0
self.mw_angle_comp_y = 0.0
self.velocities_x = []
self.velocities_y = []
self.velocities_z = []
self.velocities_yaw = []
self.alpha_yaw = 0.1 # perceived yaw smoothing alpha
self.hybrid_alpha = 0.2 # blend position with first frame and int
def write(self, data):
curr_img = np.reshape(np.fromstring(data, dtype=np.uint8),
(CAMERA_HEIGHT, CAMERA_WIDTH, 3))
curr_rostime = rospy.Time.now()
curr_time = curr_rostime.to_sec()
# start MCL localization
if self.locate_position:
curr_kp, curr_des = self.detector.detectAndCompute(curr_img, None)
if curr_kp is not None and curr_kp is not None:
# generate particles for the first time
if self.first_locate:
particle = self.estimator.initialize_particles(
NUM_PARTICLE, curr_kp, curr_des)
self.first_locate = False
self.pos = particle.position[0:3]
self.yaw = particle.position[3]
print 'first', particle
else:
# get a estimate velocity over time
velocity = [
np.average(self.velocities_x),
np.average(self.velocities_y),
np.average(self.velocities_z),
np.average(self.velocities_yaw)
]
self.velocities_x = []
self.velocities_y = []
self.velocities_z = []
self.velocities_yaw = []
if self.prev_velocity_time is None:
self.prev_velocity_time = curr_time
curr_velocity_time = time.time()
delta_time = curr_velocity_time - self.prev_velocity_time
self.prev_velocity_time = curr_velocity_time
particle = self.estimator.update(self.z, self.angle_x,
self.angle_y, velocity,
delta_time, curr_kp,
curr_des)
# update position
self.pos = [
self.hybrid_alpha * particle.position[0] +
(1.0 - self.hybrid_alpha) * self.pos[0],
self.hybrid_alpha * particle.position[1] +
(1.0 - self.hybrid_alpha) * self.pos[1], self.z
]
self.yaw = self.alpha_yaw * particle.position[3] + (
1.0 - self.alpha_yaw) * self.yaw
print 'particle', particle
# if all particles are not good estimations
if is_almost_equal(particle.weight, PROB_THRESHOLD):
self.map_counter = self.map_counter - 1
elif self.map_counter <= 0:
self.map_counter = 1
else:
self.map_counter = min(self.map_counter + 1,
-MAX_BAD_COUNT)
print 'count', self.map_counter
# if no particles are good estimations, we should restart
if self.map_counter < MAX_BAD_COUNT:
self.first_locate = True
self.map_counter = 0
print 'Restart localization'
else:
if self.hold_position:
if self.first_hold:
self.target_pos = self.pos
self.target_yaw = 0 # rotate is not implement
self.first_hold = False
image_message = self.bridge.cv2_to_imgmsg(
curr_img, encoding="bgr8")
self.first_image_pub.publish(image_message)
else:
err_x = self.target_pos[0] - self.pos[0]
err_y = self.target_pos[1] - self.pos[1]
self.mode.x_velocity = self.lr_pid.step(
err_x, curr_time - self.prev_time)
self.mode.y_velocity = self.fb_pid.step(
err_y, curr_time - self.prev_time)
err_yaw = self.target_yaw - self.yaw
self.iacc_yaw += err_yaw * self.ki_yaw
self.mode.yaw_velocity = err_yaw * self.kp_yaw + self.iacc_yaw
self.pospub.publish(self.mode)
print 'pose', self.pos, self.yaw, '\ntarget', self.target_pos, self.target_yaw
else:
print "CANNOT FIND ANY FEATURES !!!!!"
self.prev_img = curr_img
self.prev_time = curr_time
self.prev_rostime = curr_rostime
self.br.sendTransform(
(self.pos[0], self.pos[1], self.z),
tf.transformations.quaternion_from_euler(0, 0, self.yaw),
rospy.Time.now(), "base", "world")
# the angle is just estimate
def angle_callback(self, data):
# take from state_controller.py
curr_time = data.header.stamp.to_sec()
if self.angle_prev_time is None:
self.angle_prev_time = curr_time
self.mw_angle_comp_x = np.tan(
(data.twist.angular.x - self.angle_x) *
(curr_time - self.angle_prev_time)) * 10.0
self.mw_angle_comp_y = np.tan(
(data.twist.angular.y - self.angle_y) *
(curr_time - self.angle_prev_time)) * 10.0
self.angle_x = data.twist.angular.x
self.angle_y = data.twist.angular.y
self.angle_prev_time = curr_time
# assume go forward is positive and rotate to right is positive
def picam_velocity_callback(self, data):
self.velocities_x.append(
-(data.twist.linear.x - self.mw_angle_comp_x) * self.z *
VELOCITY_X_SCALE)
self.velocities_y.append(
-(data.twist.linear.y + self.mw_angle_comp_y) * self.z *
VELOCITY_Y_SCALE)
self.velocities_z.append(0.0)
self.velocities_yaw.append(-data.twist.angular.z * VELOCITY_YAW_SCALE)
def range_callback(self, data):
if data.range != -1:
self.z = data.range
def reset_callback(self, data):
print "Start localization"
self.locate_position = True
self.first_locate = True
self.hold_position = False
self.map_counter = 0
self.max_map_counter = 0
self.velocities_x = []
self.velocities_y = []
self.velocities_z = []
self.velocities_yaw = []
self.prev_velocity_time = None
def toggle_callback(self, data):
self.hold_position = not self.hold_position
self.first_hold = True
self.fb_pid._i = 0
self.lr_pid._i = 0
self.iacc_yaw = 0.0
print "Position hold", "enabled." if self.hold_position else "disabled."
def mode_callback(self, data):
if not self.hold_position or data.mode == 4 or data.mode == 3:
print "VELOCITY"
# TODO scale is not consistent, check index.html and pid_class.py
data.z_velocity = data.z_velocity * 100
self.pospub.publish(data)
else:
self.target_pos[0] += data.x_velocity / 100.
self.target_pos[1] += data.y_velocity / 100.
print "Target position", self.target_pos
