def task(self, **kwargs):
# Start ROS
ros_utils.start_master()
# Launch EKF and aerial updater
ros_utils.launch(package='localization',
launch_file='jackal_ekf.launch')
ros_utils.launch(package='localization',
launch_file='aerial_global_updater.launch',
argv={
'ugv_poses_path':
self.data_sources['ugv_poses_path'],
'relevant_update_index':
self.data_sources['relevant_update_index'],
'resolution':
self.params['resolution']
})
# Start recording output bag
output_bag_path = os.path.join(self.repetition_dir,
'%s_output.bag' % self.name)
ros_utils.start_recording_bag(
output_bag_path,
['/odometry/filtered', '/odometry/filtered/aerial_updates'])
# Start input bag and wait
_, bag_duration = ros_utils.play_bag(
self.data_sources['jackal_bag_path'],
start_time=self.params['bag_start_time'])
time.sleep(bag_duration)
# Stop recording output bag
ros_utils.stop_recording_bags()
# Plot
ekf_df = ros_utils.bag_to_dataframe(
output_bag_path,
topic=r'/odometry/filtered',
fields=['pose.pose.position.x', 'pose.pose.position.y'])
ekf_with_aerial_update_df = ros_utils.bag_to_dataframe(
output_bag_path,
topic=r'/odometry/filtered/aerial_updates',
fields=['pose.pose.position.x', 'pose.pose.position.y'])
ekf_df.to_csv(os.path.join(self.repetition_dir, 'ekf_pose.csv'))
ekf_with_aerial_update_df.to_csv(
os.path.join(self.repetition_dir,
'ekf_with_aerial_update_pose.csv'))
self.results[self.repetition_id]['ekf_pose_path'] = os.path.join(
self.repetition_dir, 'ekf_pose.csv')
self.results[
self.repetition_id]['ekf_with_aerial_update_path'] = os.path.join(
self.repetition_dir, 'ekf_with_aerial_update_pose.csv')
viz_utils.plot_2d_trajectory([ekf_df, ekf_with_aerial_update_df],
labels=['EKF', 'EKF with aerial updates'],
file_name=os.path.join(
self.repetition_dir, '2d_trajectory'))
# Delete bag file
os.remove(output_bag_path)
def task(self, **kwargs):
maxima = []
minima = []
plot_dict = {}
if kwargs.get('odom'):
odom = ros_utils.bag_to_dataframe(
self.data_sources,
topic=r'/jackal_velocity_controller/odom',
fields=['pose.pose.position.x', 'pose.pose.position.y'])
maxima.append(odom.max())
minima.append(odom.min())
plot_dict['odom'] = odom
self.results[self.repetition_id]['odom_error'] = np.linalg.norm(
odom.iloc[-1] - odom.iloc[0])
if kwargs.get('ekf'):
ekf = ros_utils.bag_to_dataframe(
self.data_sources,
topic=r'/odometry/filtered',
fields=['pose.pose.position.x', 'pose.pose.position.y'])
maxima.append((ekf.max()))
minima.append((ekf.min()))
plot_dict['ekf'] = ekf
self.results[self.repetition_id]['ekf_error'] = np.linalg.norm(
ekf.iloc[-1] - ekf.iloc[0])
if kwargs.get('gps'):
gps = ros_utils.bag_to_dataframe(
self.data_sources,
topic=r'/odometry/filtered/global',
fields=['pose.pose.position.x', 'pose.pose.position.y'])
maxima.append(gps.max())
minima.append(gps.min())
plot_dict['gps'] = gps
self.results[self.repetition_id]['gps_error'] = np.linalg.norm(
gps.iloc[-1] - gps.iloc[0])
max_xy = pd.concat(maxima, axis=1).max(axis=1)
min_xy = pd.concat(minima, axis=1).min(axis=1)
plots_colormap = {'odom': 'r', 'ekf': 'g', 'gps': 'b'}
for p in np.linspace(0.01, 1, num=20):
plot_list = []
colors = []
for plot_name, df in plot_dict.items():
plot_list.append(df.head(int(p * len(df.index))))
colors.append(plots_colormap[plot_name])
plt.figure()
viz_utils.plot_2d_trajectory(
plot_list,
labels=plot_dict.keys(),
colors=colors,
file_name=os.path.join(self.repetition_dir,
'2d_trajectory_%d' % int(100 * p)),
xlim=(min_xy.loc['pose.pose.position.x'] - 15,
max_xy.loc['pose.pose.position.x'] + 15),
ylim=(min_xy.loc['pose.pose.position.y'] - 15,
max_xy.loc['pose.pose.position.y'] + 15))
def task(self, **kwargs):
ros_utils.start_master(use_sim_time=True)
ros_utils.launch(package='localization', launch_file='jackal_scan_matcher.launch')
output_bag_path = os.path.join(self.repetition_dir, '%s_output.bag' % self.name)
ros_utils.start_recording_bag(output_bag_path, ['/scanmatcher_pose',])
_, bag_duration = ros_utils.play_bag(self.data_sources, use_clock=True)
time.sleep(bag_duration)
ros_utils.stop_recording_bags()
icp = ros_utils.bag_to_dataframe(output_bag_path, topic=r'/scanmatcher_pose', fields = ['x', 'y'])
viz_utils.plot_2d_trajectory([icp], labels=['icp'], file_name=os.path.join(self.repetition_dir, 'icp'))
self.results[self.repetition_id]['icp_error'] = np.linalg.norm(icp.iloc[-1] - icp.iloc[0])
import os
import pandas as pd
import matplotlib.pyplot as plt
from framework import utils
from framework import viz_utils
#################################################################################################
# CONFIG #
#################################################################################################
original_experiment_dir = r'/home/omer/temp/20190316-152308_apr_icp/20190316-152308_icp_snapshots_for_fork_trajectory_on_15-08-1'
repetition_ids = [1, 2, 3]
#################################################################################################
if __name__ == '__main__':
execution_dir = utils.create_new_execution_folder('icp_post_processing')
for repetition_id in repetition_ids:
plt.figure()
icp_df = pd.read_csv(os.path.join(original_experiment_dir,
str(repetition_id),
'canopies_icp_results.csv'),
index_col=0)
viz_utils.plot_2d_trajectory([
icp_df.loc[:, [
'icp_pose_x[%d]' % repetition_id,
'icp_pose_y[%d]' % repetition_id
]]
])
plt.savefig(os.path.join(execution_dir, 'icp_%d.jpg' % repetition_id))
experiment_summary = json.load(f)
with open(experiment_summary['data_sources']['ugv_poses_path']) as f:
ugv_poses_path = json.load(f)
relevant_update_index = experiment_summary['data_sources']['relevant_update_index']
ekf_df = pd.read_csv(experiment_summary['results']['1']['ekf_pose_path'], index_col=0)
ekf_with_aerial_update_df = pd.read_csv(experiment_summary['results']['1']['ekf_with_aerial_update_path'], index_col=0)
update_time = ekf_with_aerial_update_df.loc[(ekf_with_aerial_update_df['pose.pose.position.x'].diff().abs() > 0.5) |
(ekf_with_aerial_update_df['pose.pose.position.y'].diff().abs() > 0.5)].index[0]
ekf_before_update_df = ekf_with_aerial_update_df[ekf_with_aerial_update_df.index < update_time]
ekf_without_update_df = ekf_df[ekf_df.index > update_time]
ekf_with_update_df = ekf_with_aerial_update_df[ekf_with_aerial_update_df.index > update_time]
update_arrow_df = pd.concat([ekf_before_update_df.iloc[-1], ekf_with_update_df.iloc[0]], axis=1).transpose()
fig.add_subplot(4, 2, experiment_idx + 1)
label_x_axis = True if experiment_idx in [6, 7] else False
label_y_axis = True if experiment_idx in [0, 2, 4, 6] else False
viz_utils.plot_2d_trajectory([ekf_before_update_df, ekf_without_update_df, ekf_with_update_df],
colors=['black', 'black', 'deeppink'], label_x_axis=label_x_axis, label_y_axis=label_y_axis)
plt.plot(update_arrow_df.iloc[:,0], update_arrow_df.iloc[:,1], 'deeppink', linestyle='--')
ax = plt.gca()
start_circle = plt.Circle(tuple(ekf_before_update_df.iloc[0]), 2, color='red')
end_with_update_circle = plt.Circle(tuple(ekf_with_update_df.iloc[-1]), 2, color='deeppink')
end_without_update_circle = plt.Circle(tuple(ekf_without_update_df.iloc[-1]), 2, color='black')
ax.add_artist(start_circle)
ax.add_artist(end_with_update_circle)
ax.add_artist(end_without_update_circle)
plt.title(chr(65 + experiment_idx))
ekf_errors[relevant_update_index] = np.linalg.norm(ekf_df.iloc[-1])
ekf_with_aerial_update_errors[relevant_update_index] = np.linalg.norm(ekf_with_aerial_update_df.iloc[-1])
errors_comparison_df = pd.concat([pd.Series(ekf_errors), pd.Series(ekf_with_aerial_update_errors)], axis=1).rename(columns={0: 'baseline', 1: 'aerial_updates'})
errors_comparison_df.to_csv(os.path.join(execution_dir, 'ekf_updates_errors.csv'))
fig.legend([Line2D([0], [0], color='black', linewidth=2), Line2D([0], [0], color='deeppink', linewidth=2)],
['baseline', 'updated'], loc='lower center', ncol=2)
odom = ros_utils.bag_to_dataframe(
bag_descriptor.path,
topic=r'/jackal_velocity_controller/odom',
fields=['pose.pose.position.x', 'pose.pose.position.y'])
ekf = ros_utils.bag_to_dataframe(
bag_descriptor.path,
topic=r'/odometry/filtered',
fields=['pose.pose.position.x', 'pose.pose.position.y'])
icp_bag_path = os.path.join(icp_experiment_path, '1',
'icp_on_apr_%s_output.bag' % ugv_bag_name)
icp = ros_utils.bag_to_dataframe(icp_bag_path,
topic=r'/scanmatcher_pose',
fields=['x', 'y'])
plt.figure()
viz_utils.plot_2d_trajectory([odom])
plt.title('raw odometry')
plt.tight_layout()
plt.savefig(os.path.join(execution_dir, 'raw_odometry.jpg'))
plt.figure()
viz_utils.plot_2d_trajectory([ekf])
plt.title('EKF')
plt.tight_layout()
plt.savefig(os.path.join(execution_dir, 'ekf.jpg'))
plt.figure()
viz_utils.plot_2d_trajectory([icp])
plt.title('ICP')
plt.tight_layout()
plt.savefig(os.path.join(execution_dir, 'icp.jpg'))