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):
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])
def task(self, **kwargs):
launch_rviz = kwargs.get('launch_rviz', False)
origin_map_image_path = self.data_sources['map_image_path']
map_semantic_trunks = self.data_sources['map_semantic_trunks']
scan_bag_path = self.data_sources['scan_bag_path']
bounding_box_expand_ratio = self.params['bounding_box_expand_ratio']
# Generate canopies and trunk map images
map_image = cv2.imread(origin_map_image_path)
canopies_map_image = maps_generation.generate_canopies_map(map_image)
upper_left, lower_right = cv_utils.get_bounding_box(
canopies_map_image,
map_semantic_trunks.values(),
expand_ratio=bounding_box_expand_ratio)
canopies_map_image = canopies_map_image[upper_left[1]:lower_right[1],
upper_left[0]:lower_right[0]]
canopies_map_yaml_path, _ = ros_utils.save_image_to_map(
canopies_map_image,
resolution=self.params['resolution'],
map_name='canopies_map',
dir_name=self.repetition_dir)
cv2.imwrite(os.path.join(self.repetition_dir, 'map_image.jpg'),
map_image)
# Start ROS and RVIZ
ros_utils.start_master()
if launch_rviz:
ros_utils.launch_rviz(
os.path.join(config.root_dir_path,
'src/experiments_framework/framework/amcl.rviz'))
# Launch localization stack for canopies and trunks
self._launch_base_to_scan_static_tf(namespace='canopies')
self._launch_map_server(canopies_map_yaml_path, namespace='canopies')
self._launch_amcl(namespace='canopies')
self._launch_icp(namespace='canopies')
# Start input bag and wait
_, bag_duration = ros_utils.play_bag(
scan_bag_path) # TODO: need to read from data_sources
time.sleep(bag_duration)
# Stop recording output bag
ros_utils.stop_recording_bags()
# Kill RVIZ
if launch_rviz:
ros_utils.kill_rviz()
def task(self, **kwargs):
# Start ROS and RVIZ
ros_utils.start_master()
# Launch localization stack for canopies and trunks
self._launch_base_to_scan_static_tf(namespace='canopies')
self._launch_base_to_scan_static_tf(namespace='trunks')
self._launch_synthetic_scan_generator(
self.canopies_localization_image_path,
namespace='canopies',
pickle_path=self.canopies_scans_pickle_path)
self._launch_synthetic_scan_generator(
self.trunks_localization_image_path,
namespace='trunks',
pickle_path=self.trunks_scans_pickle_path)
self._launch_icp(namespace='canopies')
self._launch_icp(namespace='trunks')
# 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, [
'/ugv_pose', '/canopies/scanmatcher_pose',
'/trunks/scanmatcher_pose'
])
# Start input bag and wait
_, bag_duration = ros_utils.play_bag(self.trajectory_bag_path)
time.sleep(bag_duration)
# Stop recording output bag
ros_utils.stop_recording_bags()
# Generate results dafaframes
self._generate_results_dataframe(
namespace='canopies',
output_bag_path=output_bag_path,
image_height=self.canopies_localization_image_height)
self._generate_results_dataframe(
namespace='trunks',
output_bag_path=output_bag_path,
image_height=self.trunks_localization_image_height)
# Delete bag file
os.remove(output_bag_path)
def task(self, **kwargs):
ros_utils.start_master(use_sim_time=True)
ros_utils.launch(package='jackal_navigation',
launch_file='gmapping.launch')
_, bag_duration = ros_utils.play_bag(self.data_sources, use_clock=True)
start_time = datetime.datetime.now()
while True:
if (datetime.datetime.now() - start_time).seconds > (bag_duration +
1):
break
if kwargs.get('periodic_map_saving'):
ros_utils.save_map(
map_name=datetime.datetime.now().strftime('map_%H_%M_%S'),
dir_name=self.repetition_dir)
time.sleep(3)
ros_utils.save_map(
map_name=datetime.datetime.now().strftime('map_%H_%M_%S-final'),
dir_name=self.repetition_dir) # TODO: verify that this works
def task(self, **kwargs):
launch_rviz = kwargs.get('launch_rviz', False)
map_image_path = self.data_sources['map_image_path']
map_semantic_trunks = self.data_sources['map_semantic_trunks']
scans_and_poses_pickle_path = self.data_sources[
'scans_and_poses_pickle_path']
odom_pickle_path = self.data_sources['odom_pickle_path']
video_path = self.data_sources['video_path']
bounding_box_expand_ratio = self.params['bounding_box_expand_ratio']
map_resolution = self.params['map_resolution']
# Generate canopies map image
map_image = cv2.imread(map_image_path)
cv2.imwrite(os.path.join(self.experiment_dir, 'image_for_map.jpg'),
map_image)
canopies_map_image = maps_generation.generate_canopies_map(map_image)
lower_boundaries = [
[
canopies_map_image.shape[0] - 600,
min([trunk[0] for trunk in map_semantic_trunks.values()])
],
[
canopies_map_image.shape[0] - 600,
max([trunk[0] for trunk in map_semantic_trunks.values()])
]
]
# upper_left, lower_right = cv_utils.get_bounding_box(canopies_map_image, map_semantic_trunks.values() + lower_boundaries, expand_ratio=bounding_box_expand_ratio)
upper_left = (700, 700)
lower_right = (3200, 2700)
canopies_map_image = canopies_map_image[upper_left[1]:lower_right[1],
upper_left[0]:lower_right[0]]
canopies_map_yaml_path, _ = ros_utils.save_image_to_map(
canopies_map_image,
resolution=map_resolution,
map_name='canopies_map',
dir_name=self.experiment_dir)
# Start ROS and RVIZ
ros_utils.start_master(use_sim_time=False)
if launch_rviz:
ros_utils.launch_rviz(
os.path.join(
config.root_dir_path,
'src/experiments_framework/framework/amcl_video.rviz'))
# Launch localization stack for canopies
self._launch_base_to_scan_static_tf(namespace='canopies')
self._launch_map_server(canopies_map_yaml_path, namespace='canopies')
self._launch_amcl(namespace='canopies')
self._initialize_global_localization(namespace='canopies')
# Start playback
ros_utils.launch(package='localization',
launch_file='scan_pose_odom_playback.launch',
argv={
'ns': 'canopies',
'scans_and_poses_pickle_path':
scans_and_poses_pickle_path,
'odom_pickle_path': odom_pickle_path,
'video_path': video_path
})
time.sleep(500) # TODO: remove
# Stop recording output bag
# ros_utils.stop_recording_bags()
# Kill RVIZ
if launch_rviz:
ros_utils.kill_rviz()
def task(self, **kwargs):
launch_rviz = kwargs.get('launch_rviz', False)
# Start ROS and RVIZ
ros_utils.start_master()
if launch_rviz:
ros_utils.launch_rviz(
os.path.join(config.root_dir_path,
'src/experiments_framework/framework/amcl.rviz'))
# Launch localization stack for canopies and trunks
self._launch_world_to_map_static_tf(namespace='canopies')
self._launch_world_to_map_static_tf(namespace='trunks')
self._launch_base_to_scan_static_tf(namespace='canopies')
self._launch_base_to_scan_static_tf(namespace='trunks')
self._launch_synthetic_scan_generator(
self.canopies_localization_image_path,
namespace='canopies',
pickle_path=self.canopies_scans_pickle_path)
self._launch_synthetic_scan_generator(
self.trunks_localization_image_path,
namespace='trunks',
pickle_path=self.trunks_scans_pickle_path)
self._launch_map_server(self.canopies_map_yaml_path,
namespace='canopies')
self._launch_map_server(self.trunks_map_yaml_path, namespace='trunks')
self._launch_amcl(namespace='canopies')
self._launch_amcl(namespace='trunks')
self._initialize_global_localization(namespace='canopies')
self._initialize_global_localization(namespace='trunks')
self._launch_synthetic_odometry(namespace='canopies')
self._launch_synthetic_odometry(namespace='trunks')
if launch_rviz:
self._launch_contours_visualization()
ros_utils.play_image_to_topic(
image_path=self.data_sources['map_image_path'],
topic='map_image',
frame_id='map',
fps=config.target_system_frequency)
# 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, [
'/ugv_pose', '/canopies/amcl_pose', '/canopies/particlecloud',
'/trunks/amcl_pose', '/trunks/particlecloud'
])
# Start input bag and wait
_, bag_duration = ros_utils.play_bag(self.trajectory_bag_path)
time.sleep(bag_duration)
# Stop recording output bag
ros_utils.stop_recording_bags()
# Kill RVIZ
if launch_rviz:
ros_utils.kill_rviz()
# Generate results dafaframes
self._generate_results_dataframe(
namespace='canopies',
output_bag_path=output_bag_path,
image_height=self.canopies_localization_image_height)
self._generate_results_dataframe(
namespace='trunks',
output_bag_path=output_bag_path,
image_height=self.trunks_localization_image_height)
# Delete bag file
os.remove(output_bag_path)
import os
import time
from framework import ros_utils
from framework import config
if __name__ == '__main__':
ros_utils.start_master(use_sim_time=False)
ros_utils.launch_rviz(
os.path.join(config.root_dir_path,
'src/experiments_framework/framework/amcl.rviz'))
# Launch base_link to contours_scan_link static TF
ros_utils.launch(package='localization',
launch_file='static_identity_tf.launch',
argv={
'frame_id': 'base_link',
'child_frame_id': 'contours_scan_link'
})
# Launch map server
ros_utils.launch(package='localization',
launch_file='map.launch',
argv={
'map_yaml_path':
r'/home/omer/Downloads/panorama_cropped_map.yaml'
})
# Wait for map server to load
ros_utils.wait_for_rosout_message(
