def load_est_vel_data(self, rel_time=False):
""" Loads odometry data from the Husky computed from wheel encoders.
Input: rel_time - set time relative to first msg (rather than absolute)
Return: <float> numpy array of 7 columns:
time [s],
x_linear_velocity [m/s],
y_linear_velocity [m/s],
z_linear_velocity [m/s],
x_angular_velocity [rad/s],
y_angular_velocity [rad/s],
z_angular_velocity [rad/s],
"""
tot_msg_count = self.bag.get_message_count(
self.tpc_names["husky_odometry"])
data = np.empty((0, 7), np.float)
valid_msg_count = 0
print("Retrieving husky_odometry data from {} ...".format(self.file))
print("Number of husky_odometry messages: {}".format(tot_msg_count))
for topic, msg, time in self.bag.read_messages(
self.tpc_names["husky_odometry"]):
# Retrieve time & adjust to relative value if needed
if rel_time:
if valid_msg_count == 0:
init_time = time.to_sec()
curr_time = time.to_sec() - init_time
else:
curr_time = time.to_sec()
temp = np.array([
curr_time, msg.twist.twist.linear.x, msg.twist.twist.linear.y,
msg.twist.twist.linear.z, msg.twist.twist.angular.x,
msg.twist.twist.angular.y, msg.twist.twist.angular.z
])
# Populate main data array
data = np.vstack([data, temp])
# Show process status:
valid_msg_count += 1
self.status = round(100 * float(valid_msg_count) / tot_msg_count)
sys.stdout.write('\r')
sys.stdout.write("Progress: {} %".format(self.status))
sys.stdout.flush()
sys.stdout.write("\n")
return data
def load_encoder_data(self, rel_time=False):
""" Loads encoder data from the Husky.
Input: rel_time - set time relative to first msg (rather than absolute)
Return: <float> numpy array of 9 columns:
time [s],
front_left_wheel position [rad],
front_right_wheel position [rad],
rear_left_wheel position [rad],
rear_right_wheel position [rad],
front_left_wheel velocity [rad/s],
front_right_wheel velocity [rad/s],
rear_left_wheel velocity [rad/s],
rear_right_wheel velocity [rad/s],
"""
tot_msg_count = self.bag.get_message_count(
self.tpc_names["husky_encoder"])
data = np.empty((0, 9), np.float)
valid_msg_count = 0
print("Retrieving husky_encoder data from {} ...".format(self.file))
print("Number of husky_encoder messages: {}".format(tot_msg_count))
for topic, msg, time in self.bag.read_messages(
self.tpc_names["husky_encoder"]):
# Retrieve time & adjust to relative value if needed
if rel_time:
if valid_msg_count == 0:
init_time = time.to_sec()
curr_time = time.to_sec() - init_time
else:
curr_time = time.to_sec()
pos = np.asarray(msg.position)
vel = np.asarray(msg.velocity)
enc = np.hstack((curr_time, pos, vel))
# Populate main data array
data = np.vstack([data, enc])
# Show process status:
valid_msg_count += 1
self.status = round(100 * float(valid_msg_count) / tot_msg_count)
sys.stdout.write('\r')
sys.stdout.write("Progress: {} %".format(self.status))
sys.stdout.flush()
sys.stdout.write("\n")
return data
def monitor():
global stepCounter, robotpose, tfBuffer, tfListener, foot_frame
global ROBOT_NAME, RIGHT_FOOT_FRAME_NAME, LEFT_FOOT_FRAME_NAME
global getLinkProxy
time = rospy.Time.now()
done = False
pelvispose = getLinkProxy('pelvis', 'world')
lfootpose = getLinkProxy('leftFoot', 'world')
rfootpose = getLinkProxy('rightFoot', 'world')
lfootWorld = tfBuffer.lookup_transform('world', LEFT_FOOT_FRAME_NAME,
rospy.Time())
rfootWorld = tfBuffer.lookup_transform('world', RIGHT_FOOT_FRAME_NAME,
rospy.Time())
l_x = lfootWorld.transform.translation.x
r_t = robotpose.header.stamp.secs
r_x = robotpose.pose.pose.position.x
r_y = robotpose.pose.pose.position.y
p_x = pelvispose.link_state.pose.position.x
p_y = pelvispose.link_state.pose.position.y
os.system('clear')
#Right foot x,y,rotation (around z) from ros tf2
tf_x = rfootWorld.transform.translation.x
tf_y = rfootWorld.transform.translation.y
q = rfootWorld.transform.rotation
tf_r = euler_from_quaternion([q.x, q.y, q.z, q.w])[2]
#Right foot x,y,r directly from gazebo
gz_x = rfootpose.link_state.pose.position.x
gz_y = rfootpose.link_state.pose.position.y
q = rfootpose.link_state.pose.orientation
gz_r = euler_from_quaternion([q.x, q.y, q.z, q.w])[2]
print(
"time: %6.3f Right tf2: (%6.3f,%6.3f,%6.3f) gazebo: (%6.3f,%6.3f,%6.3f)"
% (time.to_sec(), tf_x, tf_y, tf_r, gz_x, gz_y, gz_r))
#Left foot x,y
tf_x = lfootWorld.transform.translation.x
tf_y = lfootWorld.transform.translation.y
q = lfootWorld.transform.rotation
tf_r = euler_from_quaternion([q.x, q.y, q.z, q.w])[2]
gz_x = lfootpose.link_state.pose.position.x
gz_y = lfootpose.link_state.pose.position.y
q = lfootpose.link_state.pose.orientation
gz_r = euler_from_quaternion([q.x, q.y, q.z, q.w])[2]
print(
"steps: %2d Left tf2: (%6.3f,%6.3f,%6.3f) gazebo: (%6.3f,%6.3f,%6.3f)"
% (stepCounter, tf_x, tf_y, tf_r, gz_x, gz_y, gz_r))
return done, time.to_sec()
def load_irradiance_data(self, rel_time=False):
""" Loads solar irradiance data from the pyranometer
Input: rel_time - set timestamps relative to first reading (rather than absolute)
Return: <float> numpy array with 3 columns:
timestamp [s],
measured solar irradiance [W/m^2],
clear-sky solar irradiance [W/m^2]
"""
tot_msg_count = self.bag.get_message_count(
self.tpc_names["pyranometer"])
data = np.empty((0, 3), np.float)
valid_msg_count = 0
print("Retrieving pyranometer data from {} ...".format(self.file))
print("Number of pyranometer messages: {}".format(tot_msg_count))
for topic, msg, time in self.bag.read_messages(
self.tpc_names["pyranometer"]):
# Retrieve time & adjust to relative value if needed
if rel_time:
if valid_msg_count == 0:
init_time = time.to_sec()
curr_time = time.to_sec() - init_time
else:
curr_time = time.to_sec()
# Retrieve current data
temp = np.array(
[curr_time, msg.data, msg.theoretical_clearsky_horizontal])
# Populate main data array
data = np.vstack([data, temp])
# Show process status:
valid_msg_count += 1
self.status = round(100 * float(valid_msg_count) / tot_msg_count)
sys.stdout.write('\r')
sys.stdout.write("Progress: {} %".format(self.status))
sys.stdout.flush()
sys.stdout.write("\n")
return data
def file_push():
global first_flag, first_time_global
if first_flag == True:
first_time = str(first_time_global)
out_file.write("first_time:")
out_file.write(first_time)
out_file.write("\n")
out_file.write("time\tvel\tang\tX \tY \tang\t\n")
first_flag = False
vel = str(VEL)
ang = str(ANG)
x = str(LAST_P_x)
y = str(LAST_P_y)
w = str(LAST_P_ang)
time = LAST_T - first_time_global
time = time.to_sec()
time = str(time)
out_file.write(time)
out_file.write("\t")
out_file.write(vel)
out_file.write("\t")
out_file.write(ang)
out_file.write("\t")
out_file.write(x)
out_file.write("\t")
out_file.write(y)
out_file.write("\t")
out_file.write(w)
out_file.write("\n")
def odometry_update(self, poseUpdateOdom):
time = rospy.Time(poseUpdateOdom.header.stamp.secs,
poseUpdateOdom.header.stamp.nsecs)
msg_time = time.to_sec()
try:
trans = self.tf_Buffer.lookup_transform(self.world_frame, "imu",
time, rospy.Duration(1.0))
currentDroneVector = [
trans.transform.translation.x, trans.transform.translation.y,
trans.transform.translation.z
]
except:
currentDroneVector = np.array([0, 0, 0])
newPosUpdate = np.array([
poseUpdateOdom.pose.pose.position.x,
poseUpdateOdom.pose.pose.position.y,
poseUpdateOdom.pose.pose.position.z
])
delta = np.linalg.norm(newPosUpdate - currentDroneVector)
# Track the magnitude of the correction that had to be performed
self.correctionData.AddDataPoint([msg_time, delta])
# Track the timestmap of the pose update for the plots
self.poseUpdateTimeStamps.append(msg_time)
def update(self):
self._current_topic_paths = dict()
self._current_msg_paths = dict()
idx = 0
while self.publisher_tree_widget.model().item(idx,0) != None:
cur_item = self.publisher_tree_widget.model().item(idx,0)
parent_topic = cur_item.text().encode("latin-1")
self.convert_to_path(parent_topic, "" , cur_item)
idx += 1
timestamp_to_data = dict()
for annotation in self._annotations:
start_time = annotation.x_left
end_time = annotation.x_right
for time_stamp in self.drange(start_time, end_time, self._dt):
timestamp_to_data[time_stamp] = dict()
timestamp_to_data[time_stamp]['Reward'] = annotation.reward
if len(self._exported_topics) is not 0:
messages = self.bag.read_messages(topics=self._exported_topics, start_time=rospy.Time.from_sec(annotation.x_left), end_time=rospy.Time.from_sec(annotation.x_right))
for msg_name, msg, time in messages:
dictionary = dict()
sec = time.to_sec()
key = min(timestamp_to_data.keys(), key=lambda x:abs(x-sec))
self.convert_msg_to_paths(msg_name, "", msg, msg_name, dictionary)
timestamp_to_data[key] = dict(timestamp_to_data[key].items() + dictionary.items())
timestamp_to_data = collections.OrderedDict(sorted(timestamp_to_data.items()))
self.current_output = ""
for key,value in timestamp_to_data.items():
self.current_output += str(key) + ":" + str(value) + "\n"
self.export_display_contents.setPlainText(self.current_output)
def monitor():
global stepCounter, robotpose, tfBuffer, tfListener, foot_frame
global ROBOT_NAME, RIGHT_FOOT_FRAME_NAME, LEFT_FOOT_FRAME_NAME
global start_line_crossed, finish_line_crossed
global fig1, ax1, pltdata
time = rospy.Time.now()
foot_frame = LEFT_FOOT_FRAME_NAME
footWorld = tfBuffer.lookup_transform('world', foot_frame, rospy.Time())
footstep = FootstepDataRosMessage()
footstep.orientation = footWorld.transform.rotation
footstep.location = footWorld.transform.translation
f_x = footstep.location.x
r_t = robotpose.header.stamp.secs
r_x = robotpose.pose.pose.position.x
if (r_x >= 0.5 and not start_line_crossed):
start_line_crossed = time
elif (r_x >= 4.5 and not finish_line_crossed):
finish_line_crossed = time
os.system('clear')
print("time: %6.3f steps: %2d robot_x: %6.3f l_foot_x: %6.3f" %
(time.to_sec(), stepCounter, r_x, f_x))
if (start_line_crossed):
print(" Started: %6.3f" % (start_line_crossed.to_sec()))
if (finish_line_crossed):
print("Finished: %6.3f" % (finish_line_crossed.to_sec()))
print(" Elapsed: %6.3f" %
((finish_line_crossed - start_line_crossed).to_sec()))
def getDesiredPixel(self):
# return (844, 328)
pixel = None
while pixel is None:
#u, v = self.getDesiredPixel()
pixel, time = self.desiredPixel
#print(pixel)
return pixel, time.to_sec()
def joint_reader(msg):
global state
time = rospy.Duration()
try:
#joints published alphabetically (ext, pitch, yaw)
time = msg.header.stamp
state[0] = float(msg.position[1])
state[1] = float(msg.position[2])
state[2] = float(msg.position[0])
state[3] = time.to_sec()
except:
pass
def monitor():
global stepCounter, robotpose, tfBuffer, tfListener, foot_frame
global ROBOT_NAME, RIGHT_FOOT_FRAME_NAME, LEFT_FOOT_FRAME_NAME
global start_line_crossed, finish_line_crossed
time = rospy.Time.now()
done = False
foot_frame = LEFT_FOOT_FRAME_NAME
lfootWorld = tfBuffer.lookup_transform('world', LEFT_FOOT_FRAME_NAME,
rospy.Time())
rfootWorld = tfBuffer.lookup_transform('world', RIGHT_FOOT_FRAME_NAME,
rospy.Time())
l_x = lfootWorld.transform.translation.x
r_x = rfootWorld.transform.translation.x
r_t = robotpose.header.stamp.secs
r_x = robotpose.pose.pose.position.x
r_z = robotpose.pose.pose.position.z
if (r_x >= 0.5 and not start_line_crossed):
start_line_crossed = time
elif (r_x >= 4.5 and not finish_line_crossed):
finish_line_crossed = time
os.system('clear')
print(
"time: %6.3f steps: %2d robot: (%6.3f,%6.3f) left: %6.3f right: %6.3f"
% (time.to_sec(), stepCounter, r_x, r_z, l_x, r_x))
if (start_line_crossed):
print(" Started: %6.3f" % (start_line_crossed.to_sec()))
if (finish_line_crossed):
print("Finished: %6.3f" % (finish_line_crossed.to_sec()))
print(" Elapsed: %6.3f" %
((finish_line_crossed - start_line_crossed).to_sec()))
if (time.to_sec() > 160.0 or r_x > 5.0 or r_z < 0.5):
done = True
return done, time.to_sec()
def joint_reader(msg):
global state
time = rospy.Duration()
time = msg.header.stamp
try:
#joints published alphabetically (ext, pitch, yaw)
state[0] = round(float(msg.position[1]), 6)
state[1] = round(float(msg.position[2]), 6)
state[2] = round(float(msg.position[0]), 6)
state[3] = time.to_sec()
except:
print('Joint State Error')
def file_push():
global first_flag, first_time_global
#print "LAST_T:",LAST_T
if first_flag == True:
first_time = str(first_time_global)
out_file.write("first_time:")
out_file.write(first_time)
out_file.write("\n")
out_file.write("time \tvel \tang \tX \tY \tang \tWl \tWr\n")
first_flag = False
time = LAST_T - first_time_global
time = time.to_sec()
t = str(time)
vel = str(VEL)
ang = str(ANG)
x = str(LAST_P_x)
y = str(LAST_P_y)
w = str(LAST_P_ang)
wl = str(LAST_Wl)
wr = str(LAST_Wr)
out_file.write(t)
out_file.write("\t")
out_file.write(vel)
out_file.write("\t")
out_file.write(ang)
out_file.write("\t")
out_file.write(x)
out_file.write("\t")
out_file.write(y)
out_file.write("\t")
out_file.write(w)
out_file.write("\t")
out_file.write(wl)
out_file.write("\t")
out_file.write(wr)
out_file.write("\n")
def load_imu_data(self, rel_time=False):
""" Loads IMU data
Input: rel_time - set timestamp relative to first reading (rather than absolute)
Return: <float> numpy array of 11 columns:
timestamp [s],
x_linear_acceleration [m/s^2],
y_linear_acceleration [m/s^2],
z_linear_acceleration [m/s^2],
x_angular_velocity [rad/s],
y_angular_velocity [rad/s],
z_angular_velocity [rad/s],
x_orientation,
y_orientation,
z_orientation,
w_orientation
"""
tot_msg_count = self.bag.get_message_count(self.tpc_names["imu"])
data = np.empty((0, 11), np.float)
valid_msg_count = 0
print("Retrieving IMU data from {} ...".format(self.file))
print("Number of IMU messages: {}".format(tot_msg_count))
for topic, msg, time in self.bag.read_messages(self.tpc_names["imu"]):
# Retrieve time & adjust to relative value if needed
if rel_time:
if valid_msg_count == 0:
init_time = time.to_sec()
curr_time = time.to_sec() - init_time
else:
curr_time = time.to_sec()
# Retrieve current data
temp = np.array([
curr_time, msg.linear_acceleration.x,
msg.linear_acceleration.y, msg.linear_acceleration.z,
msg.angular_velocity.x, msg.angular_velocity.y,
msg.angular_velocity.z, msg.orientation.x, msg.orientation.y,
msg.orientation.z, msg.orientation.w
])
# Populate main data array
data = np.vstack([data, temp])
# Show process status:
valid_msg_count += 1
self.status = round(100 * float(valid_msg_count) / tot_msg_count)
sys.stdout.write('\r')
sys.stdout.write("Progress: {} %".format(self.status))
sys.stdout.flush()
sys.stdout.write("\n")
return data
def main(self):
# Creates the publisher for ROS. (name of message, data type(from geometry_msgs.msg), queue size)
velocity_publisher = rospy.Publisher(
'mavros/setpoint_velocity/cmd_vel', TwistStamped, queue_size=10)
velocity_vector = TwistStamped() # Ros data type for publisher
x_linear = 0
y_linear = 0
z_linear = 0
x_ang = 0
y_ang = 0
z_ang = 0
# Creates a state publisher, for when we want to remotely disarm the system
state_publisher = rospy.Publisher('mavros/state', State, queue_size=10)
state_variable = State()
# sets the rate of the loop
rate = rospy.Rate(10)
# goal altitude
altitude_goal = input('what altitude should I hover at?')
self.altitude_current = 0
# state variable
stance = 0
count = 0
# 0 : logical element
# 1 : waiting for offb controll
# 2 : take off
# 3 : land
# 4 : search pattern
while not rospy.is_shutdown():
count += 1
if count % 100 == 1:
rospy.loginfo("State: " + str(stance))
##########################################################################################
################# stance switch ###########################################################
##########################################################################################
if stance == 0:
x_linear = 0
y_linear = 0
z_linear = 0
x_ang = 0
y_ang = 0
z_ang = 0
stance = 1
# waiting for offb control
elif stance == 1:
rate.sleep()
if self.state_current.guided:
rospy.loginfo("I'm taking control, takeoff in " +
str(countdown))
countdown -= 0.1
if countdown < 0:
rospy.loginfo("Switching to Takeoff")
stance = 2
else:
countdown = 5
# Take off:
elif stance == 2:
altitude_goal = 2
if count % 10 == 1:
rospy.loginfo("Current altitude: " +
str(self.altitude_current))
if self.altitude_current <= altitude_goal + 0.1 and self.altitude_current >= altitude_goal - 0.1:
countdown -= 0.1
if count % 10 == 1:
rospy.loginfo(
"At goal altitude, switching to logic state")
rospy.loginfo("this isnot coded")
else:
countdown = 5
if countdown < 0:
stance = 0
# Landing
elif stance == 3:
if count % 10 == 1:
rospy.loginfo(self.altitude)
z_linear = -0.02
if self.altitude_current < 0.10:
countdown -= 0.1
rospy.loginfo("WARNING: low altitude, DISARMING in: " +
str(countdown))
else:
countdown = 10
if self.altitude_current < 0.05 and countdown < 0:
rospy.loginfo("DISARM-DISARM-DISARM")
state_variable.armed = False
state_variable.guided = False
state_publisher.publish(state_variable)
elif stance == 4:
pass
elif stance == 5:
pass
elif stance == 6:
pass
##########################################################################################
################# velocity finder ########################################################
##########################################################################################
if not stance == 3: # If were not landing, this works
# set the verticle speed
if self.altitude_current >= altitude_goal - 0.1 and self.altitude_current <= altitude_goal + 0.1:
z_linear = 0
elif self.altitude_current > altitude_goal + 0.1:
z_linear = -0.02
elif self.altitude_current < altitude_goal - 0.1:
z_linear = 0.02
# need to do this for x_linear, y_linear, and possibly x,y, and z ang
##########################################################################################
################# velocity publisher #####################################################
##########################################################################################
# set linear to value
velocity_vector.twist.linear.x = x_linear
velocity_vector.twist.linear.y = y_linear
velocity_vector.twist.linear.z = z_linear
# Set angular to zero
velocity_vector.twist.angular.x = x_ang
velocity_vector.twist.angular.y = y_ang
velocity_vector.twist.angular.z = z_ang
# Get the time now for the velocity commands
time = rospy.Time.now()
velocity_vector.header.stamp.secs = int(time.to_sec())
velocity_vector.header.stamp.nsecs = int(
(time.to_sec() - int(time.to_sec())) * 1000000000)
velocity_vector.header.seq = count
# use the publisher
velocity_publisher.publish(velocity_vector)
rate.sleep()
def load_VINS_data(self, rel_time=False):
""" Loads pose estimates from VINS-Fusion
Input: rel_time - set time relative to first msg (rather than absolute)
Return: <float> numpy array of 8 columns:
time [s],
x_linear_position [m],
y_linear_position [m],
z_linear_position [m],
x_orientation,
y_orientation,
z_orientation,
w_orientation
"""
tot_msg_count = self.bag.get_message_count(
self.tpc_names["pose_estimates"])
data = np.empty((0, 8), np.float)
valid_msg_count = 0
print("Retrieving VINS pose estimate data from {} ...".format(
self.file))
print(
"Number of VINS pose estimate messages: {}".format(tot_msg_count))
# Retrieve latest path
for topic, msg, time in self.bag.read_messages(
self.tpc_names["pose_estimates"]):
# Retrieve time & adjust to relative value if needed
if rel_time:
if valid_msg_count == 0:
init_time = time.to_sec()
curr_time = time.to_sec() - init_time
else:
curr_time = time.to_sec()
# Retrieve current data
temp = np.array([
curr_time, msg.pose.pose.position.x, msg.pose.pose.position.y,
msg.pose.pose.position.z, msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y, msg.pose.pose.orientation.z,
msg.pose.pose.orientation.w
])
# Populate main data array
data = np.vstack([data, temp])
# Show process status:
valid_msg_count += 1
self.status = round(100 * float(valid_msg_count) / tot_msg_count)
sys.stdout.write('\r')
sys.stdout.write("Progress: {} %".format(self.status))
sys.stdout.flush()
sys.stdout.write("\n")
return data
def load_sun_position_data(self, rel_time=False, reference="global"):
""" Loads sun position data
Input: rel_time - set time relative to first msg (rather than absolute)
reference - sun pose vector with respect to "global" or "rover" frame
Return: <float> numpy array of 8 columns:
time [s],
x_linear_position [m],
y_linear_position [m],
z_linear_position [m],
x_orientation,
y_orientation,
z_orientation,
w_orientation
"""
tot_msg_count = self.bag.get_message_count(
self.tpc_names["relative_sun_orientation"])
data = np.empty((0, 8), np.float)
valid_msg_count = 0
print("Retrieving sun orientation data from {} ...".format(self.file))
print("Number of sun orientation data messages: {}".format(
tot_msg_count))
if reference == "global":
data_generator = self.bag.read_messages(
self.tpc_names["global_sun_orientation"])
elif reference == "relative":
data_generator = self.bag.read_messages(
self.tpc_names["relative_sun_orientation"])
else:
raise NotImplementedError
# Retrieve latest path
for topic, msg, time in data_generator:
# Retrieve time & adjust to relative value if needed
if rel_time:
if valid_msg_count == 0:
init_time = time.to_sec()
curr_time = time.to_sec() - init_time
else:
curr_time = time.to_sec()
# Retrieve current data
temp = np.array([
curr_time, msg.pose.position.x, msg.pose.position.y,
msg.pose.position.z, msg.pose.orientation.x,
msg.pose.orientation.y, msg.pose.orientation.z,
msg.pose.orientation.w
])
# Populate main data array
data = np.vstack([data, temp])
# Show process status:
valid_msg_count += 1
self.status = round(100 * float(valid_msg_count) / tot_msg_count)
sys.stdout.write('\r')
sys.stdout.write("Progress: {} %".format(self.status))
sys.stdout.flush()
sys.stdout.write("\n")
return data
def load_gps_data(self, ret_utm=False, rel_time=False):
""" Loads GPS data
Input: ret_utm - return the position as UTM coords (instead of lat-lon)
rel_time - set timestamp relative to first reading (rather than absolute)
Return: <float> numpy array of 4 columns if ret_utm argument is False:
ROS timestamp [s],
latitude [deg, +ve = Northern hemisphere]
longitude [deg, +ve = East of Prime Meridian]
altitude [m]
OR
<float> numpy array of 4 columns if ret_utm argument is True:
ROS timestamp [s],
UTM easting [m, +ve = towards East]
UTM northing [m, +ve = towards North]
altitude [m]
"""
tot_msg_count = self.bag.get_message_count(self.tpc_names["gps"])
data = np.empty((0, 4), np.float)
valid_msg_count = 0
print("Retrieving GPS data from {} ...".format(self.file))
print("Number of GPS messages: {}".format(tot_msg_count))
for topic, msg, time in self.bag.read_messages(self.tpc_names["gps"]):
# Retrieve time & adjust to relative value if needed
if rel_time:
if valid_msg_count == 0:
init_time = time.to_sec()
curr_time = time.to_sec() - init_time
else:
curr_time = time.to_sec()
lat = msg.latitude
lon = msg.longitude
alt = msg.altitude
# Retrieve current data
if ret_utm:
easting, northing = utm.from_latlon(lat, lon)[0:2]
temp = np.array([curr_time, easting, northing, alt])
else:
temp = np.array([curr_time, lat, lon, alt])
# Populate main data array
data = np.vstack([data, temp])
# Show process status:
valid_msg_count += 1
self.status = round(100 * float(valid_msg_count) / tot_msg_count)
sys.stdout.write('\r')
sys.stdout.write("Progress: {} %".format(self.status))
sys.stdout.flush()
sys.stdout.write("\n")
return data
def load_image_data(self,
img_source="mono_image",
time_range=None,
rel_time=False):
""" Loads image data from cameras mounted on the Husky
Input: string img_source - get images from {"omni_image0", ..., "omni_image9",
"omni_stitched_image", "omni_stitched_disparity", "mono_image"}
time_range - ROS time range (Unix epoch time) to load images from
with tuple (start,end), None defaults to all
rel_time - set time relative to first msg (rather than absolute)
Return: tuple of (time [s], images).
time is a <float> numpy array of dimension: (batch,).
images is a <int> numpy array of dimension:
(batch, height, width) or (batch , height, width, channel).
"""
# Check which camera to grab images from
if img_source == "mono_image":
img_gen = self.bag.read_messages(self.tpc_names["monocam"])
tot_msg_count = self.bag.get_message_count(
self.tpc_names["monocam"])
image_type = "grayscale"
elif img_source == "omni_stitched_image":
img_gen = self.bag.read_messages(self.tpc_names["pancam"])
tot_msg_count = self.bag.get_message_count(
self.tpc_names["pancam"])
image_type = "rgb"
elif img_source in ["omni_image{}".format(x) for x in range(10)]:
idx = ["omni_image{}".format(x)
for x in range(10)].index(img_source)
img_gen = self.bag.read_messages("/omni_image{}".format(idx))
tot_msg_count = self.bag.get_message_count(
"/omni_image{}".format(idx))
image_type = "rgb"
elif img_source == "omni_stitched_disparity":
img_gen = self.bag.read_messages("/omni_stitched_disparity")
tot_msg_count = self.bag.get_message_count("/omni_stitched_image")
image_type = "disp"
else:
raise ValueError(
"{} is not from {occam_image0, ..., occam_image9, omni_stitched_image, omni_stitched_disparity, mono_image}"
.format(img_source))
valid_msg_count = 0
print("Retrieving '{}' image data from {} ...".format(
img_source, self.file))
print("Number of '{}' image messages: {}".format(
img_source, tot_msg_count))
if time_range == None:
print("Loading all {} images".format(tot_msg_count))
# Calculate the amount of images to pre-allocate later
img_count = tot_msg_count
elif type(time_range) is tuple:
t_range = self.time_to_timestep(time_range, tot_msg_count)
# Quick check for tuple range
if not (0 <= t_range[0] < tot_msg_count
and 0 <= t_range[1] < tot_msg_count):
raise ValueError("Invalid timestep range {} for image dataset of size {}" \
.format(t_range, tot_msg_count))
print("Loading images from timesteps {} to {}".format(
t_range[0], t_range[1]))
# Don't load all the data, instead slice the generator at the desired range
img_gen = itertools.islice(img_gen, t_range[0], t_range[1])
# Calculate the amount of images to pre-allocate later
img_count = (t_range[1] - t_range[0]) + 1
else:
raise ValueError(
"Invalid type {} for 't_range', t_range is a tuple (start,end) or None"
% type(t_range))
time_data = np.empty((img_count))
for i, (_, msg, time) in enumerate(img_gen):
img = np.fromstring(msg.data, dtype=np.uint8)
# Retrieve time & adjust to relative value if needed
if rel_time:
if valid_msg_count == 0:
init_time = time.to_sec()
curr_time = time.to_sec() - init_time
else:
curr_time = time.to_sec()
time_data[i] = curr_time
if i == 0:
# Get the dimensions from the first image and pre-allocate
w = msg.width
h = msg.height
if image_type == "grayscale":
data = np.empty((img_count, h, w), np.uint8)
elif image_type == "disp":
data = np.empty((img_count, h, w, 3), np.uint8)
else:
data = np.empty((img_count, h, w, 3), np.uint8)
if image_type == "grayscale":
img = img.reshape(h, w)
data[i, :, :] = img
elif image_type == "disp":
img = img.reshape(h, w, 3)
data[i, :, :] = img
else:
img = img.reshape(h, w, 3)
data[i, :, :, :] = img
valid_msg_count += 1
return (time_data, data)
def load_pointcloud_data(self,
pc_source="omni_stitched_cloud",
time_range=None,
rel_time=False):
""" Loads pointcloud data from stereo omnidirectional camera mounted on the Husky
Input: pc_source - get pointclouds from {omni_cloud0, ..., omni_cloud4, omni_stitched_cloud}
time_range - ROS time range (Unix epoch time) to load images from with tuple (start,end), None defaults to all
rel_time - set time relative to first msg (rather than absolute)
Return: tuple of (times [s], clouds).
times is a <float> numpy array of dimension: (batch,)
clouds is a list of <float> numpy arrays. Each <float> numpy
array (a single pointcloud) is of dimension: (n_points, 3),
where each column corresponds to x,y,z, respectively.
"""
if pc_source == "omni_stitched_cloud":
pc_gen = self.bag.read_messages(self.tpc_names["pointclouds"])
tot_msg_count = self.bag.get_message_count(
self.tpc_names["pointclouds"])
elif pc_source in ["omni_cloud{}".format(x) for x in range(10)]:
idx = ["omni_cloud{}".format(x)
for x in range(10)].index(pc_source)
pc_gen = self.bag.read_messages("omni_cloud{}".format(idx))
tot_msg_count = self.bag.get_message_count(
"omni_cloud{}".format(idx))
valid_msg_count = 0
print("Retrieving pointcloud data from {} ...").format(self.file)
print("Number of pointcloud messages: {}").format(tot_msg_count)
if time_range == None:
print("Loading all {} pointclouds".format(tot_msg_count))
# Calculate the amount of images to pre-allocate later
pc_count = tot_msg_count
elif type(time_range) is tuple:
t_range = self.time_to_timestep(time_range, tot_msg_count)
# Quick check for tuple range
if not (0 <= t_range[0] < tot_msg_count
and 0 <= t_range[1] < tot_msg_count):
raise ValueError("Invalid timestep range {} for pointcloud dataset of size {}" \
.format(t_range, tot_msg_count))
print("Loading pointclouds from timesteps {} to {}".format(
t_range[0], t_range[1]))
# Don't load all the data, instead slice the generator at the desired range
pc_gen = itertools.islice(pc_gen, t_range[0], t_range[1])
# Calculate the amount of images to pre-allocate later
pc_count = (t_range[1] - t_range[0]) + 1
else:
raise ValueError(
"Invalid type {} for 't_range', t_range is a tuple (start,end) or None"
% type(t_range))
time_data = np.empty((pc_count))
data = []
for i, (_, msg, time) in enumerate(pc_gen):
# Retrieve time & adjust to relative value if needed
if rel_time:
if valid_msg_count == 0:
init_time = time.to_sec()
curr_time = time.to_sec() - init_time
else:
curr_time = time.to_sec()
time_data[i] = curr_time
# for PointCloud2
pc = [[point[0], point[1], point[2]] for point in pc2.read_points(
msg, field_names=("x", "y", "z"), skip_nans=True)]
# for PointCloud
# pc = [[point.x, point.y, point.z] for point in msg.points]
data.append(np.asarray(pc, dtype=np.float32))
valid_msg_count += 1
return (time_data, data)
def submit_transform_stamped(self, transform, data_storage, time):
time = time.to_sec()
pos = transform.transform.translation
quat = transform.transform.rotation
data_storage.AddStampedDataPoint(
time, [pos.x, pos.y, pos.z, quat.x, quat.y, quat.z, quat.w])
def loop_search(self):
while not rospy.is_shutdown():
# read frame from capture
ret, img = self.cap.read()
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
output = img.copy()
# Gaussian Blur
gaussian_blur = cv2.GaussianBlur(gray, (9, 9), 0)
# Get the radius range based off of altitude (alt in meter, radius in pixel)
if self.alt < 0:
self.alt = 0
self.radius = int(-12.1 * math.pow(self.alt, 4) +
49.188 * math.pow(self.alt, 3) -
21.3 * math.pow(self.alt, 2) -
132.26 * self.alt + 176.6)
circles = cv2.HoughCircles(gaussian_blur,
cv2.cv.CV_HOUGH_GRADIENT,
3,
100,
minRadius=self.radius - 5,
maxRadius=self.radius + 5)
self.pose_array = []
# ensure at least some circles were found
if circles is not None:
circles = np.round(circles[0, :]).astype("int")
# loop over the (x, y) coordinates and radius of the circles
for (x, y, r) in circles:
# for visulization:
cv2.circle(output, (x, y), r, (0, 255, 0), 4)
cv2.rectangle(output, (x - 5, y - 5), (x + 5, y + 5),
(0, 128, 255), -1)
# TO find the length on the ground in meters
# (height in meters times the distance in pixels)/360
self.temp_pose.position.x = ((x - 320) * self.alt) / 360
self.temp_pose.position.y = ((240 - y) * self.alt) / 360
# Published the pixel location as well
self.temp_pose.orientation.x = (x - 320)
self.temp_pose.orientation.y = (240 - y)
self.temp_pose.position.z = 0
self.pose_array.append(self.temp_pose)
print(
[self.temp_pose.position.x, self.temp_pose.position.y])
self.roomba_array.header.seq += 1
time = rospy.Time.now()
self.roomba_array.header.stamp.secs = int(time.to_sec())
self.roomba_array.header.stamp.nsecs = int(
(time.to_sec() - int(time.to_sec())) * 1000000000)
self.roomba_array.poses = self.pose_array
################################################
################ Line Detection ################
################################################
global xmax
global ymax
xmax, ymax = img.shape[:
2] # create the maximum values of the picture
###
self.Hllist = Float64MultiArray() ##\\\
self.Vllist = Float64MultiArray(
) ##---Set up Vllist, Hllist, HAngAverage as data types ros can read
self.HAngAverage = Float64() #######//
edges = cv2.Canny(
gaussian_blur, 50, 130,
apertureSize=3) #performs canny edge detection on self.img.
# arguments: (image file to perform, minVal [below this are surely not edges], maxVal [above this are sure to be edges],
# aperture size [default 3],L2gradient[default is true, decides which equn to use for finding the graident])
################################################
lines = cv2.HoughLines(edges, 1, np.pi / 180, 120)
################################################
##### makes lines. args:(image to work with, length, angular resolution, maximum gap between lines)
##### outputs an angle and a length. Angle is angle down from the horizontal, length is length from the origin (center of image)
##### 0 < angle < 180, length can be negathLinesive.
if lines is not None: #if there are not no lines
for rho, theta in lines[0]:
##### \/\/\/\/ ####
a = np.cos(theta)
b = np.sin(theta)
x0 = a * rho
y0 = b * rho
x1 = int(x0 + 1000 * (-b))
y1 = int(y0 + 1000 * (a))
x2 = int(x0 - 1000 * (-b))
y2 = int(y0 - 1000 * (a))
pt1 = (x1, y1)
pt2 = (x2, y2)
#### /\/\/\ #### all stuff on the open.cv website
#print rho, "\t\t'", theta*180/np.pi, "\n",
#cv2.line(self.check,pt1,pt2,(255,0,0),2)
if (100 * np.pi / 180) > theta > (80 * np.pi / 180) or (
100 * np.pi / 180
) > (theta - np.pi) > (
80 * np.pi / 180
): # horiziontal lines. 80 - 9cx ds0 deg; 260 - 280 deg
self.Hllist.data.append(
(y1 + y2) / 2
) #add the average y point to the list, so that it's in the middle of the image
#cv2.line(output,pt1,pt2,(0,0,255),2) #paint the image
#print "line in v arrary \n"
self.HAngAverage.data += theta * 180 / np.pi # adds the angle to the angle average
elif theta < (10 * np.pi / 180) or theta > (
170 * np.pi / 180
): #pycharms doesent recognise rospy vertical lines. 10 - 0 deg; 190 - 200 deg
self.Vllist.data.append(
(x1 + x2) / 2
) #add the average x point to the list, so that it's in the middle of the image
#cv2.line(output,pt1,pt2,(0,255,0),2) #paint the image
#print "line is in h arrary \n"
##############################################################
###################Processing The arrays######################
##############################################################
self.Hllist.data.sort() # sorts arrarys
self.Vllist.data.sort() # /\
### average the angle
if len(self.Hllist.data
) != 0: # if the num of elements in Hllist is not 0, then:
self.HAngAverage.data = self.HAngAverage.data / len(
self.Hllist.data
) #devide angle by num of elements (sum/number) (average)
else:
self.HAngAverage.data = 999 # if there are no elements in Hllist, then no lines, so the error angle is 999
###
#########################
# show the output image #
#########################
#for visulization:
#cv2.imshow("output", np.hstack([img, output]))
#exit condition to leave the loop
k = cv2.waitKey(30) & 0xff
if k == 27:
break
###############################################################
##############################Publisher########################
###############################################################
self.pub.publish(self.roomba_array) # PoseArray type variable
self.Vpub.publish(self.Vllist)
self.Hpub.publish(self.Hllist)
self.Angpub.publish(self.HAngAverage)
print('=============================================')
print(self.Vllist.data)
print(self.Hllist.data)
print(self.HAngAverage.data)
#self.rospy.spin()
self.rate.sleep()
cv2.destroyAllWindows()
self.cap.release()
def main(self):
# Creates the publisher for ROS. (name of message, data type(from geometry_msgs.msg), queue size)
velocity_publisher = rospy.Publisher('mavros/setpoint_velocity/cmd_vel', TwistStamped, queue_size=10)
velocity_vector = TwistStamped() # Ros data type for publisher
x_linear = 0
y_linear = 0
z_linear = 0
altitude_PID = PID(0.5,0.1,0.01)
x_ang = 0
y_ang = 0
z_ang = 0
# Creates a state publisher, for when we want to remotely disarm the system
state_publisher = rospy.Publisher('mavros/state', State, queue_size = 10)
state_variable = State()
# sets the rate of the loop
rate = rospy.Rate(10)
# goal altitude
altitude_goal = 3
self.altitude_current = 0
# state variable
stance = 0
stance_previous = 0
count = 0
# 0 : logical element
# 1 : waiting for offb controll
# 2 : take off
# 3 : land
# 4 : search pattern
while not rospy.is_shutdown():
count += 1
if count % 100 == 1:
rospy.loginfo("State: " + str(stance))
##########################################################################################
################# stance switch ###########################################################
##########################################################################################
if stance == 0:
x_linear = 0
y_linear = 0
z_linear = 0
x_ang = 0
y_ang = 0
z_ang = 0
stance = 1
# waiting for offb control
elif stance == 1:
rate.sleep()
if self.state_current.guided:
rospy.loginfo("I'm taking control, takeoff in " + str(countdown))
countdown -= 0.1
if countdown < 0:
rospy.loginfo("Switching to Takeoff")
stance_previous = 1
stance = 2
else:
countdown = 5
# Take off:
elif stance == 2:
altitude_goal = 2
if count % 10 == 1:
rospy.loginfo("Current altitude: " + str(self.altitude_current))
if self.altitude_current <= altitude_goal + 0.1 and self.altitude_current >= altitude_goal - 0.1:
countdown -= 0.1
if count % 10 == 1:
rospy.loginfo("At goal altitude, switching to logic state")
stance_previous = 2
stance = 0
else:
countdown = 5
if countdown < 0:
stance = 0
# Landing
elif stance == 3:
if count % 10 == 1:
rospy.loginfo(self.altitude)
z_linear = -0.02
if self.altitude_current < 0.10:
countdown -= 0.1
rospy.loginfo("WARNING: low altitude, DISARMING in: " + str(countdown))
else:
countdown = 10
if self.altitude_current < 0.05 and countdown < 0:
rospy.loginfo("DISARM-DISARM-DISARM")
state_variable.armed = False
state_variable.guided = False
state_publisher.publish(state_variable)
elif stance == 4:
pass
elif stance == 5:
pass
elif stance == 6:
pass
##########################################################################################
################# velocity finder ########################################################
##########################################################################################
if not stance == 3: # If were not landing, this works
# set the verticle speed
z_linear = altitude_PID.run(self.altitude_current, altitude_goal)
# need to do this for x_linear, y_linear, and possibly x,y, and z ang
##########################################################################################
################# velocity publisher #####################################################
##########################################################################################
# set linear to value
velocity_vector.twist.linear.x = x_linear
velocity_vector.twist.linear.y = y_linear
velocity_vector.twist.linear.z = z_linear
# Set angular to zero
velocity_vector.twist.angular.x = x_ang
velocity_vector.twist.angular.y = y_ang
velocity_vector.twist.angular.z = z_ang
# Get the time now for the velocity commands
time = rospy.Time.now()
velocity_vector.header.stamp.secs = int(time.to_sec())
velocity_vector.header.stamp.nsecs = int((time.to_sec() - int(time.to_sec())) * 1000000000)
velocity_vector.header.seq = count
# use the publisher
velocity_publisher.publish(velocity_vector)
rate.sleep()
def load_energy_data(self, rel_time=False):
""" Loads data from driving motors power monitors onboard the Husky
Energy values are obtained by integrating power consumption values
Input: rel_time - set timestamps relative to first reading (rather than absolute)
Return: <float> numpy array of 9 columns:
timestamp [s],
l_motor_volt [V],
l_motor_curr [A],
r_motor_volt [V],
r_motor_curr [A],
l_motor_power [W],
r_motor_power [W],
l_motor_cummul_energy [J],
r_motor_cummul_energy [J]
"""
tot_msg_count = self.bag.get_message_count(self.tpc_names["power"])
data = np.empty((0, 5), np.float)
valid_msg_count = 0
print("Retrieving power data from {} ...".format(self.file))
print("Number of power messages: {}".format(tot_msg_count))
for topic, msg, time in self.bag.read_messages(
self.tpc_names["power"]):
#if topic == self.tpc_names["power"]:
# Retrieve time & adjust to relative value if needed
if rel_time:
if valid_msg_count == 0:
init_time = time.to_sec()
curr_time = time.to_sec() - init_time
else:
curr_time = time.to_sec()
# Retrieve current data
temp = np.array([
curr_time, msg.left_driver_voltage, msg.left_driver_current,
msg.right_driver_voltage, msg.right_driver_current
])
# Populate main data array
data = np.vstack([data, temp])
# Show process status:
valid_msg_count += 1
self.status = round(100 * float(valid_msg_count) / tot_msg_count)
sys.stdout.write('\r')
sys.stdout.write("Progress: {} %".format(self.status))
sys.stdout.flush()
sys.stdout.write("\n")
# Compute motor powers and append to main data array
l_power = np.reshape(np.multiply(data[:, 1], data[:, 2]), (-1, 1))
r_power = np.reshape(np.multiply(data[:, 3], data[:, 4]), (-1, 1))
data = np.hstack([data, l_power])
data = np.hstack([data, r_power])
# Compute motor energies and append to main data array
l_energy = self.energy_from_power(data[:, 0], data[:, 5])
l_energy = np.reshape(l_energy, (-1, 1))
r_energy = self.energy_from_power(data[:, 0], data[:, 6])
r_energy = np.reshape(r_energy, (-1, 1))
data = np.hstack([data, l_energy])
data = np.hstack([data, r_energy])
return data
