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