diff --git a/examples/viewer.py b/examples/viewer.py index 78df8231ba..9abb80c273 100644 --- a/examples/viewer.py +++ b/examples/viewer.py @@ -3,6 +3,7 @@ # LICENSE file in the root directory of this source tree. import ctypes +import json import math import os import string @@ -14,8 +15,12 @@ flags = sys.getdlopenflags() sys.setdlopenflags(flags | ctypes.RTLD_GLOBAL) +import habitat.datasets.rearrange.samplers.receptacle as hab_receptacle +import habitat.sims.habitat_simulator.sim_utilities as sutils import magnum as mn import numpy as np +from habitat.datasets.rearrange.navmesh_utils import get_largest_island_index +from habitat.datasets.rearrange.samplers.object_sampler import ObjectSampler from magnum import shaders, text from magnum.platform.glfw import Application @@ -25,6 +30,59 @@ from habitat_sim.utils.common import quat_from_angle_axis from habitat_sim.utils.settings import default_sim_settings, make_cfg +# add tools directory so I can import things to try them in the viewer +sys.path.append(os.path.join(os.path.dirname(os.path.abspath(__file__)), "../tools")) +print(sys.path) +import collision_shape_automation as csa + +# CollisionProxyOptimizer initialized before the application +_cpo: Optional[csa.CollisionProxyOptimizer] = None +_cpo_threads = [] + + +def _cpo_initialized(): + global _cpo + global _cpo_threads + if _cpo is None: + return False + return all(not thread.is_alive() for thread in _cpo_threads) + + +class RecColorMode(Enum): + """ + Defines the coloring mode for receptacle debug drawing. + """ + + DEFAULT = 0 # all magenta + GT_ACCESS = 1 # red to green + GT_STABILITY = 2 + PR_ACCESS = 3 + PR_STABILITY = 4 + FILTERING = 5 # colored by filter status (green=active, yellow=manually filtered, red=automatically filtered (access), magenta=automatically filtered (access), blue=automatically filtered (height)) + + +class ColorLERP: + """ + xyz lerp between two colors. + """ + + def __init__(self, c0: mn.Color4, c1: mn.Color4): + self.c0 = c0.to_xyz() + self.c1 = c1.to_xyz() + self.delta = self.c1 - self.c0 + + def at(self, t: float) -> mn.Color4: + """ + Compute the LERP at time t [0,1]. + """ + assert t >= 0 and t <= 1, "Extrapolation not recommended in color space." + t_color_xyz = self.c0 + self.delta * t + return mn.Color4.from_xyz(t_color_xyz) + + +# red to green lerp for heatmaps +rg_lerp = ColorLERP(mn.Color4.red(), mn.Color4.green()) + class HabitatSimInteractiveViewer(Application): # the maximum number of chars displayable in the app window @@ -44,7 +102,11 @@ class HabitatSimInteractiveViewer(Application): # CPU and GPU usage info DISPLAY_FONT_SIZE = 16.0 - def __init__(self, sim_settings: Dict[str, Any]) -> None: + def __init__( + self, + sim_settings: Dict[str, Any], + mm: Optional[habitat_sim.metadata.MetadataMediator] = None, + ) -> None: self.sim_settings: Dict[str:Any] = sim_settings self.enable_batch_renderer: bool = self.sim_settings["enable_batch_renderer"] @@ -166,7 +228,43 @@ def __init__(self, sim_settings: Dict[str, Any]) -> None: self.previous_mouse_point = None # toggle physics simulation on/off - self.simulating = True + self.simulating = False + + # receptacle visualization + self.receptacles = None + self.display_receptacles = False + global _cpo + self._cpo = _cpo + self.cpo_initialized = False + self.show_filtered = True + self.rec_access_filter_threshold = 0.12 # empirically chosen + self.rec_color_mode = RecColorMode.FILTERING + # map receptacle to parent objects + self.rec_to_poh: Dict[hab_receptacle.Receptacle, str] = {} + # contains filtering metadata and classification of meshes filtered automatically and manually + self.rec_filter_data = None + self.rec_filter_path = self.sim_settings["rec_filter_file"] + + # display stability samples for selected object w/ receptacle + self.display_selected_stability_samples = True + + # collision proxy visualization + self.col_proxy_objs = None + self.col_proxies_visible = True + self.original_objs_visible = True + self.proxy_obj_postfix = "_collision_stand-in" + + # mouse raycast visualization + self.mouse_cast_results = None + # last clicked or None for stage + self.selected_object = None + self.selected_rec = None + self.ao_link_map = None + + # marker set cache to prevent parsing metadata every frame + self.marker_sets: Dict[str, Dict[int, Dict[str, List[mn.Vector3]]]] = None + self.debug_random_colors: List[mn.Color4] = [] + self.selected_marker_set_index = 0 # toggle a single simulation step at the next opportunity if not # simulating continuously. @@ -178,8 +276,33 @@ def __init__(self, sim_settings: Dict[str, Any]) -> None: self.tiled_sims: list[habitat_sim.simulator.Simulator] = None self.replay_renderer_cfg: Optional[ReplayRendererConfiguration] = None self.replay_renderer: Optional[ReplayRenderer] = None - self.reconfigure_sim() + self.reconfigure_sim(mm) self.debug_semantic_colors = {} + self.load_scene_filter_file() + + # ----------------------------------------- + # Clutter Generation Integration: + self.clutter_object_set = [ + "002_master_chef_can", + "003_cracker_box", + "004_sugar_box", + "005_tomato_soup_can", + "007_tuna_fish_can", + "008_pudding_box", + "009_gelatin_box", + "010_potted_meat_can", + "024_bowl", + ] + self.clutter_object_handles = [] + self.clutter_object_instances = [] + # cache initial states for classification of unstable objects + self.clutter_object_initial_states = [] + self.num_unstable_objects = 0 + # add some clutter objects to the MM + self.sim.metadata_mediator.object_template_manager.load_configs( + "data/objects/ycb/configs/" + ) + # ----------------------------------------- # compute NavMesh if not already loaded by the scene. if ( @@ -193,6 +316,248 @@ def __init__(self, sim_settings: Dict[str, Any]) -> None: logger.setLevel("INFO") self.print_help_text() + def modify_param_from_term(self): + """ + Prompts the user to enter an attribute name and new value. + Attempts to fulfill the user's request. + """ + # first get an attribute + user_attr = input("++++++++++++\nProvide an attribute to edit: ") + if not hasattr(self, user_attr): + print(f" The '{user_attr}' attribute does not exist.") + return + + # then get a value + user_val = input(f"Now provide a value for '{user_attr}': ") + cur_attr_val = getattr(self, user_attr) + if cur_attr_val is not None: + try: + # try type conversion + new_val = type(cur_attr_val)(user_val) + + # special handling for bool because all strings become True with cast + if isinstance(cur_attr_val, bool): + if user_val.lower() == "false": + new_val = False + elif user_val.lower() == "true": + new_val = True + + setattr(self, user_attr, new_val) + print( + f"attr '{user_attr}' set to '{getattr(self, user_attr)}' (type={type(new_val)})." + ) + except Exception: + print(f"Failed to cast '{user_val}' to {type(cur_attr_val)}.") + else: + print("That attribute is unset, so I don't know the type.") + + def load_scene_filter_file(self): + """ + Load the filter file for a scene from config. + """ + + scene_user_defined = self.sim.metadata_mediator.get_scene_user_defined( + self.sim.curr_scene_name + ) + if scene_user_defined is not None and scene_user_defined.has_value( + "scene_filter_file" + ): + scene_filter_file = scene_user_defined.get("scene_filter_file") + # construct the dataset level path for the filter data file + scene_filter_file = os.path.join( + os.path.dirname(mm.active_dataset), scene_filter_file + ) + print(f"scene_filter_file = {scene_filter_file}") + self.load_receptacles() + self.load_filtered_recs(scene_filter_file) + self.rec_filter_path = scene_filter_file + else: + print( + f"WARNING: No rec filter file configured for scene {self.sim.curr_scene_name}." + ) + + def get_rec_instance_name(self, receptacle: hab_receptacle.Receptacle) -> str: + """ + Gets a unique string name for the Receptacle instance. + Multiple Receptacles can share a name (e.g. if the object has multiple instances in the scene). + The unique name is constructed as '|'. + """ + rec_unique_name = receptacle.parent_object_handle + "|" + receptacle.name + return rec_unique_name + + def get_closest_tri_receptacle( + self, pos: mn.Vector3, max_dist: float = 3.5 + ) -> Optional[hab_receptacle.TriangleMeshReceptacle]: + """ + Return the closest receptacle to the given position or None. + + :param pos: The point to compare with receptacle verts. + :param max_dist: The maximum allowable distance to the receptacle to count. + + :return: None if failed or closest receptacle. + """ + if self.receptacles is None or not self.display_receptacles: + return None + closest_rec = None + closest_rec_dist = max_dist + for receptacle in self.receptacles: + g_trans = receptacle.get_global_transform(self.sim) + # transform the query point once instead of all verts + local_point = g_trans.inverted().transform_point(pos) + if (g_trans.translation - pos).length() < max_dist: + # receptacles object transform should be close to the point + for vert in receptacle.mesh_data.attribute( + mn.trade.MeshAttribute.POSITION + ): + v_dist = (local_point - vert).length() + if v_dist < closest_rec_dist: + closest_rec_dist = v_dist + closest_rec = receptacle + return closest_rec + + def compute_rec_filter_state( + self, + access_threshold: float = 0.12, + stab_threshold: float = 0.5, + filter_shape: str = "pr0", + ) -> None: + """ + Check all receptacles against automated filters to fill the + + :param access_threshold: Access threshold for filtering. Roughly % of sample points with some raycast access. + :param stab_threshold: Stability threshold for filtering. Roughly % of sample points with stable object support. + :param filter_shape: Which shape metrics to use for filter. Choices typically "gt"(ground truth) or "pr0"(proxy shape). + """ + # load receptacles if not done + if self.receptacles is None: + self.load_receptacles() + assert ( + self._cpo is not None + ), "Must initialize the CPO before automatic filtering. Re-run with '--init-cpo'." + + # initialize if necessary + if self.rec_filter_data is None: + self.rec_filter_data = { + "active": [], + "manually_filtered": [], + "access_filtered": [], + "access_threshold": access_threshold, # set in filter procedure + "stability_filtered": [], + "stability threshold": stab_threshold, # set in filter procedure + # TODO: + "height_filtered": [], + "max_height": 0, + "min_height": 0, + } + + for rec in self.receptacles: + rec_unique_name = self.get_rec_instance_name(rec) + # respect already marked receptacles + if rec_unique_name not in self.rec_filter_data["manually_filtered"]: + rec_dat = self._cpo.gt_data[self.rec_to_poh[rec]]["receptacles"][ + rec.name + ] + rec_shape_data = rec_dat["shape_id_results"][filter_shape] + # filter by access + if ( + "access_results" in rec_shape_data + and rec_shape_data["access_results"]["receptacle_access_score"] + < access_threshold + ): + self.rec_filter_data["access_filtered"].append(rec_unique_name) + # filter by stability + elif ( + "stability_results" in rec_shape_data + and rec_shape_data["stability_results"]["success_ratio"] + < stab_threshold + ): + self.rec_filter_data["stability_filtered"].append(rec_unique_name) + # TODO: add more filters + # TODO: 1. filter by height relative to the floor + # TODO: 2. filter outdoor (raycast up) + # TODO: 3/4: filter by access/stability in scene context (relative to other objects) + # remaining receptacles are active + else: + self.rec_filter_data["active"].append(rec_unique_name) + + def export_filtered_recs(self, filepath: Optional[str] = None) -> None: + """ + Save a JSON with filtering metadata and filtered Receptacles for a scene. + + :param filepath: Defines the output filename for this JSON. If omitted, defaults to "./rec_filter_data.json". + """ + if filepath is None: + filepath = "rec_filter_data.json" + os.makedirs(os.path.dirname(filepath), exist_ok=True) + with open(filepath, "w") as f: + f.write(json.dumps(self.rec_filter_data, indent=2)) + print(f"Exported filter annotations to {filepath}.") + + def load_filtered_recs(self, filepath: Optional[str] = None) -> None: + """ + Load a Receptacle filtering metadata JSON to visualize the state of the scene. + + :param filepath: Defines the input filename for this JSON. If omitted, defaults to "./rec_filter_data.json". + """ + if filepath is None: + filepath = "rec_filter_data.json" + if not os.path.exists(filepath): + print(f"Filtered rec metadata file {filepath} does not exist. Cannot load.") + return + with open(filepath, "r") as f: + self.rec_filter_data = json.load(f) + + # assert the format is correct + assert "active" in self.rec_filter_data + assert "manually_filtered" in self.rec_filter_data + assert "access_filtered" in self.rec_filter_data + assert "stability_filtered" in self.rec_filter_data + assert "height_filtered" in self.rec_filter_data + print(f"Loaded filter annotations from {filepath}") + + def load_receptacles(self): + """ + Load all receptacle data and setup helper datastructures. + """ + self.receptacles = hab_receptacle.find_receptacles(self.sim) + self.receptacles = [ + rec + for rec in self.receptacles + if "collision_stand-in" not in rec.parent_object_handle + ] + for receptacle in self.receptacles: + if receptacle not in self.rec_to_poh: + po_handle = sutils.get_obj_from_handle( + self.sim, receptacle.parent_object_handle + ).creation_attributes.handle + self.rec_to_poh[receptacle] = po_handle + + def add_col_proxy_object( + self, obj_instance: habitat_sim.physics.ManagedRigidObject + ) -> habitat_sim.physics.ManagedRigidObject: + """ + Add a collision object visualization proxy to the scene overlapping with the given object. + Return the new proxy object. + """ + # replace the object with a collision_object + obj_temp_handle = obj_instance.creation_attributes.handle + otm = self.sim.get_object_template_manager() + object_template = otm.get_template_by_handle(obj_temp_handle) + object_template.scale = obj_instance.scale + np.ones(3) * 0.01 + object_template.render_asset_handle = object_template.collision_asset_handle + object_template.is_collidable = False + reg_id = otm.register_template( + object_template, + object_template.handle + self.proxy_obj_postfix, + ) + ro_mngr = self.sim.get_rigid_object_manager() + new_obj = ro_mngr.add_object_by_template_id(reg_id) + new_obj.motion_type = habitat_sim.physics.MotionType.KINEMATIC + new_obj.translation = obj_instance.translation + new_obj.rotation = obj_instance.rotation + self.sim.set_object_bb_draw(True, new_obj.object_id) + return new_obj + def draw_contact_debug(self, debug_line_render: Any): """ This method is called to render a debug line overlay displaying active contact points and normals. @@ -227,6 +592,33 @@ def draw_contact_debug(self, debug_line_render: Any): normal=camera_position - cp.position_on_b_in_ws, ) + def draw_marker_sets_debug(self, debug_line_render: Any) -> None: + """ + Draw the global state of all configured marker sets. + """ + camera_position = self.render_camera.render_camera.node.absolute_translation + marker_set_counter = 0 + for ao_handle in self.marker_sets: + ao = sutils.get_obj_from_handle(self.sim, ao_handle) + for link_id in self.marker_sets[ao_handle]: + for marker_set_name in self.marker_sets[ao_handle][link_id]: + if len(self.debug_random_colors) <= marker_set_counter: + self.debug_random_colors.append( + mn.Color4(mn.Vector3(np.random.random(3))) + ) + marker_set_color = self.debug_random_colors[marker_set_counter] + marker_set_counter += 1 + global_marker_set = sutils.get_global_link_marker_set( + ao, link_id, marker_set_name, self.marker_sets[ao_handle] + ) + for global_marker_pos in global_marker_set: + debug_line_render.draw_circle( + translation=global_marker_pos, + radius=0.005, + color=marker_set_color, + normal=camera_position - global_marker_pos, + ) + def draw_region_debug(self, debug_line_render: Any) -> None: """ Draw the semantic region wireframes. @@ -245,6 +637,7 @@ def debug_draw(self): """ Additional draw commands to be called during draw_event. """ + self.sim.get_rigid_object_manager() if self.debug_bullet_draw: render_cam = self.render_camera.render_camera proj_mat = render_cam.projection_matrix.__matmul__(render_cam.camera_matrix) @@ -253,7 +646,6 @@ def debug_draw(self): debug_line_render = self.sim.get_debug_line_render() if self.contact_debug_draw: self.draw_contact_debug(debug_line_render) - if self.semantic_region_debug_draw: if len(self.debug_semantic_colors) != len(self.sim.semantic_scene.regions): for region in self.sim.semantic_scene.regions: @@ -261,6 +653,166 @@ def debug_draw(self): mn.Vector3(np.random.random(3)) ) self.draw_region_debug(debug_line_render) + if self.marker_sets is not None: + self.draw_marker_sets_debug(debug_line_render) + if self.receptacles is not None and self.display_receptacles: + if self.rec_filter_data is None and self.cpo_initialized: + self.compute_rec_filter_state( + access_threshold=self.rec_access_filter_threshold + ) + c_pos = self.render_camera.node.absolute_translation + c_forward = ( + self.render_camera.node.absolute_transformation().transform_vector( + mn.Vector3(0, 0, -1) + ) + ) + for receptacle in self.receptacles: + rec_unique_name = self.get_rec_instance_name(receptacle) + # filter all non-active receptacles + if ( + self.rec_filter_data is not None + and not self.show_filtered + and rec_unique_name not in self.rec_filter_data["active"] + ): + continue + + rec_dat = None + if self.cpo_initialized: + rec_dat = self._cpo.gt_data[self.rec_to_poh[receptacle]][ + "receptacles" + ][receptacle.name] + + r_trans = receptacle.get_global_transform(self.sim) + # display point samples for selected object + if ( + rec_dat is not None + and self.display_selected_stability_samples + and self.selected_object is not None + and self.selected_object.handle == receptacle.parent_object_handle + ): + # display colored circles for stability samples on the selected object + point_metric_dat = rec_dat["shape_id_results"]["gt"][ + "access_results" + ]["receptacle_point_access_scores"] + if self.rec_color_mode == RecColorMode.GT_STABILITY: + point_metric_dat = rec_dat["shape_id_results"]["gt"][ + "stability_results" + ]["point_stabilities"] + elif self.rec_color_mode == RecColorMode.PR_STABILITY: + point_metric_dat = rec_dat["shape_id_results"]["pr0"][ + "stability_results" + ]["point_stabilities"] + elif self.rec_color_mode == RecColorMode.PR_ACCESS: + point_metric_dat = rec_dat["shape_id_results"]["pr0"][ + "access_results" + ]["receptacle_point_access_scores"] + + for point_metric, point in zip( + point_metric_dat, + rec_dat["sample_points"], + ): + self.sim.get_debug_line_render().draw_circle( + translation=r_trans.transform_point(point), + radius=0.02, + normal=mn.Vector3(0, 1, 0), + color=rg_lerp.at(point_metric), + num_segments=12, + ) + + rec_obj = sutils.get_obj_from_handle( + self.sim, receptacle.parent_object_handle + ) + key_points = [r_trans.translation] + key_points.extend( + sutils.get_bb_corners(rec_obj.root_scene_node.cumulative_bb) + ) + + in_view = False + for ix, key_point in enumerate(key_points): + r_pos = key_point + if ix > 0: + r_pos = rec_obj.transformation.transform_point(key_point) + c_to_r = r_pos - c_pos + # only display receptacles within 4 meters centered in view + if ( + c_to_r.length() < 4 + and mn.math.dot((c_to_r).normalized(), c_forward) > 0.7 + ): + in_view = True + break + if in_view: + # handle coloring + rec_color = None + if self.selected_rec == receptacle: + # white + rec_color = mn.Color4.cyan() + elif ( + self.rec_filter_data is not None + ) and self.rec_color_mode == RecColorMode.FILTERING: + # blue indicates no filter data for the receptacle, it may be newer than the filter file. + rec_color = mn.Color4.blue() + if rec_unique_name in self.rec_filter_data["active"]: + rec_color = mn.Color4.green() + elif ( + rec_unique_name in self.rec_filter_data["manually_filtered"] + ): + rec_color = mn.Color4.yellow() + elif rec_unique_name in self.rec_filter_data["access_filtered"]: + rec_color = mn.Color4.red() + elif ( + rec_unique_name + in self.rec_filter_data["stability_filtered"] + ): + rec_color = mn.Color4.magenta() + elif rec_unique_name in self.rec_filter_data["height_filtered"]: + rec_color = mn.Color4.blue() + elif ( + self.cpo_initialized + and self.rec_color_mode != RecColorMode.DEFAULT + ): + if self.rec_color_mode == RecColorMode.GT_STABILITY: + rec_color = rg_lerp.at( + rec_dat["shape_id_results"]["gt"]["stability_results"][ + "success_ratio" + ] + ) + elif self.rec_color_mode == RecColorMode.GT_ACCESS: + rec_color = rg_lerp.at( + rec_dat["shape_id_results"]["gt"]["access_results"][ + "receptacle_access_score" + ] + ) + elif self.rec_color_mode == RecColorMode.PR_STABILITY: + rec_color = rg_lerp.at( + rec_dat["shape_id_results"]["pr0"]["stability_results"][ + "success_ratio" + ] + ) + elif self.rec_color_mode == RecColorMode.PR_ACCESS: + rec_color = rg_lerp.at( + rec_dat["shape_id_results"]["pr0"]["access_results"][ + "receptacle_access_score" + ] + ) + + receptacle.debug_draw(self.sim, color=rec_color) + if True: + dblr = self.sim.get_debug_line_render() + t_form = receptacle.get_global_transform(self.sim) + dblr.push_transform(t_form) + dblr.draw_transformed_line( + mn.Vector3(0), receptacle.up, mn.Color4.cyan() + ) + dblr.pop_transform() + # mouse raycast circle + white = mn.Color4(mn.Vector3(1.0), 1.0) + if self.mouse_cast_results is not None and self.mouse_cast_results.has_hits(): + self.sim.get_debug_line_render().draw_circle( + translation=self.mouse_cast_results.hits[0].point, + radius=0.005, + color=white, + normal=self.mouse_cast_results.hits[0].normal, + ) def draw_event( self, @@ -272,6 +824,10 @@ def draw_event( Calls continuously to re-render frames and swap the two frame buffers at a fixed rate. """ + # until cpo initialization is finished, keep checking + if not self.cpo_initialized: + self.cpo_initialized = _cpo_initialized() + agent_acts_per_sec = self.fps mn.gl.default_framebuffer.clear( @@ -290,6 +846,17 @@ def draw_event( self.simulate_single_step = False if simulation_call is not None: simulation_call() + # compute object stability after physics step + self.num_unstable_objects = 0 + for obj_initial_state, obj in zip( + self.clutter_object_initial_states, self.clutter_object_instances + ): + translation_error = ( + obj_initial_state[0] - obj.translation + ).length() + if translation_error > 0.1: + self.num_unstable_objects += 1 + if global_call is not None: global_call() @@ -362,7 +929,9 @@ def default_agent_config(self) -> habitat_sim.agent.AgentConfiguration: ) return agent_config - def reconfigure_sim(self) -> None: + def reconfigure_sim( + self, mm: Optional[habitat_sim.metadata.MetadataMediator] = None + ) -> None: """ Utilizes the current `self.sim_settings` to configure and set up a new `habitat_sim.Simulator`, and then either starts a simulation instance, or replaces @@ -370,6 +939,7 @@ def reconfigure_sim(self) -> None: """ # configure our sim_settings but then set the agent to our default self.cfg = make_cfg(self.sim_settings) + self.cfg.metadata_mediator = mm self.agent_id: int = self.sim_settings["default_agent"] self.cfg.agents[self.agent_id] = self.default_agent_config() @@ -428,6 +998,10 @@ def reconfigure_sim(self) -> None: for composite_file in sim_settings["composite_files"]: self.replay_renderer.preload_file(composite_file) + self.ao_link_map = sutils.get_ao_link_id_map(self.sim) + + self.marker_sets = sutils.parse_scene_marker_sets(self.sim) + Timer.start() self.step = -1 @@ -573,57 +1147,207 @@ def key_press_event(self, event: Application.KeyEvent) -> None: # TODO: add a nice log message with concise contact pair naming. elif key == pressed.T: + self.modify_param_from_term() + # load URDF - fixed_base = alt_pressed - urdf_file_path = "" - if shift_pressed and self.cached_urdf: - urdf_file_path = self.cached_urdf + # fixed_base = alt_pressed + # urdf_file_path = "" + # if shift_pressed and self.cached_urdf: + # urdf_file_path = self.cached_urdf + # else: + # urdf_file_path = input("Load URDF: provide a URDF filepath:").strip() + # if not urdf_file_path: + # logger.warn("Load URDF: no input provided. Aborting.") + # elif not urdf_file_path.endswith((".URDF", ".urdf")): + # logger.warn("Load URDF: input is not a URDF. Aborting.") + # elif os.path.exists(urdf_file_path): + # self.cached_urdf = urdf_file_path + # aom = self.sim.get_articulated_object_manager() + # ao = aom.add_articulated_object_from_urdf( + # urdf_file_path, + # fixed_base, + # 1.0, + # 1.0, + # True, + # maintain_link_order=False, + # intertia_from_urdf=False, + # ) + # ao.translation = ( + # self.default_agent.scene_node.transformation.transform_point( + # [0.0, 1.0, -1.5] + # ) + # ) + # # check removal and auto-creation + # joint_motor_settings = habitat_sim.physics.JointMotorSettings( + # position_target=0.0, + # position_gain=1.0, + # velocity_target=0.0, + # velocity_gain=1.0, + # max_impulse=1000.0, + # ) + # existing_motor_ids = ao.existing_joint_motor_ids + # for motor_id in existing_motor_ids: + # ao.remove_joint_motor(motor_id) + # ao.create_all_motors(joint_motor_settings) + # else: + # logger.warn("Load URDF: input file not found. Aborting.") + + elif key == pressed.M: + if ( + shift_pressed + and self.selected_object is not None + and isinstance(self.selected_object, physics.ManagedArticulatedObject) + and self.selected_object.handle in self.marker_sets + ): + sutils.write_ao_marker_sets( + self.selected_object, self.marker_sets[self.selected_object.handle] + ) + sutils.write_ao_marker_set_to_template(self.sim, self.selected_object) + print(f"Saved config for {self.selected_object.handle}.") else: - urdf_file_path = input("Load URDF: provide a URDF filepath:").strip() - - if not urdf_file_path: - logger.warn("Load URDF: no input provided. Aborting.") - elif not urdf_file_path.endswith((".URDF", ".urdf")): - logger.warn("Load URDF: input is not a URDF. Aborting.") - elif os.path.exists(urdf_file_path): - self.cached_urdf = urdf_file_path - aom = self.sim.get_articulated_object_manager() - ao = aom.add_articulated_object_from_urdf( - urdf_file_path, - fixed_base, - 1.0, - 1.0, - True, - maintain_link_order=False, - intertia_from_urdf=False, + self.cycle_mouse_mode() + logger.info(f"Command: mouse mode set to {self.mouse_interaction}") + + elif key == pressed.V: + # load receptacles and toggle visibilty or color mode (+SHIFT) + if self.receptacles is None: + self.load_receptacles() + + if shift_pressed: + self.rec_color_mode = RecColorMode( + (self.rec_color_mode.value + 1) % len(RecColorMode) ) - ao.translation = ( - self.default_agent.scene_node.transformation.transform_point( - [0.0, 1.0, -1.5] + print(f"self.rec_color_mode = {self.rec_color_mode}") + self.display_receptacles = True + else: + self.display_receptacles = not self.display_receptacles + print(f"self.display_receptacles = {self.display_receptacles}") + + elif key == pressed.F: + # toggle, load(+ALT), or save(+SHIFT) filtering + if shift_pressed and self.rec_filter_data is not None: + self.export_filtered_recs(self.rec_filter_path) + elif alt_pressed: + self.load_filtered_recs(self.rec_filter_path) + else: + self.show_filtered = not self.show_filtered + print(f"self.show_filtered = {self.show_filtered}") + + elif key == pressed.U: + rom = self.sim.get_rigid_object_manager() + # add objects to the selected receptacle or remove al objects + if shift_pressed: + # remove all + print(f"Removing {len(self.clutter_object_instances)} clutter objects.") + for obj in self.clutter_object_instances: + rom.remove_object_by_handle(obj.handle) + self.clutter_object_initial_states.clear() + self.clutter_object_instances.clear() + else: + # try to sample an object from the selected object receptacles + rec_set = None + if alt_pressed: + # use all active filter recs + rec_set = [ + rec + for rec in self.receptacles + if rec.unique_name in self.rec_filter_data["active"] + ] + elif self.selected_rec is not None: + rec_set = [self.selected_rec] + elif self.selected_object is not None: + rec_set = [ + rec + for rec in self.receptacles + if self.selected_object.handle == rec.parent_object_handle + ] + if rec_set is not None: + if len(self.clutter_object_handles) == 0: + for obj_name in self.clutter_object_set: + matching_handles = self.sim.metadata_mediator.object_template_manager.get_template_handles( + obj_name + ) + assert ( + len(matching_handles) > 0 + ), f"No matching template for '{obj_name}' in the dataset." + self.clutter_object_handles.append(matching_handles[0]) + + rec_set_unique_names = [rec.unique_name for rec in rec_set] + obj_samp = ObjectSampler( + self.clutter_object_handles, + ["rec set"], + orientation_sample="up", + num_objects=(1, 10), ) - ) - # check removal and auto-creation - joint_motor_settings = habitat_sim.physics.JointMotorSettings( - position_target=0.0, - position_gain=1.0, - velocity_target=0.0, - velocity_gain=1.0, - max_impulse=1000.0, - ) - existing_motor_ids = ao.existing_joint_motor_ids - for motor_id in existing_motor_ids: - ao.remove_joint_motor(motor_id) - ao.create_all_motors(joint_motor_settings) + rec_set_obj = hab_receptacle.ReceptacleSet( + "rec set", [""], [], rec_set_unique_names, [] + ) + obj_count_dict = {rec.unique_name: 200 for rec in rec_set} + recep_tracker = hab_receptacle.ReceptacleTracker( + obj_count_dict, + {"rec set": rec_set_obj}, + ) + new_objs = obj_samp.sample( + self.sim, recep_tracker, [], snap_down=True + ) + for obj, rec in new_objs: + self.clutter_object_instances.append(obj) + self.clutter_object_initial_states.append( + (obj.translation, obj.rotation) + ) + print(f"Sampled '{obj.handle}' in '{rec.unique_name}'") + else: + print("No object selected, cannot sample clutter.") + + elif key == pressed.O: + if shift_pressed: + # move non-proxy objects in/out of visible space + self.original_objs_visible = not self.original_objs_visible + print(f"self.original_objs_visible = {self.original_objs_visible}") + if not self.original_objs_visible: + for _obj_handle, obj in ( + self.sim.get_rigid_object_manager() + .get_objects_by_handle_substring() + .items() + ): + if self.proxy_obj_postfix not in obj.creation_attributes.handle: + obj.motion_type = habitat_sim.physics.MotionType.KINEMATIC + obj.translation = obj.translation + mn.Vector3(200, 0, 0) + obj.motion_type = habitat_sim.physics.MotionType.STATIC + else: + for _obj_handle, obj in ( + self.sim.get_rigid_object_manager() + .get_objects_by_handle_substring() + .items() + ): + if self.proxy_obj_postfix not in obj.creation_attributes.handle: + obj.motion_type = habitat_sim.physics.MotionType.KINEMATIC + obj.translation = obj.translation - mn.Vector3(200, 0, 0) + obj.motion_type = habitat_sim.physics.MotionType.STATIC + else: - logger.warn("Load URDF: input file not found. Aborting.") + if self.col_proxy_objs is None: + self.col_proxy_objs = [] + for _obj_handle, obj in ( + self.sim.get_rigid_object_manager() + .get_objects_by_handle_substring() + .items() + ): + if self.proxy_obj_postfix not in obj.creation_attributes.handle: + # add a new proxy object + self.col_proxy_objs.append(self.add_col_proxy_object(obj)) + else: + self.col_proxies_visible = not self.col_proxies_visible + print(f"self.col_proxies_visible = {self.col_proxies_visible}") - elif key == pressed.M: - self.cycle_mouse_mode() - logger.info(f"Command: mouse mode set to {self.mouse_interaction}") + # make the proxies visible or not by moving them + if not self.col_proxies_visible: + for obj in self.col_proxy_objs: + obj.translation = obj.translation + mn.Vector3(200, 0, 0) + else: + for obj in self.col_proxy_objs: + obj.translation = obj.translation - mn.Vector3(200, 0, 0) - elif key == pressed.V: - self.invert_gravity() - logger.info("Command: gravity inverted") elif key == pressed.N: # (default) - toggle navmesh visualization # NOTE: (+ALT) - re-sample the agent position on the NavMesh @@ -632,8 +1356,16 @@ def key_press_event(self, event: Application.KeyEvent) -> None: logger.info("Command: resample agent state from navmesh") if self.sim.pathfinder.is_loaded: new_agent_state = habitat_sim.AgentState() + largest_island_ix = get_largest_island_index( + pathfinder=self.sim.pathfinder, + sim=self.sim, + allow_outdoor=False, + ) + print(f"Largest indoor island index = {largest_island_ix}") new_agent_state.position = ( - self.sim.pathfinder.get_random_navigable_point() + self.sim.pathfinder.get_random_navigable_point( + island_index=largest_island_ix + ) ) new_agent_state.rotation = quat_from_angle_axis( self.sim.random.uniform_float(0, 2.0 * np.pi), @@ -680,6 +1412,11 @@ def mouse_move_event(self, event: Application.MouseMoveEvent) -> None: mouse button to steer the agent's facing direction. When in GRAB mode, continues to update the grabber's object position with our agents position. """ + + render_camera = self.render_camera.render_camera + ray = render_camera.unproject(self.get_mouse_position(event.position)) + self.mouse_cast_results = self.sim.cast_ray(ray=ray) + button = Application.MouseMoveEvent.Buttons # if interactive mode -> LOOK MODE if event.buttons == button.LEFT and self.mouse_interaction == MouseMode.LOOK: @@ -712,92 +1449,231 @@ def mouse_press_event(self, event: Application.MouseEvent) -> None: """ button = Application.MouseEvent.Button physics_enabled = self.sim.get_physics_simulation_library() + mod = Application.InputEvent.Modifier + shift_pressed = bool(event.modifiers & mod.SHIFT) + alt_pressed = bool(event.modifiers & mod.ALT) # if interactive mode is True -> GRAB MODE if self.mouse_interaction == MouseMode.GRAB and physics_enabled: - render_camera = self.render_camera.render_camera - ray = render_camera.unproject(self.get_mouse_position(event.position)) - raycast_results = self.sim.cast_ray(ray=ray) - - if raycast_results.has_hits(): - hit_object, ao_link = -1, -1 - hit_info = raycast_results.hits[0] - - if hit_info.object_id > habitat_sim.stage_id: - # we hit an non-staged collision object - ro_mngr = self.sim.get_rigid_object_manager() - ao_mngr = self.sim.get_articulated_object_manager() - ao = ao_mngr.get_object_by_id(hit_info.object_id) - ro = ro_mngr.get_object_by_id(hit_info.object_id) - - if ro: - # if grabbed an object - hit_object = hit_info.object_id - object_pivot = ro.transformation.inverted().transform_point( - hit_info.point - ) - object_frame = ro.rotation.inverted() - elif ao: - # if grabbed the base link - hit_object = hit_info.object_id - object_pivot = ao.transformation.inverted().transform_point( - hit_info.point - ) - object_frame = ao.rotation.inverted() - else: - for ao_handle in ao_mngr.get_objects_by_handle_substring(): - ao = ao_mngr.get_object_by_handle(ao_handle) - link_to_obj_ids = ao.link_object_ids - - if hit_info.object_id in link_to_obj_ids: - # if we got a link - ao_link = link_to_obj_ids[hit_info.object_id] - object_pivot = ( - ao.get_link_scene_node(ao_link) - .transformation.inverted() - .transform_point(hit_info.point) - ) - object_frame = ao.get_link_scene_node( - ao_link - ).rotation.inverted() - hit_object = ao.object_id - break - # done checking for AO - - if hit_object >= 0: - node = self.default_agent.scene_node - constraint_settings = physics.RigidConstraintSettings() - - constraint_settings.object_id_a = hit_object - constraint_settings.link_id_a = ao_link - constraint_settings.pivot_a = object_pivot - constraint_settings.frame_a = ( - object_frame.to_matrix() @ node.rotation.to_matrix() - ) - constraint_settings.frame_b = node.rotation.to_matrix() - constraint_settings.pivot_b = hit_info.point + ao_link = -1 + hit_info = self.mouse_cast_results.hits[0] - # by default use a point 2 point constraint - if event.button == button.RIGHT: - constraint_settings.constraint_type = ( - physics.RigidConstraintType.Fixed - ) + if hit_info.object_id > habitat_sim.stage_id: + obj = sutils.get_obj_from_id( + self.sim, hit_info.object_id, self.ao_link_map + ) - grip_depth = ( - hit_info.point - render_camera.node.absolute_translation - ).length() + if obj is None: + raise AssertionError( + "hit object_id is not valid. Did not find object or link." + ) + + if obj.object_id == hit_info.object_id: + # ro or ao base + object_pivot = obj.transformation.inverted().transform_point( + hit_info.point + ) + object_frame = obj.rotation.inverted() + elif isinstance(obj, physics.ManagedArticulatedObject): + # link + ao_link = obj.link_object_ids[hit_info.object_id] + object_pivot = ( + obj.get_link_scene_node(ao_link) + .transformation.inverted() + .transform_point(hit_info.point) + ) + object_frame = obj.get_link_scene_node(ao_link).rotation.inverted() + + print(f"Grabbed object {obj.handle}") + if ao_link >= 0: + print(f" link id {ao_link}") + + # setup the grabbing constraints + node = self.default_agent.scene_node + constraint_settings = physics.RigidConstraintSettings() + + constraint_settings.object_id_a = obj.object_id + constraint_settings.link_id_a = ao_link + constraint_settings.pivot_a = object_pivot + constraint_settings.frame_a = ( + object_frame.to_matrix() @ node.rotation.to_matrix() + ) + constraint_settings.frame_b = node.rotation.to_matrix() + constraint_settings.pivot_b = hit_info.point + + # by default use a point 2 point constraint + if event.button == button.RIGHT: + constraint_settings.constraint_type = ( + physics.RigidConstraintType.Fixed + ) + + grip_depth = ( + hit_info.point + - self.render_camera.render_camera.node.absolute_translation + ).length() + + self.mouse_grabber = MouseGrabber( + constraint_settings, + grip_depth, + self.sim, + ) - self.mouse_grabber = MouseGrabber( - constraint_settings, - grip_depth, - self.sim, - ) - else: - logger.warn("Oops, couldn't find the hit object. That's odd.") # end if didn't hit the scene # end has raycast hit # end has physics enabled + elif ( + self.mouse_interaction == MouseMode.LOOK + and physics_enabled + and self.mouse_cast_results is not None + and self.mouse_cast_results.has_hits() + and event.button == button.RIGHT + ): + self.selected_object = None + self.selected_rec = None + hit_id = self.mouse_cast_results.hits[0].object_id + # right click in look mode to print object information + if hit_id == habitat_sim.stage_id: + print("This is the stage.") + else: + obj = sutils.get_obj_from_id(self.sim, hit_id) + self.selected_object = obj + print(f"Object: {obj.handle}") + if self.receptacles is not None: + for rec in self.receptacles: + if rec.parent_object_handle == obj.handle: + print(f" - Receptacle: {rec.name}") + if shift_pressed: + self.selected_rec = self.get_closest_tri_receptacle( + self.mouse_cast_results.hits[0].point + ) + if self.selected_rec is not None: + print(f"Selected Receptacle: {self.selected_rec.name}") + elif alt_pressed: + filtered_rec = self.get_closest_tri_receptacle( + self.mouse_cast_results.hits[0].point + ) + if filtered_rec is not None: + filtered_rec_name = self.get_rec_instance_name(filtered_rec) + print(f"Modified Receptacle Filter State: {filtered_rec_name}") + if ( + filtered_rec_name + in self.rec_filter_data["manually_filtered"] + ): + print(" remove from manual filter") + # this was manually filtered, remove it and try to make active + self.rec_filter_data["manually_filtered"].remove( + filtered_rec_name + ) + add_to_active = True + for other_out_set in [ + "access_filtered", + "stability_filtered", + "height_filtered", + ]: + if ( + filtered_rec_name + in self.rec_filter_data[other_out_set] + ): + print(f" is in {other_out_set}") + add_to_active = False + break + if add_to_active: + print(" is active") + self.rec_filter_data["active"].append(filtered_rec_name) + elif filtered_rec_name in self.rec_filter_data["active"]: + print(" remove from active, add manual filter") + # this was active, remove it and mark manually filtered + self.rec_filter_data["active"].remove(filtered_rec_name) + self.rec_filter_data["manually_filtered"].append( + filtered_rec_name + ) + else: + print(" add to manual filter, but has other filter") + # this is already filtered, but add it to manual filters + self.rec_filter_data["manually_filtered"].append( + filtered_rec_name + ) + elif ( + self.mouse_interaction == MouseMode.MARKER + and physics_enabled + and self.mouse_cast_results is not None + and self.mouse_cast_results.has_hits() + ): + hit_info = self.mouse_cast_results.hits[0] + obj = sutils.get_obj_from_id(self.sim, hit_info.object_id, self.ao_link_map) + + if ( + isinstance(obj, physics.ManagedArticulatedObject) + and obj.object_id != hit_info.object_id + ): + # this is an ArticulatedLink, so we can add markers' + link_ix = obj.link_object_ids[hit_info.object_id] + link_node = obj.get_link_scene_node(link_ix) + local_hit_point = link_node.transformation.inverted().transform_point( + hit_info.point + ) + if self.marker_sets is not None: + if obj.handle not in self.marker_sets: + self.marker_sets[obj.handle] = {} + if link_ix not in self.marker_sets[obj.handle]: + self.marker_sets[obj.handle][link_ix] = {} + existing_set_names = list( + self.marker_sets[obj.handle][link_ix].keys() + ) + selected_set_name = None + if self.selected_marker_set_index >= len(existing_set_names): + self.selected_marker_set_index = len(existing_set_names) + selected_set_name = f"handle_{self.selected_marker_set_index}" + else: + selected_set_name = existing_set_names[ + self.selected_marker_set_index + ] + if ( + event.button == button.RIGHT + and selected_set_name in existing_set_names + ): + # remove instead of add + closest_marker_index = None + closest_marker_dist = 9999 + for m_ix in range( + len( + self.marker_sets[obj.handle][link_ix][selected_set_name] + ) + ): + m_dist = ( + local_hit_point + - self.marker_sets[obj.handle][link_ix][ + selected_set_name + ][m_ix] + ).length() + if m_dist < closest_marker_dist: + closest_marker_dist = m_dist + closest_marker_index = m_ix + if closest_marker_index is not None: + del self.marker_sets[obj.handle][link_ix][ + selected_set_name + ][closest_marker_index] + elif event.button == button.LEFT: + # add a point + if ( + selected_set_name + not in self.marker_sets[obj.handle][link_ix] + ): + self.marker_sets[obj.handle][link_ix][ + selected_set_name + ] = [] + self.marker_sets[obj.handle][link_ix][selected_set_name].append( + local_hit_point + ) + else: + obj_name = "stage" + if obj is not None: + obj_name = obj.handle + print( + f"Can't add marker, hit non link object ({hit_info.object_id}): '{obj_name}'." + ) + self.previous_mouse_point = self.get_mouse_position(event.position) self.redraw() event.accepted = True @@ -852,6 +1728,14 @@ def mouse_scroll_event(self, event: Application.MouseScrollEvent) -> None: # update location of grabbed object self.mouse_grabber.grip_depth += scroll_delta self.update_grab_position(self.get_mouse_position(event.position)) + elif self.mouse_interaction == MouseMode.MARKER: + if scroll_mod_val > 0: + self.selected_marker_set_index += 1 + else: + self.selected_marker_set_index = max( + self.selected_marker_set_index - 1, 0 + ) + self.redraw() event.accepted = True @@ -899,6 +1783,8 @@ def cycle_mouse_mode(self) -> None: if self.mouse_interaction == MouseMode.LOOK: self.mouse_interaction = MouseMode.GRAB elif self.mouse_interaction == MouseMode.GRAB: + self.mouse_interaction = MouseMode.MARKER + elif self.mouse_interaction == MouseMode.MARKER: self.mouse_interaction = MouseMode.LOOK def navmesh_config_and_recompute(self) -> None: @@ -944,12 +1830,16 @@ def draw_text(self, sensor_spec): mouse_mode_string = "LOOK" elif self.mouse_interaction == MouseMode.GRAB: mouse_mode_string = "GRAB" + elif self.mouse_interaction == MouseMode.MARKER: + mouse_mode_string = "MARKER" self.window_text.render( f""" {self.fps} FPS Sensor Type: {sensor_type_string} Sensor Subtype: {sensor_subtype_string} Mouse Interaction Mode: {mouse_mode_string} +Selected marker_set index: {self.selected_marker_set_index} +Unstable Objects: {self.num_unstable_objects} of {len(self.clutter_object_instances)} """ ) self.shader.draw(self.window_text.mesh) @@ -973,7 +1863,10 @@ def print_help_text(self) -> None: Click and drag to rotate the agent and look up/down. WHEEL: Modify orthographic camera zoom/perspective camera FOV (+SHIFT for fine grained control) - + RIGHT: + Click an object to select the object. Prints object name and attached receptacle names. Selected object displays sample points when cpo is initialized. + (+SHIFT) select a receptacle. + (+ALT) add or remove a receptacle from the "manual filter set". In GRAB mode (with 'enable-physics'): LEFT: Click and drag to pickup and move an object with a point-to-point constraint (e.g. ball joint). @@ -1011,10 +1904,20 @@ def print_help_text(self) -> None: Object Interactions: SPACE: Toggle physics simulation on/off. '.': Take a single simulation step if not simulating continuously. - 'v': (physics) Invert gravity. - 't': Load URDF from filepath - (+SHIFT) quick re-load the previously specified URDF - (+ALT) load the URDF with fixed base + + Receptacle Evaluation Tool UI: + 'v': Load all Receptacles for the scene and toggle Receptacle visibility. + (+SHIFT) Iterate through receptacle color modes. + 'f': Toggle Receptacle view filtering. When on, only non-filtered Receptacles are visible. + (+SHIFT) Export current filter metadata to file. + (+ALT) Import filter metadata from file. + 'o': Toggle display of collision proxy shapes for the scene. + (+SHIFT) Toggle display of original render shapes (and Receptacles). + 't': CLI for modifying un-bound viewer parameters during runtime. + 'u': Sample an object placement from the currently selected object or receptacle. + (+SHIFT) Remove all previously sampled objects. + (+ALT) Sample from all "active" unfiltered Receptacles. + ===================================================== """ ) @@ -1024,6 +1927,7 @@ class MouseMode(Enum): LOOK = 0 GRAB = 1 MOTION = 2 + MARKER = 3 class MouseGrabber: @@ -1133,6 +2037,45 @@ def next_frame() -> None: Timer.prev_frame_time = time.time() +def init_cpo_for_scene(sim_settings, mm: habitat_sim.metadata.MetadataMediator): + """ + Initialize and run th CPO for all objects in the scene. + """ + global _cpo + global _cpo_threads + + _cpo = csa.CollisionProxyOptimizer(sim_settings, None, mm) + + # get object handles from a specific scene + objects_in_scene = csa.get_objects_in_scene( + dataset_path=sim_settings["scene_dataset_config_file"], + scene_handle=sim_settings["scene"], + mm=_cpo.mm, + ) + # get a subset with receptacles defined + objects_in_scene = [ + objects_in_scene[i] + for i in range(len(objects_in_scene)) + if csa.object_has_receptacles(objects_in_scene[i], mm.object_template_manager) + ] + + def run_cpo_for_obj(obj_handle): + _cpo.setup_obj_gt(obj_handle) + _cpo.compute_receptacle_stability(obj_handle, use_gt=True) + _cpo.compute_receptacle_stability(obj_handle) + _cpo.compute_receptacle_access_metrics(obj_handle, use_gt=True) + _cpo.compute_receptacle_access_metrics(obj_handle, use_gt=False) + + # run CPO initialization multi-threaded to unblock viewer initialization and use + + threads = [] + for obj_handle in objects_in_scene: + run_cpo_for_obj(obj_handle) + # threads.append(threading.Thread(target=run_cpo_for_obj, args=(obj_handle,))) + for thread in threads: + thread.start() + + if __name__ == "__main__": import argparse @@ -1152,6 +2095,17 @@ def next_frame() -> None: metavar="DATASET", help='dataset configuration file to use (default: "default")', ) + parser.add_argument( + "--rec-filter-file", + default="./rec_filter_data.json", + type=str, + help='Receptacle filtering metadata (default: "./rec_filter_data.json")', + ) + parser.add_argument( + "--init-cpo", + action="store_true", + help="Initialize and run the CPO for the current scene.", + ) parser.add_argument( "--disable-physics", action="store_true", @@ -1186,13 +2140,13 @@ def next_frame() -> None: ) parser.add_argument( "--width", - default=800, + default=1080, type=int, help="Horizontal resolution of the window.", ) parser.add_argument( "--height", - default=600, + default=720, type=int, help="Vertical resolution of the window.", ) @@ -1217,8 +2171,20 @@ def next_frame() -> None: sim_settings["composite_files"] = args.composite_files sim_settings["window_width"] = args.width sim_settings["window_height"] = args.height + sim_settings["rec_filter_file"] = args.rec_filter_file sim_settings["default_agent_navmesh"] = False sim_settings["enable_hbao"] = args.hbao + # don't need auto-navmesh + sim_settings["default_agent_navmesh"] = False + + mm = habitat_sim.metadata.MetadataMediator() + mm.active_dataset = sim_settings["scene_dataset_config_file"] + + # initialize the CPO. + # this will be done in parallel to viewer setup via multithreading + if args.init_cpo: + init_cpo_for_scene(sim_settings, mm) + # start the application - HabitatSimInteractiveViewer(sim_settings).exec() + HabitatSimInteractiveViewer(sim_settings, mm).exec() diff --git a/src/esp/bindings/AttributesBindings.cpp b/src/esp/bindings/AttributesBindings.cpp index 0e501abe26..bb28f38a03 100644 --- a/src/esp/bindings/AttributesBindings.cpp +++ b/src/esp/bindings/AttributesBindings.cpp @@ -11,6 +11,7 @@ #include "esp/metadata/attributes/ArticulatedObjectAttributes.h" #include "esp/metadata/attributes/AttributesBase.h" #include "esp/metadata/attributes/LightLayoutAttributes.h" +#include "esp/metadata/attributes/MarkerSets.h" #include "esp/metadata/attributes/ObjectAttributes.h" #include "esp/metadata/attributes/PbrShaderAttributes.h" #include "esp/metadata/attributes/PhysicsManagerAttributes.h" @@ -33,10 +34,14 @@ using Attrs::CylinderPrimitiveAttributes; using Attrs::IcospherePrimitiveAttributes; using Attrs::LightInstanceAttributes; using Attrs::LightLayoutAttributes; +using Attrs::LinkSet; +using Attrs::MarkerSet; +using Attrs::MarkerSets; using Attrs::ObjectAttributes; using Attrs::PbrShaderAttributes; using Attrs::PhysicsManagerAttributes; using Attrs::StageAttributes; +using Attrs::TaskSet; using Attrs::UVSpherePrimitiveAttributes; using esp::core::managedContainers::AbstractFileBasedManagedObject; using esp::core::managedContainers::AbstractManagedObject; @@ -229,11 +234,168 @@ void initAttributesBindings(py::module& m) { .value("TREE_TRAVERSAL", metadata::attributes::ArticulatedObjectLinkOrder::TreeTraversal); + // ==== Markersets and subordinate classes === + + py::class_(m, "MarkerSet") + .def(py::init(&MarkerSet::create<>)) + .def_property_readonly( + "num_points", &MarkerSet::getNumPoints, + R"(The current number of marker points present in this MarkerSet.)") + .def( + "get_points", &MarkerSet::getAllPoints, + R"(Get an ordered list of all the 3D marker points in this MarkerSet)") + .def( + "set_points", &MarkerSet::setAllPoints, + R"(Set the marker points for this MarkerSet to be the passed list of 3D points)", + "markers"_a); + + py::class_(m, "LinkSet") + .def(py::init(&LinkSet::create<>)) + .def_property_readonly( + "num_markersets", &LinkSet::getNumMarkerSets, + R"(The current number of MarkerSets present in this LinkSet.)") + .def("has_markerset", &LinkSet::hasMarkerSet, + R"(Whether or not this LinkSet has a MarkerSet with the given name)", + "markerset_name"_a) + .def("get_all_pointset_names", &LinkSet::getAllMarkerSetNames, + R"(Get a list of all the MarkerSet names within this LinkSet)") + .def("get_markerset", &LinkSet::editMarkerSet, + R"(Get an editable reference to the specified MarkerSet, possibly + empty if it does not exist)", + "markerset_name"_a) + .def("set_markerset_points", &LinkSet::setMarkerSetPoints, + R"(Sets the marker points for the specified MarkerSet)", + "markerset_name"_a, "marker_list"_a) + .def("get_markerset_points", &LinkSet::getMarkerSetPoints, + R"(Gets the marker points for the named MarkerSet of this LinkSet)", + "markerset_name"_a) + .def( + "set_all_points", &LinkSet::setAllMarkerPoints, + R"(Sets the marker points for all the MarkerSets of this LinkSet to the + passed dictionary of values, keyed by MarkerSet name, referencing a list + of 3d points)", + "markerset_dict"_a) + .def("get_all_points", &LinkSet::getAllMarkerPoints, + R"(Get a dictionary holding all the points in this LinkSet, keyed by + MarkerSet name, referencing lists of the MarkerSet's 3d points)"); + + py::class_(m, "TaskSet") + .def(py::init(&TaskSet::create<>)) + .def_property_readonly( + "num_linksets", &TaskSet::getNumLinkSets, + R"(The current number of LinkSets present in this TaskSet.)") + .def("has_linkset", &TaskSet::hasLinkSet, + R"(Whether or not this TaskSet has a LinkSet with the given name)", + "linkset_name"_a) + .def("get_all_linkset_names", &TaskSet::getAllLinkSetNames, + R"(Get a list of all the LinkSet names within this TaskSet)") + .def("get_linkset", &TaskSet::editLinkSet, + R"(Get an editable reference to the specified LinkSet, possibly + empty if it does not exist)", + "linkset_name"_a) + .def( + "set_link_markerset_points", &TaskSet::setLinkMarkerSetPoints, + R"(Set the marker points for the specified LinkSet's specified MarkerSet + to the given list of 3d points)", + "linkset_name"_a, "markerset_name"_a, "marker_list"_a) + .def( + "set_linkset_points", &TaskSet::setLinkSetPoints, + R"(Set the marker points in each of the MarkerSets for the specified LinkSet + to the given dictionary of 3d points, keyed by the MarkerSet name)", + "linkset_name"_a, "markerset_dict"_a) + .def( + "set_all_points", &TaskSet::setAllMarkerPoints, + R"(Set the marker points for every MarkerSet of every link in this TaskSet to the values in + the passed dict of dicts. The format should be dictionary keyed by link name of dictionaries, + each keyed by MarkerSet name for the particular link with the value being a list of 3d points)", + "link_markerset_dict"_a) + + .def( + "get_link_markerset_points", &TaskSet::getLinkMarkerSetPoints, + R"(Get the marker points for the specified LinkSet's specified MarkerSet as a list of 3d points)", + "linkset_name"_a, "markerset_name"_a) + .def( + "get_linkset_points", &TaskSet::getLinkSetPoints, + R"(Get the marker points in each of the MarkerSets for the specified LinkSet + as a dictionary of lists of 3d points, keyed by the LinkSet's MarkerSet name)", + "linkset_name"_a) + .def( + "get_all_points", &TaskSet::getAllMarkerPoints, + R"(Get the marker points for every MarkerSet of every link in this TaskSet as a dict + of dicts. The format is a dictionary keyed by link name of dictionaries, + each keyed by MarkerSet name for the particular link with the value being a list + of the marker points)"); + py::class_(m, "MarkerSets") + .def(py::init(&MarkerSets::create<>)) + .def_property_readonly( + "num_tasksets", &MarkerSets::getNumTaskSets, + R"(The current number of TaskSets present in the MarkerSets collection.)") + .def("has_taskset", &MarkerSets::hasTaskSet, + R"(Whether or not a TaskSet with the given name exists)", + "taskset_name"_a) + .def("get_all_taskset_names", &MarkerSets::getAllTaskSetNames, + R"(Get a list of all the existing TaskSet names)") + .def("get_taskset", &MarkerSets::editTaskSet, + R"(Get an editable reference to the specified TaskSet, possibly + empty if it does not exist)", + "taskset_name"_a) + .def( + "set_task_link_markerset_points", + &MarkerSets::setTaskLinkMarkerSetPoints, + R"(Set the marker points for the specified TaskSet's specified LinkSet's + specified MarkerSet to the given list of 3d points)", + "taskset_name"_a, "linkset_name"_a, "markerset_name"_a, + "marker_list"_a) + .def( + "set_task_linkset_points", &MarkerSets::setTaskLinkSetPoints, + R"(Set the points in all the MarkerSets of the specified LinkSet, in the + specified TaskSet, to the given dictionary, keyed by each MarkerSet's name + and referencing a list of 3d points.)", + "taskset_name"_a, "linkset_name"_a, "markerset_dict"_a) + .def( + "set_taskset_points", &MarkerSets::setTaskSetPoints, + R"(Set all the marker points in the specified TaskSet to the 3d point values in the + passed dict of dicts. The format should be a dictionary keyed by LinkSet name, of + dictionaries, each keyed by MarkerSet name and referencing a list of 3d points)", + "taskset_name"_a, "link_markerset_dict"_a) + .def( + "set_all_points", &MarkerSets::setAllMarkerPoints, + R"(Set the marker points for every MarkerSet of every LinkSet of every TaskSet present to the values in + the passed dict of dicts of dicts. The format should be dictionary, keyed by TaskSet name, of dictionaries, + keyed by link name, of dictionary, each keyed by MarkerSet name and value being a list + of that MarkerSet's marker points)", + "task_link_markerset_dict"_a) + .def( + "get_task_link_markerset_points", + &MarkerSets::getTaskLinkMarkerSetPoints, + R"(Get the marker points for the specified TaskSet's specified LinkSet's specified MarkerSet + as a list of 3d points)", + "taskset_name"_a, "linkset_name"_a, "markerset_name"_a) + .def( + "get_task_linkset_points", &MarkerSets::getTaskLinkSetPoints, + R"(Get the points in all the MarkerSets of the specified LinkSet, in the + specified TaskSet, as a dictionary, keyed by each MarkerSet's name + and referencing a list of 3d points.)", + "taskset_name"_a, "linkset_name"_a) + + .def( + "get_taskset_points", &MarkerSets::getTaskSetPoints, + R"(Get all the marker points in the specified TaskSet as a dict of dicts. + The format is a dictionary keyed by LinkSet name, of dictionaries, + each keyed by MarkerSet name and referencing a list of 3d points)", + "taskset_name"_a) + .def( + "get_all_marker_points", &MarkerSets::getAllMarkerPoints, + R"(Get the marker points for every MarkerSet of every link of every TaskSet present as a dict + of dicts of dicts. The format is a dictionary keyed by TaskSet name, of dictionaries, + keyed by LinkSet name, of dictionaries, each keyed by MarkerSet name referencing a list + of that MarkerSet's marker points)"); + // ==== ArticulatedObjectAttributes ==== py::class_( m, "ArticulatedObjectAttributes", - R"(A metadata template for articulated object configurations. Can be imported from + R"(A metadata template for articulated object configurations. Is imported from .ao_config.json files.)") .def(py::init(&ArticulatedObjectAttributes::create<>)) .def(py::init(&ArticulatedObjectAttributes::create)) @@ -333,12 +495,15 @@ void initAttributesBindings(py::module& m) { "rolling_friction_coefficient", &AbstractObjectAttributes::getRollingFrictionCoefficient, &AbstractObjectAttributes::setRollingFrictionCoefficient, - R"(Rolling friction coefficient for constructions built from this template. Damps angular velocity about axis orthogonal to the contact normal to prevent rounded shapes from rolling forever.)") + R"(Rolling friction coefficient for constructions built from this template. + Damps angular velocity about axis orthogonal to the contact normal to + prevent rounded shapes from rolling forever.)") .def_property( "spinning_friction_coefficient", &AbstractObjectAttributes::getSpinningFrictionCoefficient, &AbstractObjectAttributes::setSpinningFrictionCoefficient, - R"(Spinning friction coefficient for constructions built from this template. Damps angular velocity about the contact normal.)") + R"(Spinning friction coefficient for constructions built from this template. + Damps angular velocity about the contact normal.)") .def_property( "restitution_coefficient", &AbstractObjectAttributes::getRestitutionCoefficient, @@ -406,7 +571,7 @@ void initAttributesBindings(py::module& m) { m, "ObjectAttributes", R"(A metadata template for rigid objects pre-instantiation. Defines asset paths, physical properties, scale, semantic ids, shader type overrides, and user defined metadata. - ManagedRigidObjects are instantiated from these blueprints. Can be imported from + ManagedRigidObjects are instantiated from these blueprints. Is imported from .object_config.json files.)") .def(py::init(&ObjectAttributes::create<>)) .def(py::init(&ObjectAttributes::create)) @@ -458,7 +623,11 @@ void initAttributesBindings(py::module& m) { // ==== StageAttributes ==== py::class_( m, "StageAttributes", - R"(A metadata template for stages pre-instantiation. Defines asset paths, collision properties, gravity direction, shader type overrides, semantic asset information, and user defined metadata. Consumed to instantiate the static background of a scene (e.g. the building architecture). Can be imported from .stage_config.json files.)") + R"(A metadata template for stages pre-instantiation. Defines asset paths, + collision properties, gravity direction, shader type overrides, semantic + asset information, and user defined metadata. Consumed to instantiate the + static background of a scene (e.g. the building architecture). + Is imported from .stage_config.json files.)") .def(py::init(&StageAttributes::create<>)) .def(py::init(&StageAttributes::create)) .def_property( @@ -506,7 +675,8 @@ void initAttributesBindings(py::module& m) { py::class_( m, "LightInstanceAttributes", - R"(A metadata template for light configurations. Supports point and directional lights. Can be imported from .lighting_config.json files.)") + R"(A metadata template for light configurations. Supports point and directional lights. + Is imported from .lighting_config.json files.)") .def(py::init(&LightInstanceAttributes::create<>)) .def(py::init(&LightInstanceAttributes::create)) .def_property( @@ -545,7 +715,7 @@ void initAttributesBindings(py::module& m) { m, "PbrShaderAttributes", R"(A metadata template for PBR shader creation and control values and multipliers, such as enabling Image Based Lighting and controlling the mix of direct and indirect - lighting contributions. Can be imported from .pbr_config.json files.)") + lighting contributions. Is imported from .pbr_config.json files.)") .def(py::init(&PbrShaderAttributes::create<>)) .def(py::init(&PbrShaderAttributes::create)) .def_property( @@ -701,7 +871,7 @@ void initAttributesBindings(py::module& m) { m, "PhysicsManagerAttributes", R"(A metadata template for Simulation parameters (e.g. timestep, simulation backend, default gravity direction) and defaults. Consumed to instace a Simulator object. - Can be imported from .physics_config.json files.)") + Is imported from .physics_config.json files.)") .def(py::init(&PhysicsManagerAttributes::create<>)) .def(py::init(&PhysicsManagerAttributes::create)) .def_property_readonly( diff --git a/src/esp/bindings/ConfigBindings.cpp b/src/esp/bindings/ConfigBindings.cpp index b8b072757c..37f6b0c7d6 100644 --- a/src/esp/bindings/ConfigBindings.cpp +++ b/src/esp/bindings/ConfigBindings.cpp @@ -169,10 +169,10 @@ void initConfigBindings(py::module& m) { R"(Retrieves a string representation of the value referred to by the passed key.)") .def( - "get_keys_by_type", &Configuration::getStoredKeys, + "get_keys_by_type", &Configuration::getKeysByType, R"(Retrieves a list of all the keys of values of the specified types. Takes ConfigValType - enum value as argument.)", - "value_type"_a) + enum value as argument, and whether the keys should be sorted or not.)", + "value_type"_a, "sorted"_a = false) .def("get_keys_and_types", &Configuration::getValueTypes, R"(Returns a dictionary where the keys are the names of the values @@ -202,7 +202,9 @@ void initConfigBindings(py::module& m) { // subconfigs .def( "get_subconfig_keys", &Configuration::getSubconfigKeys, - R"(Retrieves a list of the keys of this configuration's subconfigurations)") + R"(Retrieves a list of the keys of this configuration's subconfigurations, + specifying whether the keys should be sorted or not)", + "sorted"_a = false) .def("get_subconfig", &Configuration::editSubconfig, py::return_value_policy::reference_internal, diff --git a/src/esp/bindings/PhysicsObjectBindings.cpp b/src/esp/bindings/PhysicsObjectBindings.cpp index 7f6a0b8f21..f597a3bd76 100644 --- a/src/esp/bindings/PhysicsObjectBindings.cpp +++ b/src/esp/bindings/PhysicsObjectBindings.cpp @@ -66,6 +66,20 @@ void declareBasePhysicsObjectWrapper(py::module& m, ("Get or set the translation vector of this " + objType + "'s root SceneNode. If modified, sim state will be updated.") .c_str()) + .def( + "transform_world_pts_to_local", + &PhysObjWrapper::transformWorldPointsToLocal, + R"(Given the list of passed points in world space, return those points + transformed to this object's local space. The link_id is for articulated + objects and is ignored for rigid objects and stages )", + "ws_points"_a, "link_id"_a) + .def( + "transform_local_pts_to_world", + &PhysObjWrapper::transformLocalPointsToWorld, + R"(Given the list of passed points in this object's local space, return + those points transformed to world space. The link_id is for articulated + objects and is ignored for rigid objects and stages )", + "ws_points"_a, "link_id"_a) .def_property( "rotation", &PhysObjWrapper::getRotation, &PhysObjWrapper::setRotation, @@ -484,7 +498,7 @@ void declareArticulatedObjectWrapper(py::module& m, .c_str(), "link_id"_a) .def("get_link_name", &ManagedArticulatedObject::getLinkName, - ("Get the name of the this " + objType + + ("Get the name of this " + objType + "'s link specified by the given link_id.") .c_str(), "link_id"_a) diff --git a/src/esp/core/Configuration.cpp b/src/esp/core/Configuration.cpp index ecc9199732..9fe9cd9a59 100644 --- a/src/esp/core/Configuration.cpp +++ b/src/esp/core/Configuration.cpp @@ -496,7 +496,7 @@ int Configuration::loadOneConfigFromJson(int numConfigSettings, for (size_t i = 0; i < jsonObj.Size(); ++i) { const std::string subKey = - Cr::Utility::formatString("{}_{:.02d}", key, i); + Cr::Utility::formatString("{}_{:.03d}", key, i); numConfigSettings += subGroupPtr->loadOneConfigFromJson( numConfigSettings, subKey, jsonObj[i]); } @@ -511,7 +511,7 @@ int Configuration::loadOneConfigFromJson(int numConfigSettings, for (size_t i = 0; i < jsonObj.Size(); ++i) { const std::string subKey = - Cr::Utility::formatString("{}_{:.02d}", key, i); + Cr::Utility::formatString("{}_{:.03d}", key, i); numConfigSettings += subGroupPtr->loadOneConfigFromJson( numConfigSettings, subKey, jsonObj[i]); } @@ -576,10 +576,10 @@ void Configuration::writeValuesToJson(io::JsonGenericValue& jsonObj, auto jsonVal = valIter->second.writeToJsonObject(allocator); jsonObj.AddMember(name, jsonVal, allocator); } else { - ESP_VERY_VERBOSE() - << "Unitialized ConfigValue in Configuration @ key [" + ESP_VERY_VERBOSE(Mn::Debug::Flag::NoSpace) + << "Unitialized ConfigValue in Configuration @ key `" << valIter->first - << "], so nothing will be written to JSON for this key."; + << "`, so nothing will be written to JSON for this key."; } } // iterate through all values } // Configuration::writeValuesToJson @@ -600,10 +600,10 @@ void Configuration::writeSubconfigsToJson(io::JsonGenericValue& jsonObj, cfgIter->second->writeToJsonObject(allocator); jsonObj.AddMember(name, subObj, allocator); } else { - ESP_VERY_VERBOSE() - << "Unitialized/empty Subconfig in Configuration @ key [" + ESP_VERY_VERBOSE(Mn::Debug::Flag::NoSpace) + << "Unitialized/empty Subconfig in Configuration @ key `" << cfgIter->first - << "], so nothing will be written to JSON for this key."; + << "`, so nothing will be written to JSON for this key."; } } // iterate through all configurations @@ -648,7 +648,7 @@ template <> std::shared_ptr Configuration::editSubconfig( const std::string& name) { // retrieve existing (or create new) subgroup, with passed name - return addSubgroup(name); + return addOrEditSubgroup(name).first->second; } template <> @@ -656,7 +656,50 @@ void Configuration::setSubconfigPtr( const std::string& name, std::shared_ptr& configPtr) { configMap_[name] = std::move(configPtr); -} // setSubconfigPtr +} + +int Configuration::rekeyAllValues() { + // Get all sorted key-value pairs of values + std::map sortedValMap(valueMap_.begin(), + valueMap_.end()); + // clear out existing value map - subconfigs are not touched. + valueMap_.clear(); + // place sorted values with newly constructed keys + int keyIter = 0; + for (auto it = sortedValMap.cbegin(); it != sortedValMap.cend(); ++it) { + std::string newKey = Cr::Utility::formatString("{:.03d}", keyIter++); + valueMap_[newKey] = it->second; + } + return keyIter; +} // Configuration::rekeyAllValues() + +int Configuration::rekeySubconfigValues(const std::string& subconfigKey) { + std::pair rekeySubconfigEntry = + addOrEditSubgroup(subconfigKey); + // check if subconfig existed already - result from addOrEditSubgroup would + // be false if add failed. + if (rekeySubconfigEntry.second) { + // Subconfig did not exist, was created by addOrEdit, so delete and return + // 0. + ESP_DEBUG(Mn::Debug::Flag::NoSpace) + << "No subconfig found with key `" << subconfigKey + << "` so rekeying aborted."; + configMap_.erase(rekeySubconfigEntry.first); + return 0; + } + // retrieve subconfig + auto rekeySubconfig = rekeySubconfigEntry.first->second; + if (rekeySubconfig->getNumValues() == 0) { + // Subconfig exists but has no values defined, so no rekeying possible + ESP_DEBUG(Mn::Debug::Flag::NoSpace) + << "Subconfig with key `" << subconfigKey + << "` has no values, so no rekeying accomplished."; + return 0; + } + // rekey subconfig and return the number of values affected + return rekeySubconfig->rekeyAllValues(); + +} // Configuration::rekeySubconfig int Configuration::findValueInternal(const Configuration& config, const std::string& key, diff --git a/src/esp/core/Configuration.h b/src/esp/core/Configuration.h index 3c0cb54b54..6690a76030 100644 --- a/src/esp/core/Configuration.h +++ b/src/esp/core/Configuration.h @@ -24,7 +24,7 @@ namespace core { namespace config { -constexpr int CONFIG_VAL_SIZE = 8; // 2 * 4 +constexpr int CONFIG_VAL_SIZE = 8; // 2 * 4(DWORDs) /** * @brief This enum lists every type of value that can be currently stored @@ -556,36 +556,49 @@ class Configuration { /** * @brief Retrieve list of keys present in this @ref Configuration's - * valueMap_. Subconfigs are not included. + * valueMap_. Subconfigs are not included. + * @return a vector of strings representing the subconfig keys for this + * configuration. */ - std::vector getKeys() const { + std::vector getKeys(bool sorted = false) const { std::vector keys; keys.reserve(valueMap_.size()); for (const auto& entry : valueMap_) { keys.push_back(entry.first); } + if (sorted) { + std::sort(keys.begin(), keys.end()); + } return keys; } /** * @brief This function returns this @ref Configuration's subconfig keys. + * @param sorted whether the keys should be sorted or not. + * @return a vector of strings representing the subconfig keys for this + * configuration. */ - std::vector getSubconfigKeys() const { + std::vector getSubconfigKeys(bool sorted = false) const { std::vector keys; keys.reserve(configMap_.size()); for (const auto& entry : configMap_) { keys.push_back(entry.first); } + if (sorted) { + std::sort(keys.begin(), keys.end()); + } return keys; } /** * @brief Retrieve a list of all the keys in this @ref Configuration pointing - to values of passed type @p storedType. + * to values of passed type @p storedType. * @param storedType The desired type of value whose key should be returned. + * @param sorted whether the keys should be sorted or not. * @return vector of string keys pointing to values of desired @p storedType */ - std::vector getStoredKeys(ConfigValType storedType) const { + std::vector getKeysByType(ConfigValType storedType, + bool sorted = false) const { std::vector keys; // reserve space for all keys keys.reserve(valueMap_.size()); @@ -596,6 +609,12 @@ class Configuration { keys.push_back(entry.first); } } + if (valueMap_.size() > count) { + keys.shrink_to_fit(); + } + if (sorted) { + std::sort(keys.begin(), keys.end()); + } return keys; } @@ -675,6 +694,8 @@ class Configuration { return {}; } + // ************************* Queries ************************** + /** * @brief Return number of value and subconfig entries in this * Configuration. This only counts each subconfiguration entry as a single @@ -858,7 +879,7 @@ class Configuration { "Configuration : Desired subconfig must be derived from " "core::config::Configuration"); // retrieve existing (or create new) subgroup, with passed name - return std::static_pointer_cast(addSubgroup(name)); + return std::static_pointer_cast(addOrEditSubgroup(name).first->second); } /** @@ -946,7 +967,8 @@ class Configuration { for (const auto& subConfig : src->configMap_) { const auto name = subConfig.first; // make if DNE and merge src subconfig - addSubgroup(name)->overwriteWithConfig(subConfig.second); + addOrEditSubgroup(name).first->second->overwriteWithConfig( + subConfig.second); } } @@ -1033,6 +1055,34 @@ class Configuration { */ std::string getAllValsAsString(const std::string& newLineStr = "\n") const; + /** + * @brief Find the subconfiguration with the given handle and rekey it such + * that all the value entries it contains are keyed by sequential numeric + * strings. These keys will preserve the order of the original keys. + * + * NOTE : all values regardless of type in this subconfig will be rekeyed. + * This function is primarily intended to be used on subconfigs holding a + * collection of the same type of date, to facilitate accessing this data as a + * sorted list. + * + * @param subconfigKey The key of this Configuration's subconfig whose values' + * keys we wish to have rekeyed. + * @return the number of values whose keys have been changed. + */ + int rekeySubconfigValues(const std::string& subconfigKey); + + /** + * @brief Rekey all the value entries in this Configuration such that all + * their keys are sequential numeric strings that presever the order of their + * original strings. + */ + int rekeyAllValues(); + + /** + * @brief Clear all key-value pairs from this Configuration's valueMap + */ + void _clearAllValues() { valueMap_.clear(); } + protected: /** * @brief Process passed json object into this Configuration, using passed @@ -1083,12 +1133,17 @@ class Configuration { } /** - * @brief if no subgroup with given name this will make one, otherwise does - * nothing. - * @param name Desired name of new subgroup. - * @return whether a group was made or not + * @brief Retrieves named subgroup; if no subgroup with given name exists this + * will make one. + * @param name Name of desired existing or new subgroup. + * @return The resultant pair after attempting to emplace a Configuration at + * the requested location given by @p name. Consists of an iterator to the + * Configuration, and a boolean value that denotes whether this is a new + * Configuration or it existed already. */ - std::shared_ptr addSubgroup(const std::string& name) { + + std::pair addOrEditSubgroup( + const std::string& name) { // Attempt to insert an empty pointer auto result = configMap_.emplace(name, std::shared_ptr{}); // If name not already present (insert succeeded) then add new @@ -1096,7 +1151,7 @@ class Configuration { if (result.second) { result.first->second = std::make_shared(); } - return result.first->second; + return result; } /** diff --git a/src/esp/metadata/CMakeLists.txt b/src/esp/metadata/CMakeLists.txt index b79813a6ef..37e09391ef 100644 --- a/src/esp/metadata/CMakeLists.txt +++ b/src/esp/metadata/CMakeLists.txt @@ -14,6 +14,7 @@ set( attributes/ArticulatedObjectAttributes.cpp attributes/LightLayoutAttributes.h attributes/LightLayoutAttributes.cpp + attributes/MarkerSets.h attributes/ObjectAttributes.h attributes/ObjectAttributes.cpp attributes/PhysicsManagerAttributes.h diff --git a/src/esp/metadata/URDFParser.cpp b/src/esp/metadata/URDFParser.cpp index d40f8f57ba..05919d1d02 100644 --- a/src/esp/metadata/URDFParser.cpp +++ b/src/esp/metadata/URDFParser.cpp @@ -839,7 +839,7 @@ bool Parser::validateMeshFile(std::string& meshFilename) { bool Parser::initTreeAndRoot(const std::shared_ptr& model) const { // every link has children links and joints, but no parents, so we create a // local convenience data structure for keeping child->parent relations - std::map parentLinkTree; + model->m_parentLinkTree.clear(); // loop through all joints, for every link, assign children links and children // joints @@ -875,7 +875,7 @@ bool Parser::initTreeAndRoot(const std::shared_ptr& model) const { childLink->m_parentJoint = joint; parentLink->m_childJoints.push_back(joint); parentLink->m_childLinks.push_back(childLink); - parentLinkTree[childLink->m_name] = parentLink->m_name; + model->m_parentLinkTree[childLink->m_name] = parentLink->m_name; } // search for children that have no parent, those are 'root' diff --git a/src/esp/metadata/URDFParser.h b/src/esp/metadata/URDFParser.h index 48e1281b22..c22284f8f2 100644 --- a/src/esp/metadata/URDFParser.h +++ b/src/esp/metadata/URDFParser.h @@ -285,6 +285,9 @@ class Model { //! list of root links (usually 1) std::vector> m_rootLinks; + //! Keyed by child link name, provides reference to parent link + std::map m_parentLinkTree{}; + //! if true, force this model to produce a fixed base instance (e.g. the root //! is not dynamic) bool m_overrideFixedBase{false}; diff --git a/src/esp/metadata/attributes/AbstractObjectAttributes.h b/src/esp/metadata/attributes/AbstractObjectAttributes.h index 323855df28..6ab7f4f9a1 100644 --- a/src/esp/metadata/attributes/AbstractObjectAttributes.h +++ b/src/esp/metadata/attributes/AbstractObjectAttributes.h @@ -6,6 +6,7 @@ #define ESP_METADATA_ATTRIBUTES_ABSTRACTOBJECTATTRIBUTES_H_ #include "AttributesBase.h" +#include "MarkerSets.h" namespace esp { namespace metadata { @@ -267,8 +268,8 @@ class AbstractObjectAttributes : public AbstractAttributes { * @brief Gets a smart pointer reference to a copy of the marker_sets * configuration data from config file. */ - std::shared_ptr getMarkerSetsConfiguration() const { - return getSubconfigCopy("marker_sets"); + std::shared_ptr getMarkerSetsConfiguration() const { + return getSubconfigCopy("marker_sets"); } /** @@ -276,8 +277,17 @@ class AbstractObjectAttributes : public AbstractAttributes { * configuration data from config file. This method is for editing the * configuration. */ - std::shared_ptr editMarkerSetsConfiguration() { - return editSubconfig("marker_sets"); + std::shared_ptr editMarkerSetsConfiguration() { + return editSubconfig("marker_sets"); + } + + /** + * @brief Rekey all the markers in the marker_sets subconfiguration such that + * each point is keyed by a sequential numeric string that preserves the + * natural ordering of the key strings. + */ + int rekeyAllMarkerSets() { + return editSubconfig("marker_sets")->rekeyAllMarkers(); } protected: diff --git a/src/esp/metadata/attributes/ArticulatedObjectAttributes.h b/src/esp/metadata/attributes/ArticulatedObjectAttributes.h index 5dfad75756..8bdde9210d 100644 --- a/src/esp/metadata/attributes/ArticulatedObjectAttributes.h +++ b/src/esp/metadata/attributes/ArticulatedObjectAttributes.h @@ -6,6 +6,7 @@ #define ESP_METADATA_ATTRIBUTES_ARTICULATEDOBJECTATTRIBUTES_H_ #include "AttributesBase.h" +#include "MarkerSets.h" namespace esp { namespace metadata { @@ -254,8 +255,8 @@ class ArticulatedObjectAttributes : public AbstractAttributes { * @brief Gets a smart pointer reference to a copy of the marker_sets * configuration data from config file. */ - std::shared_ptr getMarkerSetsConfiguration() const { - return getSubconfigCopy("marker_sets"); + std::shared_ptr getMarkerSetsConfiguration() const { + return getSubconfigCopy("marker_sets"); } /** @@ -263,8 +264,16 @@ class ArticulatedObjectAttributes : public AbstractAttributes { * configuration data from config file. This method is for editing the * configuration. */ - std::shared_ptr editMarkerSetsConfiguration() { - return editSubconfig("marker_sets"); + std::shared_ptr editMarkerSetsConfiguration() { + return editSubconfig("marker_sets"); + } + /** + * @brief Rekey all the markers in the marker_sets subconfiguration such that + * each point is keyed by a sequential numeric string that preserves the + * natural ordering of the key strings. + */ + int rekeyAllMarkerSets() { + return editSubconfig("marker_sets")->rekeyAllMarkers(); } protected: diff --git a/src/esp/metadata/attributes/AttributesBase.cpp b/src/esp/metadata/attributes/AttributesBase.cpp index 656d80fc12..8e4852f899 100644 --- a/src/esp/metadata/attributes/AttributesBase.cpp +++ b/src/esp/metadata/attributes/AttributesBase.cpp @@ -31,7 +31,7 @@ AbstractAttributes::AbstractAttributes(const std::string& attributesClassKey, const std::string& handle) : Configuration() { // set up an existing subgroup for user_defined attributes - addSubgroup("user_defined"); + addOrEditSubgroup("user_defined"); AbstractAttributes::setClassKey(attributesClassKey); AbstractAttributes::setHandle(handle); // set initial vals, will be overwritten when registered diff --git a/src/esp/metadata/attributes/MarkerSets.h b/src/esp/metadata/attributes/MarkerSets.h new file mode 100644 index 0000000000..6b851fb299 --- /dev/null +++ b/src/esp/metadata/attributes/MarkerSets.h @@ -0,0 +1,607 @@ +// Copyright (c) Meta Platforms, Inc. and its affiliates. +// This source code is licensed under the MIT license found in the +// LICENSE file in the root directory of this source tree. + +#ifndef ESP_METADATA_ATTRIBUTES_MARKERSETS_H_ +#define ESP_METADATA_ATTRIBUTES_MARKERSETS_H_ + +#include "esp/core/Configuration.h" +namespace esp { +namespace metadata { +namespace attributes { +/** @file + * @brief Class @ref esp::metadata::attributes::Markersets, + * Class @ref esp::metadata::attributes::TaskSet, + * Class @ref esp::metadata::attributes::LinkSet, + * Class @ref esp::metadata::attributes::MarkerSet + */ + +/** + * @brief This class provides an alias for a single Configuration holding 1 or + * more marker points within a particular LinkSet. + */ +class MarkerSet : public esp::core::config::Configuration { + public: + MarkerSet() : Configuration() {} + /** + * @brief Returns the number of existing marker points in this MarkerSet. + */ + int getNumPoints() const { + return getSubconfigView("markers")->getNumValues(); + } + + /** + * @brief Returns a list of all the marker points in this MarkerSet + */ + std::vector getAllPoints() const { + const auto markersPtr = getSubconfigView("markers"); + // Get all vector3 keys from subconfig in sorted vector + std::vector markerTags = markersPtr->getKeysByType( + esp::core::config::ConfigValType::MagnumVec3, true); + std::vector res; + res.reserve(markerTags.size()); + for (const auto& tag : markerTags) { + res.emplace_back(markersPtr->get(tag)); + } + return res; + } + + /** + * @brief Set the list of all the marker points for this MarkerSet + */ + void setAllPoints(const std::vector& markers) { + auto markersPtr = editSubconfig("markers"); + // Remove all existing marker points + markersPtr->_clearAllValues(); + for (std::size_t i = 0; i < markers.size(); ++i) { + const std::string& key = Cr::Utility::formatString("{:.03d}", i); + markersPtr->set(key, markers[i]); + } + } + + /** + * @brief Rekeys all marker points to have vector IDXs as string keys, + * retaining their original keys' natural ordering. + * @return returns how many marker points have been processed with new keys. + */ + int rekeyAllMarkers() { return rekeySubconfigValues("markers"); } + + ESP_SMART_POINTERS(MarkerSet) +}; // class MarkerSet + +/** + * @brief This class provides an alias for the nested Configuration tree used + * for a single link's 1 or more NarkerSets that should be attached to the + * named link. + */ +class LinkSet : public esp::core::config::Configuration { + public: + LinkSet() : Configuration() {} + + /** + * @brief Returns the number of existing MarkerSets in this LinkSet. + */ + int getNumMarkerSets() const { return getNumSubconfigs(); } + + /** + * @brief whether the given @p markerSetName exists as a MarkerSet in + * this LinkSet. + * + * @param markerSetName The desired marker set's name. + * @return whether the name is found as a MarkerSet subConfiguration. + */ + bool hasMarkerSet(const std::string& markerSetName) const { + return hasSubconfig(markerSetName); + } + + /** + * @brief Retrieve a listing of all the MarkerSet handles in this + * LinkSet. + */ + std::vector getAllMarkerSetNames() const { + return getSubconfigKeys(true); + } + + /** + * @brief Retrivess a copy of the named MarkerSet, if it exists, and + * nullptr if it does not. + */ + MarkerSet::ptr getMarkerSetCopy(const std::string& markerSetName) { + return getSubconfigCopy(markerSetName); + } + + /** + * @brief Retrieve a view of the naamed MarkerSet, if it exists, and + * nullptr if it does not. + */ + MarkerSet::cptr getMarkerSetView(const std::string& markerSetName) const { + return std::static_pointer_cast( + getSubconfigView(markerSetName)); + } + + /** + * @brief Retrieves a reference to a (potentially newly created) + * MarkerSet with the given @p markerSetName , which can be modified + * and the modifications will be retained. + * + * @param markerSetName The desired MarkerSet name. + * @return a reference to the MarkerSet. + */ + MarkerSet::ptr editMarkerSet(const std::string& markerSetName) { + return editSubconfig(markerSetName); + } + + /** + * @brief Removes named MarkerSet. Does nothing if DNE. + */ + void removeLinkSet(const std::string& markerSetName) { + removeSubconfig(markerSetName); + } + + /** + * @brief Set the specified MarkerSet's points to the given values. + * @param markerSetName the name of the MarkerSet + * @param markerList the list of the specified MarkerSet's points. + */ + void setMarkerSetPoints(const std::string& markerSetName, + const std::vector& markerList) { + editMarkerSet(markerSetName)->setAllPoints(markerList); + } + + /** + * @brief Set the marker points of all the MarkerSets specified by name in the + * passed map. + * @param markerMap a map holding all the MarkerSet points within this + * LinkSet, with MarkerSet name as the key, referncing a vector of 3d points, + */ + void setAllMarkerPoints( + const std::unordered_map>& + markerMap) { + for (const auto& markers : markerMap) { + setMarkerSetPoints(markers.first, markers.second); + } + } + + /** + * @brief Retrieve all the marker points for the specified MarkerSet in this + * LinkSet + * @return a vector of 3d points + */ + std::vector getMarkerSetPoints(const std::string& key) const { + return getMarkerSetView(key)->getAllPoints(); + } + + /** + * @brief Retrieve all the marker points across all MarkerSets for this link, + * as a map. + * @return a map holding all the MarkerSet points within this LinkSet, with + * MarkerSet name as the key, referncing a vector of 3d points + */ + std::unordered_map> getAllMarkerPoints() + const { + std::unordered_map> resMap; + const auto& subsetKeys = getSubconfigKeys(); + for (const auto& key : subsetKeys) { + resMap[key] = getMarkerSetPoints(key); + } + return resMap; + } // getAllMarkerPoints + + /** + * @brief Rekeys all marker collections to have vector IDXs as string keys + * @return returns how many markers have been processed with new keys in this + * LinkSet's MarkerSets. + */ + int rekeyAllMarkers() { + int res = 0; + const auto& subsetKeys = getSubconfigKeys(); + for (const auto& key : subsetKeys) { + res += editMarkerSet(key)->rekeyAllMarkers(); + } + return res; + } + + ESP_SMART_POINTERS(LinkSet) +}; // class LinkSet + +/** + * @brief This class provides an alias for the nested Configuration tree used + * for a single TaskSet, holding 1 or more LinkSets + */ +class TaskSet : public esp::core::config::Configuration { + public: + TaskSet() : Configuration() {} + + /** + * @brief Returns the number of existing LinkSets in this collection. + */ + int getNumLinkSets() const { return getNumSubconfigs(); } + + /** + * @brief Whether the given @p linkSetName exists as a LinkSet in this + * collection. + * + * @param linkSetName The desired LinkSet' name. + * @return whether the name is found as a LinkSet subConfiguration. + */ + bool hasLinkSet(const std::string& linkSetName) const { + return hasSubconfig(linkSetName); + } + + /** + * @brief Retrieve a listing of all the LinkSet handles in this + * collection. + */ + std::vector getAllLinkSetNames() const { + return getSubconfigKeys(true); + } + + /** + * @brief Retrivess a copy of the named LinkSet, if it exists, and + * nullptr if it does not. + */ + LinkSet::ptr getLinkSetCopy(const std::string& linkSetName) { + return getSubconfigCopy(linkSetName); + } + + /** + * @brief Retrieve a view of the naamed LinkSet, if it exists, and + * nullptr if it does not. + */ + LinkSet::cptr getLinkSetView(const std::string& linkSetName) const { + return std::static_pointer_cast( + getSubconfigView(linkSetName)); + } + + /** + * @brief Retrieves a reference to a (potentially newly created) + * LinkSet with the given @p linkSetName , which can be modified and + * the modifications will be retained. + * + * @param linkSetName The desired LinkSet's name. + * @return a reference to the LinkSet. + */ + LinkSet::ptr editLinkSet(const std::string& linkSetName) { + return editSubconfig(linkSetName); + } + + /** + * @brief Removes named LinkSet. Does nothing if DNE. + */ + void removeLinkSet(const std::string& linkSetName) { + removeSubconfig(linkSetName); + } + + /** + * @brief Set a specified LinkSet's specified MarkerSet's points to the given + * list of points. + * @param linkSetName the name of the LinkSet + * @param markerSetName the name of the MarkerSet within @p linkSetName + * @param markerList the list of the specified MarkerSet's points. + */ + void setLinkMarkerSetPoints(const std::string& linkSetName, + const std::string& markerSetName, + const std::vector& markerList) { + editLinkSet(linkSetName)->setMarkerSetPoints(markerSetName, markerList); + } + + /** + * @brief Sets all the MarkerSet points in the specified LinkSet to the given + * marker values specified in the map. + * @param linkSetName the name of the LinkSet within @p taskSetName + * @param markerMap a map holding all the MarkerSet points within the + * specified LinkSet, with MarkerSet name as the key, referncing a vector of + * 3d points, + */ + void setLinkSetPoints( + const std::string& linkSetName, + const std::unordered_map>& + markers) { + editLinkSet(linkSetName)->setAllMarkerPoints(markers); + } + + /** + * @brief Sets all the LinkSet's MarkerSets' points in this TaskSet + * to the given marker values specified in the map. + * @param markerMap an unordered map keyed by LinkSet name of unordered maps, + * each keyed by MarkerSet name of Markers as a vector of 3d points. + */ + void setAllMarkerPoints( + const std::unordered_map< + std::string, + std::unordered_map>>& + markerMap) { + for (const auto& markers : markerMap) { + setLinkSetPoints(markers.first, markers.second); + } + } + + /** + * @brief Retrieve the specified LinkSet's MarkerSet as a vector of 3d + * points. + * @param linkSetName the name of the LinkSet + * @param markerSetName the name of the MarkerSet within @p linkSetName + * @return a vector of 3d points + */ + std::vector getLinkMarkerSetPoints( + const std::string& linkSetName, + const std::string& markerSetName) const { + return getLinkSetView(linkSetName)->getMarkerSetPoints(markerSetName); + } + + /** + * @brief Retrieve all the MarkerSet points for the specified LinkSet within + * this TaskSet. + * @param linkSetName the name of the LinkSet + * @return a map holding all the MarkerSet points within the + * specified LinkSet, with MarkerSet name as the key, referncing a vector of + * 3d points. + */ + std::unordered_map> getLinkSetPoints( + const std::string& linkSetName) const { + return getLinkSetView(linkSetName)->getAllMarkerPoints(); + } + + /** + * @brief this retrieves all the marker points across all the LinkSets in this + * TaskSet. + * @return an unordered map keyed by LinkSet name of unordered maps, each + * keyed by MarkerSet name of Markers as a vector of 3d points. + */ + std::unordered_map>> + getAllMarkerPoints() const { + std::unordered_map< + std::string, std::unordered_map>> + resMap; + const auto& subsetKeys = getSubconfigKeys(); + for (const auto& linkSetName : subsetKeys) { + resMap[linkSetName] = getLinkSetPoints(linkSetName); + } + return resMap; + } // getAllMarkerPoints + + /** + * @brief Rekeys all marker collections to have vector IDXs as string keys + * @return returns how many markers have been processed with new keys in this + * TaskSet. + */ + int rekeyAllMarkers() { + int res = 0; + const auto& subsetKeys = getSubconfigKeys(); + for (const auto& key : subsetKeys) { + res += editLinkSet(key)->rekeyAllMarkers(); + } + return res; + } + + ESP_SMART_POINTERS(TaskSet) +}; // class TaskSet + +/** + * @brief This class provides an alias for the nested Configuration tree used + * to hold multiple TaskSets. + */ +class MarkerSets : public esp::core::config::Configuration { + public: + MarkerSets() : Configuration() {} + + /** + * @brief Returns the number of existing TaskSets in this collection. + */ + int getNumTaskSets() const { return getNumSubconfigs(); } + + /** + * @brief whether the given @p taskSetName exists as a TaskSet in this + * collection. + * + * @param taskSetName The desired TaskSet's name. + * @return whether the name is found as a TaskSet subConfiguration. + */ + bool hasTaskSet(const std::string& taskSetName) const { + return hasSubconfig(taskSetName); + } + + /** + * @brief Retrieve a listing of all the TaskSet handles in this collection. + */ + std::vector getAllTaskSetNames() const { + return getSubconfigKeys(true); + } + + /** + * @brief Retrivess a copy of the named TaskSet, if it exists, and nullptr + * if it does not. + */ + TaskSet::ptr getTaskSetCopy(const std::string& taskSetName) { + return getSubconfigCopy(taskSetName); + } + + /** + * @brief Retrieve a view of the naamed TaskSet, if it exists, and + * nullptr if it does not. + */ + TaskSet::cptr getTaskSetView(const std::string& taskSetName) const { + return std::static_pointer_cast( + getSubconfigView(taskSetName)); + } + + /** + * @brief Retrieves a reference to a (potentially newly created) TaskSet + * with the given @p taskSetName , which can be modified and the + * modifications will be retained. + * + * @param taskSetName The desired TaskSet's name. + * @return a reference to the TaskSet. + */ + TaskSet::ptr editTaskSet(const std::string& taskSetName) { + return editSubconfig(taskSetName); + } + + /** + * @brief Removes named TaskSet. Does nothing if DNE. + */ + void removeTaskSet(const std::string& taskSetName) { + removeSubconfig(taskSetName); + } + + /** + * @brief Set the specified TaskSet's specified LinkSet's specified + * MarkerSet's marker points. + * @param taskSetName the name of the TaskSet + * @param linkSetName the name of the LinkSet within @p taskSetName + * @param markerSetName the name of the MarkerSet within @p linkSetName + * @param markerList the list of the specified MarkerSet' points. + */ + void setTaskLinkMarkerSetPoints(const std::string& taskSetName, + const std::string& linkSetName, + const std::string& markerSetName, + const std::vector& markerList) { + editTaskSet(taskSetName) + ->setLinkMarkerSetPoints(linkSetName, markerSetName, markerList); + } + + /** + * @brief Sets all the MarkerSet points in the specified TaskSet's specified + * LinkSet to the given marker values specified in the map. + * @param taskSetName the name of the TaskSet + * @param linkSetName the name of the LinkSet within @p taskSetName + * @param markerMap a map holding all the MarkerSet points within the + * specified LinkSet, with MarkerSet name as the key, referncing a vector of + * 3d points, + */ + void setTaskLinkSetPoints( + const std::string& taskSetName, + const std::string& linkSetName, + const std::unordered_map>& + markerMap) { + editTaskSet(taskSetName)->setLinkSetPoints(linkSetName, markerMap); + } + + /** + * @brief Sets all the LinkSet's MarkerSets' points in the specified TaskSet + * to the given marker values specified in the map. + * @param taskSetName the name of the TaskSet + * @param markerMap an unordered map keyed by LinkSet name of unordered maps, + * each keyed by MarkerSet name of Markers as a vector of 3d points. + */ + void setTaskSetPoints( + const std::string& taskSetName, + const std::unordered_map< + std::string, + std::unordered_map>>& + markerMap) { + editTaskSet(taskSetName)->setAllMarkerPoints(markerMap); + } + + /** + * @brief Set all the marker points across every TaskSet using the values in + * the passed map. + * @param markerMap an unordered map keyed by TaskSet name, of unordered maps, + * each keyed by LinkSet name, of unordered maps, each keyed by MarkerSet name + * of Markers as a vector of 3d points. + */ + void setAllMarkerPoints( + const std::unordered_map< + std::string, + std::unordered_map< + std::string, + std::unordered_map>>>& + markerMap) { + for (const auto& markers : markerMap) { + setTaskSetPoints(markers.first, markers.second); + } + } + + /** + * @brief Retrieve a single TaskSet's LinkSet's MarkerSet as a vector of 3d + * points. + * @param taskSetName the name of the TaskSet + * @param linkSetName the name of the LinkSet within @p taskSetName + * @param markerSetName the name of the MarkerSet within @p linkSetName + * @return a vector of 3d points + */ + std::vector getTaskLinkMarkerSetPoints( + const std::string& taskSetName, + const std::string& linkSetName, + const std::string& markerSetName) const { + return getTaskSetView(taskSetName) + ->getLinkMarkerSetPoints(linkSetName, markerSetName); + } + + /** + * @brief Retrieve all of the MarkerSets for a particular LinkSet within the + * specified TaskSet, as an unordered map keyed by MarkerSet name of + * Markers as a vector of 3d points. + * @param taskSetName the name of the TaskSet + * @param linkSetName the name of the LinkSet within @p taskSetName + * @return a map holding all the MarkerSet points within the specified + * LinkSet, with MarkerSet name as the key, referncing a vector of 3d points, + */ + + std::unordered_map> + getTaskLinkSetPoints(const std::string& taskSetName, + const std::string& linkSetName) const { + return getTaskSetView(taskSetName)->getLinkSetPoints(linkSetName); + } + + /** + * @brief Retrieve all the marker points for the named TaskSet, as an + * unordered map keyed by LinkSet name of unordered maps, each keyed by + * MarkerSet name of Markers as a vector of 3d points. + * @param taskSetName the name of the TaskSet + * @return an unordered map keyed by LinkSet name of unordered maps, each + * keyed by MarkerSet name of Markers as a vector of 3d points. + */ + std::unordered_map>> + getTaskSetPoints(const std::string& taskSetName) const { + return getTaskSetView(taskSetName)->getAllMarkerPoints(); + } + + /** + * @brief Retrieve all the MarkerSet points across all the TaskSets. + * @return an unordered map keyed by TaskSet name, of unordered maps, each + * keyed by LinkSet name, of unordered maps, each keyed by MarkerSet name of + * Markers as a vector of 3d points. + */ + std::unordered_map< + std::string, + std::unordered_map< + std::string, + std::unordered_map>>> + getAllMarkerPoints() const { + std::unordered_map< + std::string, + std::unordered_map< + std::string, + std::unordered_map>>> + resMap; + const auto& subsetKeys = getSubconfigKeys(); + for (const auto& taskSetName : subsetKeys) { + resMap[taskSetName] = getTaskSetPoints(taskSetName); + } + return resMap; + } // getAllMarkerPoints + + /** + * @brief Rekeys all marker collections to have vector IDXs as string keys + * @return returns how many marker points have been processed with new keys. + */ + int rekeyAllMarkers() { + int res = 0; + const auto& subsetKeys = getSubconfigKeys(); + for (const auto& key : subsetKeys) { + res += editTaskSet(key)->rekeyAllMarkers(); + } + return res; + } + + ESP_SMART_POINTERS(MarkerSets) +}; // class MarkerSets + +} // namespace attributes +} // namespace metadata +} // namespace esp + +#endif // ESP_METADATA_ATTRIBUTES_MARKERSETS_H_ diff --git a/src/esp/metadata/managers/AOAttributesManager.cpp b/src/esp/metadata/managers/AOAttributesManager.cpp index d0848aa22e..54e65b82ac 100644 --- a/src/esp/metadata/managers/AOAttributesManager.cpp +++ b/src/esp/metadata/managers/AOAttributesManager.cpp @@ -101,7 +101,7 @@ void AOAttributesManager::setValsFromJSONDoc( [aoAttr](const std::string& val) { aoAttr->setLinkOrder(val); }); // check for the existing of markersets - this->parseSubconfigJsonVals("marker_sets", aoAttr, jsonConfig); + this->parseMarkerSets(aoAttr, jsonConfig); // check for user defined attributes this->parseUserDefinedJsonVals(aoAttr, jsonConfig); diff --git a/src/esp/metadata/managers/AOAttributesManager.h b/src/esp/metadata/managers/AOAttributesManager.h index 53a6bb3a2e..0164566df8 100644 --- a/src/esp/metadata/managers/AOAttributesManager.h +++ b/src/esp/metadata/managers/AOAttributesManager.h @@ -74,6 +74,28 @@ class AOAttributesManager std::map getArticulatedObjectModelFilenames() const; protected: + /** + * @brief Parse Marker_sets object in json, if present. + * @param attribs (out) an existing attributes to be modified. + * @param jsonConfig json document to parse + * @return true if tag is found, of appropriate configuration, and holds + * actual values. + */ + bool parseMarkerSets( + const attributes::ArticulatedObjectAttributes::ptr& attribs, + const io::JsonGenericValue& jsonConfig) const { + // check for the existing of markersets + bool hasMarkersets = + this->parseSubconfigJsonVals("marker_sets", attribs, jsonConfig); + if (hasMarkersets) { + // Cast "marker_sets" Configuration to MarkerSets object and rekey all + // markers to make keys consistent while preserving the natural order of + // their original keys. + attribs->rekeyAllMarkerSets(); + } + return hasMarkersets; + } + /** * @brief Used Internally. Create and configure newly-created attributes with * any default values, before any specific values are set. diff --git a/src/esp/metadata/managers/AbstractObjectAttributesManager.h b/src/esp/metadata/managers/AbstractObjectAttributesManager.h index dc480c5f38..d4ba4004d7 100644 --- a/src/esp/metadata/managers/AbstractObjectAttributesManager.h +++ b/src/esp/metadata/managers/AbstractObjectAttributesManager.h @@ -98,6 +98,26 @@ class AbstractObjectAttributesManager : public AttributesManager { bool registerTemplate = true) = 0; protected: + /** + * @brief Parse Marker_sets object in json, if present. + * @param attribs (out) an existing attributes to be modified. + * @param jsonConfig json document to parse + * @return true if tag is found, of appropriate configuration, and holds + * actual values. + */ + bool parseMarkerSets(const AbsObjAttrPtr& attribs, + const io::JsonGenericValue& jsonConfig) const { + // check for the existing of markersets + bool hasMarkersets = + this->parseSubconfigJsonVals("marker_sets", attribs, jsonConfig); + if (hasMarkersets) { + // Cast "marker_sets" Configuration to MarkerSets object and rekey all + // markers to make keys consistent while preserving the natural order of + // their original keys. + attribs->rekeyAllMarkerSets(); + } + return hasMarkersets; + } //======== Common JSON import functions ======== /** @@ -347,6 +367,9 @@ auto AbstractObjectAttributesManager:: attributes::ShaderTypeNamesMap, [attributes](const std::string& val) { attributes->setShaderType(val); }); + // Markersets for stages or objects + this->parseMarkerSets(attributes, jsonDoc); + return attributes; } // AbstractObjectAttributesManager::setAbstractObjectAttributesFromJson diff --git a/src/esp/metadata/managers/ObjectAttributesManager.cpp b/src/esp/metadata/managers/ObjectAttributesManager.cpp index 813b33e26d..5689563e40 100644 --- a/src/esp/metadata/managers/ObjectAttributesManager.cpp +++ b/src/esp/metadata/managers/ObjectAttributesManager.cpp @@ -127,9 +127,6 @@ void ObjectAttributesManager::setValsFromJSONDoc( // if com is set from json, don't compute from shape, and vice versa objAttributes->setComputeCOMFromShape(!comIsSet); - // check for the existing of markersets - this->parseSubconfigJsonVals("marker_sets", objAttributes, jsonConfig); - // check for user defined attributes this->parseUserDefinedJsonVals(objAttributes, jsonConfig); diff --git a/src/esp/metadata/managers/StageAttributesManager.cpp b/src/esp/metadata/managers/StageAttributesManager.cpp index a8924e0b20..b89ee61f33 100644 --- a/src/esp/metadata/managers/StageAttributesManager.cpp +++ b/src/esp/metadata/managers/StageAttributesManager.cpp @@ -444,8 +444,6 @@ void StageAttributesManager::setValsFromJSONDoc( } stageAttributes->setSemanticDescriptorFilename(semanticSceneDescriptor); } - // check for the existing of markersets - this->parseSubconfigJsonVals("marker_sets", stageAttributes, jsonConfig); // check for user defined attributes this->parseUserDefinedJsonVals(stageAttributes, jsonConfig); diff --git a/src/esp/physics/ArticulatedObject.h b/src/esp/physics/ArticulatedObject.h index 129f679941..e869aa1389 100644 --- a/src/esp/physics/ArticulatedObject.h +++ b/src/esp/physics/ArticulatedObject.h @@ -174,6 +174,12 @@ class ArticulatedLink : public RigidBase { return true; } + void initializeArticulatedLink(const std::string& _linkName, + const Mn::Vector3& _scale) { + linkName = _linkName; + setScale(_scale); + } + /** * @brief Finalize the creation of the link. * @return whether successful finalization. @@ -446,6 +452,40 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase { return objectIdToLinkId_; } + /** + * @brief Given the list of passed points in this object's local space, return + * those points transformed to world space. + * @param points vector of points in object local space + * @param linkID Unused for rigids. + * @return vector of points transformed into world space + */ + std::vector transformLocalPointsToWorld( + const std::vector& points, + int linkID) const override { + auto linkIter = links_.find(linkID); + if (linkIter != links_.end()) { + return linkIter->second->transformLocalPointsToWorld(points, linkID); + } + return points; + } + + /** + * @brief Given the list of passed points in world space, return + * those points transformed to this object's local space. + * @param points vector of points in world space + * @param linkID Unused for rigids. + * @return vector of points transformed to be in local space + */ + std::vector transformWorldPointsToLocal( + const std::vector& points, + int linkID) const override { + auto linkIter = links_.find(linkID); + if (linkIter != links_.end()) { + return linkIter->second->transformWorldPointsToLocal(points, linkID); + } + return points; + } + /** * @brief Set forces/torques for all joints indexed by degrees of freedom. * diff --git a/src/esp/physics/PhysicsObjectBase.h b/src/esp/physics/PhysicsObjectBase.h index db77ff4370..c70c2a8ec1 100644 --- a/src/esp/physics/PhysicsObjectBase.h +++ b/src/esp/physics/PhysicsObjectBase.h @@ -219,6 +219,47 @@ class PhysicsObjectBase : public Magnum::SceneGraph::AbstractFeature3D { } } + /** + * @brief Given the list of passed points in this object's local space, return + * those points transformed to world space. + * @param points vector of points in object local space + * @param linkID Unused for rigids. + * @return vector of points transformed into world space + */ + virtual std::vector transformLocalPointsToWorld( + const std::vector& points, + CORRADE_UNUSED int linkID) const { + std::vector wsPoints; + wsPoints.reserve(points.size()); + Mn::Vector3 objScale = getScale(); + Mn::Matrix4 worldTransform = getTransformation(); + for (const auto& lsPoint : points) { + wsPoints.emplace_back(worldTransform.transformPoint(lsPoint * objScale)); + } + return wsPoints; + } + + /** + * @brief Given the list of passed points in world space, return + * those points transformed to this object's local space. + * @param points vector of points in world space + * @param linkID Unused for rigids. + * @return vector of points transformed to be in local space + */ + virtual std::vector transformWorldPointsToLocal( + const std::vector& points, + CORRADE_UNUSED int linkID) const { + std::vector lsPoints; + lsPoints.reserve(points.size()); + Mn::Vector3 objScale = getScale(); + Mn::Matrix4 worldTransform = getTransformation(); + for (const auto wsPoint : points) { + lsPoints.emplace_back(worldTransform.inverted().transformPoint(wsPoint) / + objScale); + } + return lsPoints; + } + /** * @brief Get the 3D position of the object. */ @@ -470,6 +511,11 @@ class PhysicsObjectBase : public Magnum::SceneGraph::AbstractFeature3D { userAttributes_->overwriteWithConfig(attr); } + /** @brief Get the scale of the object set during initialization. + * @return The scaling for the object relative to its initially loaded meshes. + */ + virtual Magnum::Vector3 getScale() const { return _creationScale; } + protected: /** @brief Accessed internally. Get an appropriately cast copy of the @ref * metadata::attributes::SceneObjectInstanceAttributes used to place the @@ -570,6 +616,13 @@ class PhysicsObjectBase : public Magnum::SceneGraph::AbstractFeature3D { metadata::attributes::AbstractAttributes::ptr initializationAttributes_ = nullptr; + /** + * @brief Set the object's creation scale + */ + void setScale(const Magnum::Vector3& creationScale) { + _creationScale = creationScale; + } + private: /** * @brief This object's instancing attributes, if any were used during its @@ -578,6 +631,11 @@ class PhysicsObjectBase : public Magnum::SceneGraph::AbstractFeature3D { std::shared_ptr _initObjInstanceAttrs = nullptr; + /** + * @brief The scale applied to this object on creation + */ + Mn::Vector3 _creationScale{1.0f, 1.0f, 1.0f}; + public: ESP_SMART_POINTERS(PhysicsObjectBase) }; // class PhysicsObjectBase diff --git a/src/esp/physics/RigidBase.h b/src/esp/physics/RigidBase.h index 411395e121..280f989ed5 100644 --- a/src/esp/physics/RigidBase.h +++ b/src/esp/physics/RigidBase.h @@ -361,15 +361,6 @@ class RigidBase : public esp::physics::PhysicsObjectBase { virtual void setRestitutionCoefficient( CORRADE_UNUSED const double restitutionCoefficient) {} - /** @brief Get the scale of the object set during initialization. - * @return The scaling for the object relative to its initially loaded meshes. - */ - virtual Magnum::Vector3 getScale() const { - return PhysicsObjectBase::getInitializationAttributes< - metadata::attributes::AbstractObjectAttributes>() - ->getScale(); - } - /** * @brief Get the semantic ID for this object. */ diff --git a/src/esp/physics/RigidObject.cpp b/src/esp/physics/RigidObject.cpp index f9e55093c3..4001b50398 100644 --- a/src/esp/physics/RigidObject.cpp +++ b/src/esp/physics/RigidObject.cpp @@ -20,6 +20,8 @@ bool RigidObject::initialize( return false; } + setScale(initAttributes->getScale()); + // save the copy of the template used to create the object at initialization // time setUserAttributes(initAttributes->getUserConfiguration()); diff --git a/src/esp/physics/RigidStage.cpp b/src/esp/physics/RigidStage.cpp index b5a787a4af..5a44a23292 100644 --- a/src/esp/physics/RigidStage.cpp +++ b/src/esp/physics/RigidStage.cpp @@ -20,6 +20,8 @@ bool RigidStage::initialize( objectMotionType_ = MotionType::STATIC; objectName_ = Cr::Utility::formatString( "Stage from {}", initAttributes->getSimplifiedHandle()); + + setScale(initAttributes->getScale()); // save the copy of the template used to create the object at initialization // time setUserAttributes(initAttributes->getUserConfiguration()); diff --git a/src/esp/physics/bullet/BulletArticulatedObject.cpp b/src/esp/physics/bullet/BulletArticulatedObject.cpp index cd6cd5901f..dde323d734 100644 --- a/src/esp/physics/bullet/BulletArticulatedObject.cpp +++ b/src/esp/physics/bullet/BulletArticulatedObject.cpp @@ -87,6 +87,7 @@ void BulletArticulatedObject::initializeFromURDF( // cache the global scaling from the source model globalScale_ = urdfModel->getGlobalScaling(); + setScale(Mn::Vector3{globalScale_, globalScale_, globalScale_}); u2b.convertURDFToBullet(rootTransformInWorldSpace, bWorld_.get(), linkCompoundShapes_, linkChildShapes_); @@ -145,6 +146,7 @@ void BulletArticulatedObject::initializeFromURDF( } linkObject = baseLink_.get(); } + linkObject->initializeArticulatedLink(urdfLink->m_name, getScale()); linkObject->linkName = urdfLink->m_name; linkObject->node().setType(esp::scene::SceneNodeType::OBJECT); diff --git a/src/esp/physics/bullet/BulletURDFImporter.cpp b/src/esp/physics/bullet/BulletURDFImporter.cpp index 0fe253d49d..b4c23e6758 100644 --- a/src/esp/physics/bullet/BulletURDFImporter.cpp +++ b/src/esp/physics/bullet/BulletURDFImporter.cpp @@ -493,8 +493,6 @@ Mn::Matrix4 BulletURDFImporter::convertURDFToBulletInternal( switch (jointType) { case metadata::URDF::SphericalJoint: { - // TODO: link mapping? - // creation.addLinkMapping(urdfLinkIndex, mbLinkIndex); cache->m_bulletMultiBody->setupSpherical( mbLinkIndex, mass, btVector3(localInertiaDiagonal), mbParentIndex, btQuaternion(parentRotToThis), btVector3(offsetInA.translation()), @@ -503,8 +501,6 @@ Mn::Matrix4 BulletURDFImporter::convertURDFToBulletInternal( break; } case metadata::URDF::PlanarJoint: { - // TODO: link mapping? - // creation.addLinkMapping(urdfLinkIndex, mbLinkIndex); cache->m_bulletMultiBody->setupPlanar( mbLinkIndex, mass, btVector3(localInertiaDiagonal), mbParentIndex, btQuaternion(parentRotToThis), @@ -515,17 +511,14 @@ Mn::Matrix4 BulletURDFImporter::convertURDFToBulletInternal( break; } case metadata::URDF::FloatingJoint: - case metadata::URDF::FixedJoint: { - if ((jointType == metadata::URDF::FloatingJoint) || - (jointType == metadata::URDF::PlanarJoint)) { - printf( - "Warning: joint unsupported, creating a fixed joint instead."); + if (jointType == metadata::URDF::FloatingJoint) + //|| (jointType == metadata::URDF::PlanarJoint)) + { + ESP_WARNING() << "Warning: Floating joint unsupported, creating a " + "fixed joint instead."; } - // TODO: link mapping? - // creation.addLinkMapping(urdfLinkIndex, mbLinkIndex); - - // todo: adjust the center of mass transform and pivot axis properly + // TODO: adjust the center of mass transform and pivot axis properly cache->m_bulletMultiBody->setupFixed( mbLinkIndex, mass, btVector3(localInertiaDiagonal), mbParentIndex, btQuaternion(parentRotToThis), btVector3(offsetInA.translation()), @@ -534,9 +527,6 @@ Mn::Matrix4 BulletURDFImporter::convertURDFToBulletInternal( } case metadata::URDF::ContinuousJoint: case metadata::URDF::RevoluteJoint: { - // TODO: link mapping? - // creation.addLinkMapping(urdfLinkIndex, mbLinkIndex); - cache->m_bulletMultiBody->setupRevolute( mbLinkIndex, mass, btVector3(localInertiaDiagonal), mbParentIndex, btQuaternion(parentRotToThis), @@ -561,9 +551,6 @@ Mn::Matrix4 BulletURDFImporter::convertURDFToBulletInternal( break; } case metadata::URDF::PrismaticJoint: { - // TODO: link mapping? - // creation.addLinkMapping(urdfLinkIndex, mbLinkIndex); - cache->m_bulletMultiBody->setupPrismatic( mbLinkIndex, mass, btVector3(localInertiaDiagonal), mbParentIndex, btQuaternion(parentRotToThis), @@ -590,6 +577,8 @@ Mn::Matrix4 BulletURDFImporter::convertURDFToBulletInternal( ESP_VERY_VERBOSE() << "Invalid joint type." btAssert(0); } } + // TODO: link mapping? + // creation.addLinkMapping(urdfLinkIndex, mbLinkIndex); } { diff --git a/src/esp/physics/objectWrappers/ManagedPhysicsObjectBase.h b/src/esp/physics/objectWrappers/ManagedPhysicsObjectBase.h index 21fbf5ddf7..9cc21cda89 100644 --- a/src/esp/physics/objectWrappers/ManagedPhysicsObjectBase.h +++ b/src/esp/physics/objectWrappers/ManagedPhysicsObjectBase.h @@ -163,6 +163,40 @@ class AbstractManagedPhysicsObject } } // setTranslation + /** + * @brief Given the list of passed points in this object's local space, return + * those points transformed to world space. + * @param points vector of points in object local space + * @param linkID The link ID for the object (only pertinent for articulated + * objects). Ignored for rigid objects and stages. + * @return vector of points transformed into world space + */ + std::vector transformLocalPointsToWorld( + const std::vector& points, + int linkID) const { + if (auto sp = this->getObjectReference()) { + sp->transformLocalPointsToWorld(points, linkID); + } + return {}; + } + + /** + * @brief Given the list of passed points in world space, return + * those points transformed to this object's local space. + * @param points vector of points in world space + * @param linkID The link ID for the object (only pertinent for articulated + * objects). Ignored for rigid objects and stages. + * @return vector of points transformed to be in local space + */ + std::vector transformWorldPointsToLocal( + const std::vector& points, + int linkID) const { + if (auto sp = this->getObjectReference()) { + sp->transformWorldPointsToLocal(points, linkID); + } + return {}; + } + Magnum::Quaternion getRotation() const { if (auto sp = this->getObjectReference()) { return sp->getRotation(); diff --git a/src/tests/AttributesConfigsTest.cpp b/src/tests/AttributesConfigsTest.cpp index 2e9b9f3961..2ace038ba2 100644 --- a/src/tests/AttributesConfigsTest.cpp +++ b/src/tests/AttributesConfigsTest.cpp @@ -32,6 +32,7 @@ using esp::physics::MotionType; using AttrMgrs::AttributesManager; using Attrs::ArticulatedObjectAttributes; +using Attrs::MarkerSets; using Attrs::ObjectAttributes; using Attrs::PbrShaderAttributes; using Attrs::PhysicsManagerAttributes; @@ -96,53 +97,62 @@ struct AttributesConfigsTest : Cr::TestSuite::Tester { Mn::Quaternion quatValue, Mn::Vector4 vec4Value); - typedef std::unordered_map MarkersTestMap; - typedef std::unordered_map SubsetTestMap; - typedef std::unordered_map LinksTestMap; - typedef std::unordered_map MarkerSetTestMap; + /** + * @brief Collection of markers making up a single MarkerSet + */ + typedef std::unordered_map MarkerSetTestMap; + /** + * @brief Collection of MarkerSet making up a single LinkSet + */ + typedef std::unordered_map LinkSetTestMap; + /** + * @brief Collection of LinkSets making up a single TaskSet + */ + typedef std::unordered_map TaskSetTestMap; + /** + * @brief Collection of TaskSets making up the entire MarkerSets construction. + */ + typedef std::unordered_map AllMarkerSetsTestMap; /** * @brief This method will test the marker-sets configurations being loaded in * stage, object and ao configs. * @param markerSetsConfig The marker-sets configuration object whose contents * are to be tested. - * @param markerSetsHierarchy The hieraarchy the marker sets should follow. + * @param markerSetsInfoHierarchy The hieraarchy the marker sets should + * follow. */ void testMarkerSetsConfigVals( - std::shared_ptr markerSetsConfig, - const MarkerSetTestMap& // markers in link subset - markerSetsHierarchy); + std::shared_ptr markerSetsConfig, + const AllMarkerSetsTestMap& // markers in link subset + markerSetsInfoHierarchy); /** * @brief This test will verify that the physics attributes' managers' JSON * loading process is working as expected. */ void testPhysicsAttrVals( - std::shared_ptr - physMgrAttr); + std::shared_ptr physMgrAttr); /** * @brief This test will verify that the PBR/IBL shader config attributes' * managers' JSON loading process is working as expected. */ void testPbrShaderAttrVals( - std::shared_ptr - pbrShaderAttr); + std::shared_ptr pbrShaderAttr); /** * @brief This test will verify that the Light Attributes' managers' JSON * loading process is working as expected. */ void testLightAttrVals( - std::shared_ptr - lightLayoutAttr); + std::shared_ptr lightLayoutAttr); /** * @brief This test will verify that the Scene Instance Attributes' managers' * JSON loading process is working as expected. */ void testSceneInstanceAttrVals( - std::shared_ptr - sceneInstAttr); + std::shared_ptr sceneInstAttr); /** * @brief This test will verify that the root-level scene instance user @@ -155,31 +165,27 @@ struct AttributesConfigsTest : Cr::TestSuite::Tester { * loading process is working as expected. */ void testSemanticAttrVals( - std::shared_ptr - semanticAttr, + std::shared_ptr semanticAttr, const std::string& assetPath); /** * @brief This test will verify that the Stage attributes' managers' JSON * loading process is working as expected. */ - void testStageAttrVals( - std::shared_ptr stageAttr, - const std::string& assetPath); + void testStageAttrVals(std::shared_ptr stageAttr, + const std::string& assetPath); /** * @brief This test will verify that the Object attributes' managers' JSON * loading process is working as expected. */ - void testObjectAttrVals( - std::shared_ptr objAttr, - const std::string& assetPath); + void testObjectAttrVals(std::shared_ptr objAttr, + const std::string& assetPath); /** * @brief This test will verify that the Articulated Object attributes' * managers' JSON loading process is working as expected. */ void testArticulatedObjectAttrVals( - std::shared_ptr - artObjAttr, + std::shared_ptr artObjAttr, const std::string& assetPath, const std::string& urdfPath); @@ -278,7 +284,7 @@ void AttributesConfigsTest::testUserDefinedConfigVals( // ["test_00", "test_01", "test_02", "test_03"], for (int i = 0; i < strListSize; ++i) { const std::string subKey = - Cr::Utility::formatString("user_str_array_{:.02d}", i); + Cr::Utility::formatString("user_str_array_{:.03d}", i); const std::string fieldVal = Cr::Utility::formatString("test_{:.02d}", i); CORRADE_COMPARE(userStrListSubconfig->get(subKey), fieldVal); @@ -307,38 +313,38 @@ void AttributesConfigsTest::testUserDefinedConfigVals( } // AttributesConfigsTest::testUserDefinedConfigVals void AttributesConfigsTest::testMarkerSetsConfigVals( - std::shared_ptr markerSetsConfig, - const MarkerSetTestMap& // markers in link subset - markerSetsHierarchy) { - for (const auto& markerSetInfoEntry : markerSetsHierarchy) { + std::shared_ptr markerSetsConfig, + const AllMarkerSetsTestMap& markerSetsInfoHierarchy) { + // Testing as nested subconfigs + for (const auto& taskSetInfoEntry : markerSetsInfoHierarchy) { // Key is first - const std::string markerSetKey = markerSetInfoEntry.first; - CORRADE_VERIFY(markerSetsConfig->hasSubconfig(markerSetKey)); - // Retrive named markerset - const auto markerSet = markerSetsConfig->getSubconfigView(markerSetKey); - CORRADE_VERIFY(markerSet); - // Submap - const auto& markerSetInfoMap = markerSetInfoEntry.second; - for (const auto& linkEntry : markerSetInfoMap) { - const std::string linkIDKey = linkEntry.first; - CORRADE_VERIFY(markerSet->hasSubconfig(linkIDKey)); + const std::string taskSetKey = taskSetInfoEntry.first; + CORRADE_VERIFY(markerSetsConfig->hasSubconfig(taskSetKey)); + // Retrive named TaskSet + const auto taskSet = markerSetsConfig->getSubconfigView(taskSetKey); + CORRADE_VERIFY(taskSet); + // Submap of test data + const auto& taskSetInfoMap = taskSetInfoEntry.second; + for (const auto& linkSetInfoEntry : taskSetInfoMap) { + const std::string linkSetKey = linkSetInfoEntry.first; + CORRADE_VERIFY(taskSet->hasSubconfig(linkSetKey)); // Retrieve named per-link markerset - const auto linkMarkerSet = markerSet->getSubconfigView(linkIDKey); - CORRADE_VERIFY(linkMarkerSet); - // Per link subsets - const auto& linkSetInfoMap = linkEntry.second; - for (const auto& linkSubsetEntry : linkSetInfoMap) { - const std::string subsetIDKey = linkSubsetEntry.first; - CORRADE_VERIFY(linkMarkerSet->hasSubconfig(subsetIDKey)); - // Retrive a specific link's subset - const auto linkSubset = linkMarkerSet->getSubconfigView(subsetIDKey); - CORRADE_VERIFY(linkSubset); + const auto linkSet = taskSet->getSubconfigView(linkSetKey); + CORRADE_VERIFY(linkSet); + // Per linkmarker sets + const auto& linkSetInfoMap = linkSetInfoEntry.second; + for (const auto& markerSetInfoEntry : linkSetInfoMap) { + const std::string markerSetKey = markerSetInfoEntry.first; + CORRADE_VERIFY(linkSet->hasSubconfig(markerSetKey)); + // Retrive a specific link's markerSet + const auto markerSet = linkSet->getSubconfigView(markerSetKey); + CORRADE_VERIFY(markerSet); // Verify that subconfig named "markers' exists" - CORRADE_VERIFY(linkSubset->hasSubconfig("markers")); - // Get array of markers - const auto& markers = linkSubset->getSubconfigView("markers"); + CORRADE_VERIFY(markerSet->hasSubconfig("markers")); + // Get the set of markers + const auto& markers = markerSet->getSubconfigView("markers"); // key-value map that should match mapping of markers in link subset - const auto& markersInfo = linkSubsetEntry.second; + const auto& markersInfo = markerSetInfoEntry.second; // Verify there are the expected number of marker points CORRADE_COMPARE(markers->getNumValues(), markersInfo.size()); // Verify that subconfig has each marker point @@ -349,17 +355,17 @@ void AttributesConfigsTest::testMarkerSetsConfigVals( const Mn::Vector3 markerPoint = markerInfo.second; // Make sure marker point is present CORRADE_COMPARE(markers->get(markerKey), markerPoint); - } // for each marker within subset - } // for each subset within link set - } // for each link set in marker set - } // for each marker_set + } // for each marker within marker set + } // for each marker set within link set + } // for each link set in task set + } // for each task set within MarkerSets aggregation + // TODO : test as MarkerSets class hierarchy } // AttributesConfigsTest::testMarkerSetsConfigVals ///////////// Begin JSON String-based tests void AttributesConfigsTest::testPhysicsAttrVals( - std::shared_ptr - physMgrAttr) { + std::shared_ptr physMgrAttr) { // match values set in test JSON CORRADE_COMPARE(physMgrAttr->getGravity(), Mn::Vector3(1, 2, 3)); @@ -442,8 +448,7 @@ void AttributesConfigsTest::testPhysicsJSONLoad() { } // AttributesConfigsTest::testPhysicsJSONLoad void AttributesConfigsTest::testPbrShaderAttrVals( - std::shared_ptr - pbrShaderAttr) { + std::shared_ptr pbrShaderAttr) { CORRADE_VERIFY(!pbrShaderAttr->getEnableDirectLighting()); CORRADE_VERIFY(!pbrShaderAttr->getEnableIBL()); @@ -574,8 +579,7 @@ void AttributesConfigsTest::testPbrShaderAttrJSONLoad() { } // AttributesConfigsTest::testPbrShaderAttrJSONLoad void AttributesConfigsTest::testLightAttrVals( - std::shared_ptr - lightLayoutAttr) { + std::shared_ptr lightLayoutAttr) { // test light layout attributes-level user config vals testUserDefinedConfigVals(lightLayoutAttr->getUserConfiguration(), 4, "light attribs defined string", true, 23, 2.3, @@ -730,8 +734,7 @@ void AttributesConfigsTest::testSceneInstanceRootUserDefinedAttrVals( } // AttributesConfigsTest::testSceneInstanceRootUserDefinedAttrVals void AttributesConfigsTest::testSceneInstanceAttrVals( - std::shared_ptr - sceneAttr) { + std::shared_ptr sceneAttr) { // match values set in test JSON CORRADE_COMPARE( static_cast(sceneAttr->getTranslationOrigin()), @@ -1138,7 +1141,7 @@ void AttributesConfigsTest::testSceneInstanceJSONLoad() { } // AttributesConfigsTest::testSceneInstanceJSONLoad void AttributesConfigsTest::testSemanticAttrVals( - std::shared_ptr semanticAttr, + std::shared_ptr semanticAttr, const std::string& assetPath) { CORRADE_COMPARE( semanticAttr->getSemanticDescriptorFilename(), @@ -1307,7 +1310,7 @@ void AttributesConfigsTest::testSemanticJSONLoad() { } // AttributesConfigsTest::testSemanticJSONLoad void AttributesConfigsTest::testStageAttrVals( - std::shared_ptr stageAttr, + std::shared_ptr stageAttr, const std::string& assetPath) { // match values set in test JSON CORRADE_COMPARE(stageAttr->getScale(), Mn::Vector3(2, 3, 4)); @@ -1349,71 +1352,72 @@ void AttributesConfigsTest::testStageAttrVals( { // Test marker sets - MarkersTestMap subset_id_000_stage_name( - {{"markers_03", Mn::Vector3{4.1, 5.2, 6.3}}, - {"markers_02", Mn::Vector3{3.1, 4.2, 5.3}}, - {"markers_01", Mn::Vector3{2.1, 3.2, 4.3}}, - {"markers_00", Mn::Vector3{1.1, 2.2, 3.3}}}); - - MarkersTestMap subset_id_001_stage_name( - {{"3", Mn::Vector3{4.2, 5.3, 6.4}}, - {"2", Mn::Vector3{3.2, 4.3, 5.4}}, - {"1", Mn::Vector3{2.2, 3.3, 4.4}}, - {"0", Mn::Vector3{1.2, 2.3, 3.4}}}); - - SubsetTestMap link_id_00_stage_name( - {{"subset_id_000_stage_name", subset_id_000_stage_name}, - {"subset_id_001_stage_name", subset_id_001_stage_name}}); - - LinksTestMap marker_set_0_stage_name{ - {{"link_id_00_stage_name", link_id_00_stage_name}}}; - - MarkersTestMap subset_id_100_stage_name( - {{"markers_03", Mn::Vector3{14.1, 15.2, 16.3}}, - {"markers_02", Mn::Vector3{13.1, 14.2, 15.3}}, - {"markers_01", Mn::Vector3{12.1, 13.2, 14.3}}, - {"markers_00", Mn::Vector3{11.1, 12.2, 13.3}}}); - - MarkersTestMap subset_id_101_stage_name( - {{"3", Mn::Vector3{14.2, 15.3, 16.4}}, - {"2", Mn::Vector3{13.2, 14.3, 15.4}}, - {"1", Mn::Vector3{12.2, 13.3, 14.4}}, - {"0", Mn::Vector3{11.2, 12.3, 13.4}}}); - - MarkersTestMap subset_id_102_stage_name( - {{"3", Mn::Vector3{124.2, 125.3, 126.4}}, - {"2", Mn::Vector3{123.2, 124.3, 125.4}}, - {"1", Mn::Vector3{122.2, 123.3, 124.4}}, - {"0", Mn::Vector3{121.2, 122.3, 123.4}}}); - - SubsetTestMap link_id_10_stage_name( - {{"subset_id_100_stage_name", subset_id_100_stage_name}, - {"subset_id_101_stage_name", subset_id_101_stage_name}, - {"subset_id_102_stage_name", subset_id_102_stage_name}}); - - MarkersTestMap subset_id_110_stage_name( - {{"3", Mn::Vector3{14.3, 15.4, 16.5}}, - {"2", Mn::Vector3{13.3, 14.4, 15.5}}, - {"1", Mn::Vector3{12.3, 13.4, 14.5}}, - {"0", Mn::Vector3{11.3, 12.4, 13.5}}}); - - MarkersTestMap subset_id_111_stage_name( - {{"markers_03", Mn::Vector3{14.4, 15.5, 16.6}}, - {"markers_02", Mn::Vector3{13.4, 14.5, 15.6}}, - {"markers_01", Mn::Vector3{12.4, 13.5, 14.6}}, - {"markers_00", Mn::Vector3{11.4, 12.5, 13.6}}}); - - SubsetTestMap link_id_11_stage_name( - {{"subset_id_110_stage_name", subset_id_110_stage_name}, - {"subset_id_110_stage_name", subset_id_110_stage_name}}); - - LinksTestMap marker_set_1_stage_name( - {{"link_id_10_stage_name", link_id_10_stage_name}, - {"link_id_11_stage_name", link_id_11_stage_name}}); - - MarkerSetTestMap markerSetMap( - {{"marker_set_0_stage_name", marker_set_0_stage_name}, - {"marker_set_1_stage_name", marker_set_1_stage_name}}); + MarkerSetTestMap marker_set_000_stage_name( + {{"003", Mn::Vector3{4.1, 5.2, 6.3}}, + {"002", Mn::Vector3{3.1, 4.2, 5.3}}, + {"001", Mn::Vector3{2.1, 3.2, 4.3}}, + {"000", Mn::Vector3{1.1, 2.2, 3.3}}}); + + MarkerSetTestMap marker_set_001_stage_name( + {{"003", Mn::Vector3{4.2, 5.3, 6.4}}, + {"002", Mn::Vector3{3.2, 4.3, 5.4}}, + {"001", Mn::Vector3{2.2, 3.3, 4.4}}, + {"000", Mn::Vector3{1.2, 2.3, 3.4}}}); + + LinkSetTestMap link_set_00_stage_name( + {{"marker_set_000_stage_name", marker_set_000_stage_name}, + {"marker_set_001_stage_name", marker_set_001_stage_name}}); + + TaskSetTestMap task_set_0_stage_name{ + {{"link_set_00_stage_name", link_set_00_stage_name}}}; + + MarkerSetTestMap marker_set_100_stage_name( + {{"003", Mn::Vector3{14.1, 15.2, 16.3}}, + {"002", Mn::Vector3{13.1, 14.2, 15.3}}, + {"001", Mn::Vector3{12.1, 13.2, 14.3}}, + {"000", Mn::Vector3{11.1, 12.2, 13.3}}}); + + MarkerSetTestMap marker_set_101_stage_name( + {{"003", Mn::Vector3{14.2, 15.3, 16.4}}, + {"002", Mn::Vector3{13.2, 14.3, 15.4}}, + {"001", Mn::Vector3{12.2, 13.3, 14.4}}, + {"000", Mn::Vector3{11.2, 12.3, 13.4}}}); + + MarkerSetTestMap marker_set_102_stage_name( + {{"003", Mn::Vector3{124.2, 125.3, 126.4}}, + {"002", Mn::Vector3{123.2, 124.3, 125.4}}, + {"001", Mn::Vector3{122.2, 123.3, 124.4}}, + {"000", Mn::Vector3{121.2, 122.3, 123.4}}}); + + LinkSetTestMap link_set_10_stage_name( + {{"marker_set_100_stage_name", marker_set_100_stage_name}, + {"marker_set_101_stage_name", marker_set_101_stage_name}, + {"marker_set_102_stage_name", marker_set_102_stage_name}}); + + MarkerSetTestMap marker_set_110_stage_name( + {{"003", Mn::Vector3{14.3, 15.4, 16.5}}, + {"002", Mn::Vector3{13.3, 14.4, 15.5}}, + {"001", Mn::Vector3{12.3, 13.4, 14.5}}, + {"000", Mn::Vector3{11.3, 12.4, 13.5}}}); + + MarkerSetTestMap marker_set_111_stage_name( + {{"003", Mn::Vector3{14.4, 15.5, 16.6}}, + {"002", Mn::Vector3{13.4, 14.5, 15.6}}, + {"004", Mn::Vector3{15.4, 16.5, 17.6}}, + {"001", Mn::Vector3{12.4, 13.5, 14.6}}, + {"000", Mn::Vector3{11.4, 12.5, 13.6}}}); + + LinkSetTestMap link_set_11_stage_name( + {{"marker_set_110_stage_name", marker_set_110_stage_name}, + {"marker_set_110_stage_name", marker_set_110_stage_name}}); + + TaskSetTestMap task_set_1_stage_name( + {{"link_set_10_stage_name", link_set_10_stage_name}, + {"link_set_11_stage_name", link_set_11_stage_name}}); + + AllMarkerSetsTestMap markerSetMap( + {{"task_set_0_stage_name", task_set_0_stage_name}, + {"task_set_1_stage_name", task_set_1_stage_name}}); testMarkerSetsConfigVals(stageAttr->getMarkerSetsConfiguration(), markerSetMap); @@ -1443,9 +1447,9 @@ void AttributesConfigsTest::testStageJSONLoad() { "nav_asset":"testJSONNavMeshAsset.glb", "shader_type" : "material", "marker_sets" : { - "marker_set_0_stage_name" : { - "link_id_00_stage_name" : { - "subset_id_000_stage_name" : { + "task_set_0_stage_name" : { + "link_set_00_stage_name" : { + "marker_set_000_stage_name" : { "markers" : [ [1.1, 2.2, 3.3], [2.1, 3.2, 4.3], @@ -1453,7 +1457,7 @@ void AttributesConfigsTest::testStageJSONLoad() { [4.1, 5.2, 6.3] ] }, - "subset_id_001_stage_name" : { + "marker_set_001_stage_name" : { "markers" : { "0":[1.2, 2.3, 3.4], "1":[2.2, 3.3, 4.4], @@ -1463,9 +1467,9 @@ void AttributesConfigsTest::testStageJSONLoad() { } } }, - "marker_set_1_stage_name" : { - "link_id_10_stage_name" : { - "subset_id_100_stage_name" : { + "task_set_1_stage_name" : { + "link_set_10_stage_name" : { + "marker_set_100_stage_name" : { "markers" : [ [11.1, 12.2, 13.3], [12.1, 13.2, 14.3], @@ -1473,7 +1477,7 @@ void AttributesConfigsTest::testStageJSONLoad() { [14.1, 15.2, 16.3] ] }, - "subset_id_101_stage_name" : { + "marker_set_101_stage_name" : { "markers" : { "0":[11.2, 12.3, 13.4], "1":[12.2, 13.3, 14.4], @@ -1481,7 +1485,7 @@ void AttributesConfigsTest::testStageJSONLoad() { "3":[14.2, 15.3, 16.4] } }, - "subset_id_102_stage_name" : { + "marker_set_102_stage_name" : { "markers" : { "0":[121.2, 122.3, 123.4], "1":[122.2, 123.3, 124.4], @@ -1490,8 +1494,8 @@ void AttributesConfigsTest::testStageJSONLoad() { } } }, - "link_id_11_stage_name" : { - "subset_id_110_stage_name" : { + "link_set_11_stage_name" : { + "marker_set_110_stage_name" : { "markers" : { "0":[11.3, 12.4, 13.5], "1":[12.3, 13.4, 14.5], @@ -1499,12 +1503,13 @@ void AttributesConfigsTest::testStageJSONLoad() { "3":[14.3, 15.4, 16.5] } }, - "subset_id_111_stage_name" : { + "marker_set_111_stage_name" : { "markers" : [ [11.4, 12.5, 13.6], [12.4, 13.5, 14.6], [13.4, 14.5, 15.6], - [14.4, 15.5, 16.6] + [14.4, 15.5, 16.6], + [15.4, 16.5, 17.6] ] } } @@ -1585,7 +1590,7 @@ void AttributesConfigsTest::testStageJSONLoad() { } // AttributesConfigsTest::testStageJSONLoad( void AttributesConfigsTest::testObjectAttrVals( - std::shared_ptr objAttr, + std::shared_ptr objAttr, const std::string& assetPath) { // match values set in test JSON @@ -1619,79 +1624,81 @@ void AttributesConfigsTest::testObjectAttrVals( { // Test marker sets - MarkersTestMap subset_id_000_obj_name( - {{"markers_03", Mn::Vector3{4.1, 5.2, 6.3}}, - {"markers_02", Mn::Vector3{3.1, 4.2, 5.3}}, - {"markers_01", Mn::Vector3{2.1, 3.2, 4.3}}, - {"markers_00", Mn::Vector3{1.1, 2.2, 3.3}}}); - - MarkersTestMap subset_id_001_obj_name({{"3", Mn::Vector3{4.2, 5.3, 6.4}}, - {"2", Mn::Vector3{3.2, 4.3, 5.4}}, - {"1", Mn::Vector3{2.2, 3.3, 4.4}}, - {"0", Mn::Vector3{1.2, 2.3, 3.4}}}); - - SubsetTestMap link_id_00_obj_name( - {{"subset_id_000_obj_name", subset_id_000_obj_name}, - {"subset_id_001_obj_name", subset_id_001_obj_name}}); - - MarkersTestMap subset_id_010_obj_name({{"3", Mn::Vector3{4.3, 5.4, 6.5}}, - {"2", Mn::Vector3{3.3, 4.4, 5.5}}, - {"1", Mn::Vector3{2.3, 3.4, 4.5}}, - {"0", Mn::Vector3{1.3, 2.4, 3.5}}}); - - MarkersTestMap subset_id_011_obj_name( - {{"markers_03", Mn::Vector3{4.4, 5.5, 6.6}}, - {"markers_02", Mn::Vector3{3.4, 4.5, 5.6}}, - {"markers_01", Mn::Vector3{2.4, 3.5, 4.6}}, - {"markers_00", Mn::Vector3{1.4, 2.5, 3.6}}}); - - SubsetTestMap link_id_01_obj_name( - {{"subset_id_010_obj_name", subset_id_010_obj_name}, - {"subset_id_011_obj_name", subset_id_011_obj_name}}); - - LinksTestMap marker_set_0_obj_name{ - {{"link_id_00_obj_name", link_id_00_obj_name}, - {"link_id_01_obj_name", link_id_01_obj_name}}}; - - MarkersTestMap subset_id_100_obj_name( - {{"markers_03", Mn::Vector3{14.1, 15.2, 16.3}}, - {"markers_02", Mn::Vector3{13.1, 14.2, 15.3}}, - {"markers_01", Mn::Vector3{12.1, 13.2, 14.3}}, - {"markers_00", Mn::Vector3{11.1, 12.2, 13.3}}}); - - MarkersTestMap subset_id_101_obj_name( - {{"3", Mn::Vector3{14.2, 15.3, 16.4}}, - {"2", Mn::Vector3{13.2, 14.3, 15.4}}, - {"1", Mn::Vector3{12.2, 13.3, 14.4}}, - {"0", Mn::Vector3{11.2, 12.3, 13.4}}}); - - SubsetTestMap link_id_10_obj_name( - {{"subset_id_100_obj_name", subset_id_100_obj_name}, - {"subset_id_101_obj_name", subset_id_101_obj_name}}); - - MarkersTestMap subset_id_110_obj_name( - {{"3", Mn::Vector3{14.3, 15.4, 16.5}}, - {"2", Mn::Vector3{13.3, 14.4, 15.5}}, - {"1", Mn::Vector3{12.3, 13.4, 14.5}}, - {"0", Mn::Vector3{11.3, 12.4, 13.5}}}); - - MarkersTestMap subset_id_111_obj_name( - {{"markers_03", Mn::Vector3{14.4, 15.5, 16.6}}, - {"markers_02", Mn::Vector3{13.4, 14.5, 15.6}}, - {"markers_01", Mn::Vector3{12.4, 13.5, 14.6}}, - {"markers_00", Mn::Vector3{11.4, 12.5, 13.6}}}); - - SubsetTestMap link_id_11_obj_name( - {{"subset_id_110_obj_name", subset_id_110_obj_name}, - {"subset_id_110_obj_name", subset_id_110_obj_name}}); - - LinksTestMap marker_set_1_obj_name( - {{"link_id_10_obj_name", link_id_10_obj_name}, - {"link_id_11_obj_name", link_id_11_obj_name}}); - - MarkerSetTestMap markerSetMap( - {{"marker_set_0_obj_name", marker_set_0_obj_name}, - {"marker_set_1_obj_name", marker_set_1_obj_name}}); + MarkerSetTestMap marker_set_000_obj_name( + {{"003", Mn::Vector3{4.1, 5.2, 6.3}}, + {"002", Mn::Vector3{3.1, 4.2, 5.3}}, + {"001", Mn::Vector3{2.1, 3.2, 4.3}}, + {"000", Mn::Vector3{1.1, 2.2, 3.3}}}); + + MarkerSetTestMap marker_set_001_obj_name( + {{"003", Mn::Vector3{4.2, 5.3, 6.4}}, + {"002", Mn::Vector3{3.2, 4.3, 5.4}}, + {"001", Mn::Vector3{2.2, 3.3, 4.4}}, + {"000", Mn::Vector3{1.2, 2.3, 3.4}}}); + + LinkSetTestMap link_set_00_obj_name( + {{"marker_set_000_obj_name", marker_set_000_obj_name}, + {"marker_set_001_obj_name", marker_set_001_obj_name}}); + + MarkerSetTestMap marker_set_010_obj_name( + {{"003", Mn::Vector3{4.3, 5.4, 6.5}}, + {"002", Mn::Vector3{3.3, 4.4, 5.5}}, + {"001", Mn::Vector3{2.3, 3.4, 4.5}}, + {"000", Mn::Vector3{1.3, 2.4, 3.5}}}); + + MarkerSetTestMap marker_set_011_obj_name( + {{"003", Mn::Vector3{4.4, 5.5, 6.6}}, + {"002", Mn::Vector3{3.4, 4.5, 5.6}}, + {"001", Mn::Vector3{2.4, 3.5, 4.6}}, + {"000", Mn::Vector3{1.4, 2.5, 3.6}}}); + + LinkSetTestMap link_set_01_obj_name( + {{"marker_set_010_obj_name", marker_set_010_obj_name}, + {"marker_set_011_obj_name", marker_set_011_obj_name}}); + + TaskSetTestMap task_set_0_obj_name{ + {{"link_set_00_obj_name", link_set_00_obj_name}, + {"link_set_01_obj_name", link_set_01_obj_name}}}; + + MarkerSetTestMap marker_set_100_obj_name( + {{"003", Mn::Vector3{14.1, 15.2, 16.3}}, + {"002", Mn::Vector3{13.1, 14.2, 15.3}}, + {"001", Mn::Vector3{12.1, 13.2, 14.3}}, + {"000", Mn::Vector3{11.1, 12.2, 13.3}}}); + + MarkerSetTestMap marker_set_101_obj_name( + {{"003", Mn::Vector3{14.2, 15.3, 16.4}}, + {"002", Mn::Vector3{13.2, 14.3, 15.4}}, + {"001", Mn::Vector3{12.2, 13.3, 14.4}}, + {"000", Mn::Vector3{11.2, 12.3, 13.4}}}); + + LinkSetTestMap link_set_10_obj_name( + {{"marker_set_100_obj_name", marker_set_100_obj_name}, + {"marker_set_101_obj_name", marker_set_101_obj_name}}); + + MarkerSetTestMap marker_set_110_obj_name( + {{"003", Mn::Vector3{14.3, 15.4, 16.5}}, + {"002", Mn::Vector3{13.3, 14.4, 15.5}}, + {"001", Mn::Vector3{12.3, 13.4, 14.5}}, + {"000", Mn::Vector3{11.3, 12.4, 13.5}}}); + + MarkerSetTestMap marker_set_111_obj_name( + {{"003", Mn::Vector3{14.4, 15.5, 16.6}}, + {"002", Mn::Vector3{13.4, 14.5, 15.6}}, + {"001", Mn::Vector3{12.4, 13.5, 14.6}}, + {"000", Mn::Vector3{11.4, 12.5, 13.6}}}); + + LinkSetTestMap link_set_11_obj_name( + {{"marker_set_110_obj_name", marker_set_110_obj_name}, + {"marker_set_110_obj_name", marker_set_110_obj_name}}); + + TaskSetTestMap task_set_1_obj_name( + {{"link_set_10_obj_name", link_set_10_obj_name}, + {"link_set_11_obj_name", link_set_11_obj_name}}); + + AllMarkerSetsTestMap markerSetMap( + {{"task_set_0_obj_name", task_set_0_obj_name}, + {"task_set_1_obj_name", task_set_1_obj_name}}); testMarkerSetsConfigVals(objAttr->getMarkerSetsConfiguration(), markerSetMap); @@ -1727,9 +1734,9 @@ void AttributesConfigsTest::testObjectJSONLoad() { "COM": [0.1,0.2,0.3], "shader_type" : "phong", "marker_sets" : { - "marker_set_0_obj_name" : { - "link_id_00_obj_name" : { - "subset_id_000_obj_name" : { + "task_set_0_obj_name" : { + "link_set_00_obj_name" : { + "marker_set_000_obj_name" : { "markers" : [ [1.1, 2.2, 3.3], [2.1, 3.2, 4.3], @@ -1737,7 +1744,7 @@ void AttributesConfigsTest::testObjectJSONLoad() { [4.1, 5.2, 6.3] ] }, - "subset_id_001_obj_name" : { + "marker_set_001_obj_name" : { "markers" : { "0":[1.2, 2.3, 3.4], "1":[2.2, 3.3, 4.4], @@ -1746,8 +1753,8 @@ void AttributesConfigsTest::testObjectJSONLoad() { } } }, - "link_id_01_obj_name" : { - "subset_id_010_obj_name" : { + "link_set_01_obj_name" : { + "marker_set_010_obj_name" : { "markers" : { "0":[1.3, 2.4, 3.5], "1":[2.3, 3.4, 4.5], @@ -1755,7 +1762,7 @@ void AttributesConfigsTest::testObjectJSONLoad() { "3":[4.3, 5.4, 6.5] } }, - "subset_id_011_obj_name" : { + "marker_set_011_obj_name" : { "markers" : [ [1.4, 2.5, 3.6], [2.4, 3.5, 4.6], @@ -1765,9 +1772,9 @@ void AttributesConfigsTest::testObjectJSONLoad() { } } }, - "marker_set_1_obj_name" : { - "link_id_10_obj_name" : { - "subset_id_100_obj_name" : { + "task_set_1_obj_name" : { + "link_set_10_obj_name" : { + "marker_set_100_obj_name" : { "markers" : [ [11.1, 12.2, 13.3], [12.1, 13.2, 14.3], @@ -1775,7 +1782,7 @@ void AttributesConfigsTest::testObjectJSONLoad() { [14.1, 15.2, 16.3] ] }, - "subset_id_101_obj_name" : { + "marker_set_101_obj_name" : { "markers" : { "0":[11.2, 12.3, 13.4], "1":[12.2, 13.3, 14.4], @@ -1784,8 +1791,8 @@ void AttributesConfigsTest::testObjectJSONLoad() { } } }, - "link_id_11_obj_name" : { - "subset_id_110_obj_name" : { + "link_set_11_obj_name" : { + "marker_set_110_obj_name" : { "markers" : { "0":[11.3, 12.4, 13.5], "1":[12.3, 13.4, 14.5], @@ -1793,7 +1800,7 @@ void AttributesConfigsTest::testObjectJSONLoad() { "3":[14.3, 15.4, 16.5] } }, - "subset_id_111_obj_name" : { + "marker_set_111_obj_name" : { "markers" : [ [11.4, 12.5, 13.6], [12.4, 13.5, 14.6], @@ -1868,8 +1875,7 @@ void AttributesConfigsTest::testObjectJSONLoad() { } // AttributesConfigsTest::testObjectJSONLoadTest void AttributesConfigsTest::testArticulatedObjectAttrVals( - std::shared_ptr - artObjAttr, + std::shared_ptr artObjAttr, const std::string& assetPath, const std::string& urdfPath) { // match values set in test JSON @@ -1905,102 +1911,105 @@ void AttributesConfigsTest::testArticulatedObjectAttrVals( { // Test marker sets - MarkersTestMap subset_id_000_ao_name( - {{"markers_03", Mn::Vector3{14.1, 5.2, 6.3}}, - {"markers_02", Mn::Vector3{13.1, 4.2, 5.3}}, - {"markers_01", Mn::Vector3{12.1, 3.2, 4.3}}, - {"markers_00", Mn::Vector3{11.1, 2.2, 3.3}}}); - - MarkersTestMap subset_id_001_ao_name({{"3", Mn::Vector3{14.2, 5.3, 6.4}}, - {"2", Mn::Vector3{13.2, 4.3, 5.4}}, - {"1", Mn::Vector3{12.2, 3.3, 4.4}}, - {"0", Mn::Vector3{11.2, 2.3, 3.4}}}); - - SubsetTestMap link_id_00_ao_name( - {{"subset_id_000_ao_name", subset_id_000_ao_name}, - {"subset_id_001_ao_name", subset_id_001_ao_name}}); - - MarkersTestMap subset_id_010_ao_name({{"3", Mn::Vector3{14.3, 5.4, 6.5}}, - {"2", Mn::Vector3{13.3, 4.4, 5.5}}, - {"1", Mn::Vector3{12.3, 3.4, 4.5}}, - {"0", Mn::Vector3{11.3, 2.4, 3.5}}}); - - MarkersTestMap subset_id_011_ao_name( - {{"markers_03", Mn::Vector3{14.4, 5.5, 6.6}}, - {"markers_02", Mn::Vector3{13.4, 4.5, 5.6}}, - {"markers_01", Mn::Vector3{12.4, 3.5, 4.6}}, - {"markers_00", Mn::Vector3{11.4, 2.5, 3.6}}}); - - SubsetTestMap link_id_01_ao_name( - {{"subset_id_010_ao_name", subset_id_010_ao_name}, - {"subset_id_011_ao_name", subset_id_011_ao_name}}); - - MarkersTestMap subset_id_020_ao_name({{"3", Mn::Vector3{24.3, 5.4, 6.5}}, - {"2", Mn::Vector3{23.3, 4.4, 5.5}}, - {"1", Mn::Vector3{22.3, 3.4, 4.5}}, - {"0", Mn::Vector3{21.3, 2.4, 3.5}}}); - - MarkersTestMap subset_id_021_ao_name( - {{"markers_03", Mn::Vector3{24.4, 5.5, 6.6}}, - {"markers_02", Mn::Vector3{23.4, 4.5, 5.6}}, - {"markers_01", Mn::Vector3{22.4, 3.5, 4.6}}, - {"markers_00", Mn::Vector3{21.4, 2.5, 3.6}}}); - - MarkersTestMap subset_id_022_ao_name( - {{"markers_03", Mn::Vector3{224.4, 5.5, 6.6}}, - {"markers_02", Mn::Vector3{223.4, 4.5, 5.6}}, - {"markers_01", Mn::Vector3{222.4, 3.5, 4.6}}, - {"markers_00", Mn::Vector3{221.4, 2.5, 3.6}}}); - - SubsetTestMap link_id_02_ao_name( - {{"subset_id_020_ao_name", subset_id_020_ao_name}, - {"subset_id_021_ao_name", subset_id_021_ao_name}, - {"subset_id_022_ao_name", subset_id_022_ao_name}}); - - LinksTestMap marker_set_0_ao_name{ - {{"link_id_00_ao_name", link_id_00_ao_name}, - {"link_id_01_ao_name", link_id_01_ao_name}, - {"link_id_02_ao_name", link_id_02_ao_name}}}; - - MarkersTestMap subset_id_100_ao_name( - {{"markers_03", Mn::Vector3{114.1, 15.2, 16.3}}, - {"markers_02", Mn::Vector3{113.1, 14.2, 15.3}}, - {"markers_01", Mn::Vector3{112.1, 13.2, 14.3}}, - {"markers_00", Mn::Vector3{111.1, 12.2, 13.3}}}); - - MarkersTestMap subset_id_101_ao_name( - {{"3", Mn::Vector3{114.2, 15.3, 16.4}}, - {"2", Mn::Vector3{113.2, 14.3, 15.4}}, - {"1", Mn::Vector3{112.2, 13.3, 14.4}}, - {"0", Mn::Vector3{111.2, 12.3, 13.4}}}); - - SubsetTestMap link_id_10_ao_name( - {{"subset_id_100_ao_name", subset_id_100_ao_name}, - {"subset_id_101_ao_name", subset_id_101_ao_name}}); - - MarkersTestMap subset_id_110_ao_name( - {{"3", Mn::Vector3{114.3, 15.4, 16.5}}, - {"2", Mn::Vector3{113.3, 14.4, 15.5}}, - {"1", Mn::Vector3{112.3, 13.4, 14.5}}, - {"0", Mn::Vector3{111.3, 12.4, 13.5}}}); - - MarkersTestMap subset_id_111_ao_name( - {{"markers_03", Mn::Vector3{114.4, 15.5, 16.6}}, - {"markers_02", Mn::Vector3{113.4, 14.5, 15.6}}, - {"markers_01", Mn::Vector3{112.4, 13.5, 14.6}}, - {"markers_00", Mn::Vector3{111.4, 12.5, 13.6}}}); - - SubsetTestMap link_id_11_ao_name( - {{"subset_id_110_ao_name", subset_id_110_ao_name}, - {"subset_id_110_ao_name", subset_id_110_ao_name}}); - - LinksTestMap marker_set_1_ao_name( - {{"link_id_10_ao_name", link_id_10_ao_name}, - {"link_id_11_ao_name", link_id_11_ao_name}}); - - MarkerSetTestMap markerSetMap( - {{"marker_set_0_ao_name", marker_set_0_ao_name}, - {"marker_set_1_ao_name", marker_set_1_ao_name}}); + MarkerSetTestMap marker_set_000_ao_name( + {{"003", Mn::Vector3{14.1, 5.2, 6.3}}, + {"002", Mn::Vector3{13.1, 4.2, 5.3}}, + {"001", Mn::Vector3{12.1, 3.2, 4.3}}, + {"000", Mn::Vector3{11.1, 2.2, 3.3}}}); + + MarkerSetTestMap marker_set_001_ao_name( + {{"003", Mn::Vector3{14.2, 5.3, 6.4}}, + {"002", Mn::Vector3{13.2, 4.3, 5.4}}, + {"001", Mn::Vector3{12.2, 3.3, 4.4}}, + {"000", Mn::Vector3{11.2, 2.3, 3.4}}}); + + LinkSetTestMap link_set_00_ao_name( + {{"marker_set_000_ao_name", marker_set_000_ao_name}, + {"marker_set_001_ao_name", marker_set_001_ao_name}}); + + MarkerSetTestMap marker_set_010_ao_name( + {{"003", Mn::Vector3{14.3, 5.4, 6.5}}, + {"002", Mn::Vector3{13.3, 4.4, 5.5}}, + {"001", Mn::Vector3{12.3, 3.4, 4.5}}, + {"000", Mn::Vector3{11.3, 2.4, 3.5}}}); + + MarkerSetTestMap marker_set_011_ao_name( + {{"003", Mn::Vector3{14.4, 5.5, 6.6}}, + {"002", Mn::Vector3{13.4, 4.5, 5.6}}, + {"001", Mn::Vector3{12.4, 3.5, 4.6}}, + {"000", Mn::Vector3{11.4, 2.5, 3.6}}}); + + LinkSetTestMap link_set_01_ao_name( + {{"marker_set_010_ao_name", marker_set_010_ao_name}, + {"marker_set_011_ao_name", marker_set_011_ao_name}}); + + MarkerSetTestMap marker_set_020_ao_name( + {{"003", Mn::Vector3{24.3, 5.4, 6.5}}, + {"002", Mn::Vector3{23.3, 4.4, 5.5}}, + {"001", Mn::Vector3{22.3, 3.4, 4.5}}, + {"000", Mn::Vector3{21.3, 2.4, 3.5}}}); + + MarkerSetTestMap marker_set_021_ao_name( + {{"003", Mn::Vector3{24.4, 5.5, 6.6}}, + {"002", Mn::Vector3{23.4, 4.5, 5.6}}, + {"001", Mn::Vector3{22.4, 3.5, 4.6}}, + {"000", Mn::Vector3{21.4, 2.5, 3.6}}}); + + MarkerSetTestMap marker_set_022_ao_name( + {{"003", Mn::Vector3{224.4, 5.5, 6.6}}, + {"002", Mn::Vector3{223.4, 4.5, 5.6}}, + {"001", Mn::Vector3{222.4, 3.5, 4.6}}, + {"000", Mn::Vector3{221.4, 2.5, 3.6}}}); + + LinkSetTestMap link_set_02_ao_name( + {{"marker_set_020_ao_name", marker_set_020_ao_name}, + {"marker_set_021_ao_name", marker_set_021_ao_name}, + {"marker_set_022_ao_name", marker_set_022_ao_name}}); + + TaskSetTestMap task_set_0_ao_name{ + {{"link_set_00_ao_name", link_set_00_ao_name}, + {"link_set_01_ao_name", link_set_01_ao_name}, + {"link_set_02_ao_name", link_set_02_ao_name}}}; + + MarkerSetTestMap marker_set_100_ao_name( + {{"003", Mn::Vector3{114.1, 15.2, 16.3}}, + {"002", Mn::Vector3{113.1, 14.2, 15.3}}, + {"001", Mn::Vector3{112.1, 13.2, 14.3}}, + {"000", Mn::Vector3{111.1, 12.2, 13.3}}}); + + MarkerSetTestMap marker_set_101_ao_name( + {{"003", Mn::Vector3{114.2, 15.3, 16.4}}, + {"002", Mn::Vector3{113.2, 14.3, 15.4}}, + {"001", Mn::Vector3{112.2, 13.3, 14.4}}, + {"000", Mn::Vector3{111.2, 12.3, 13.4}}}); + + LinkSetTestMap link_set_10_ao_name( + {{"marker_set_100_ao_name", marker_set_100_ao_name}, + {"marker_set_101_ao_name", marker_set_101_ao_name}}); + + MarkerSetTestMap marker_set_110_ao_name( + {{"003", Mn::Vector3{114.3, 15.4, 16.5}}, + {"002", Mn::Vector3{113.3, 14.4, 15.5}}, + {"001", Mn::Vector3{112.3, 13.4, 14.5}}, + {"000", Mn::Vector3{111.3, 12.4, 13.5}}}); + + MarkerSetTestMap marker_set_111_ao_name( + {{"003", Mn::Vector3{114.4, 15.5, 16.6}}, + {"002", Mn::Vector3{113.4, 14.5, 15.6}}, + {"001", Mn::Vector3{112.4, 13.5, 14.6}}, + {"000", Mn::Vector3{111.4, 12.5, 13.6}}}); + + LinkSetTestMap link_set_11_ao_name( + {{"marker_set_110_ao_name", marker_set_110_ao_name}, + {"marker_set_110_ao_name", marker_set_110_ao_name}}); + + TaskSetTestMap task_set_1_ao_name( + {{"link_set_10_ao_name", link_set_10_ao_name}, + {"link_set_11_ao_name", link_set_11_ao_name}}); + + AllMarkerSetsTestMap markerSetMap( + {{"task_set_0_ao_name", task_set_0_ao_name}, + {"task_set_1_ao_name", task_set_1_ao_name}}); testMarkerSetsConfigVals(artObjAttr->getMarkerSetsConfiguration(), markerSetMap); @@ -2024,9 +2033,9 @@ void AttributesConfigsTest::testArticulatedObjectJSONLoad() { "render_mode": "skin", "shader_type" : "pbr", "marker_sets" : { - "marker_set_0_ao_name" : { - "link_id_00_ao_name" : { - "subset_id_000_ao_name" : { + "task_set_0_ao_name" : { + "link_set_00_ao_name" : { + "marker_set_000_ao_name" : { "markers" : [ [11.1, 2.2, 3.3], [12.1, 3.2, 4.3], @@ -2034,7 +2043,7 @@ void AttributesConfigsTest::testArticulatedObjectJSONLoad() { [14.1, 5.2, 6.3] ] }, - "subset_id_001_ao_name" : { + "marker_set_001_ao_name" : { "markers" : { "0":[11.2, 2.3, 3.4], "1":[12.2, 3.3, 4.4], @@ -2043,8 +2052,8 @@ void AttributesConfigsTest::testArticulatedObjectJSONLoad() { } } }, - "link_id_01_ao_name" : { - "subset_id_010_ao_name" : { + "link_set_01_ao_name" : { + "marker_set_010_ao_name" : { "markers" : { "0":[11.3, 2.4, 3.5], "1":[12.3, 3.4, 4.5], @@ -2052,7 +2061,7 @@ void AttributesConfigsTest::testArticulatedObjectJSONLoad() { "3":[14.3, 5.4, 6.5] } }, - "subset_id_011_ao_name" : { + "marker_set_011_ao_name" : { "markers" : [ [11.4, 2.5, 3.6], [12.4, 3.5, 4.6], @@ -2061,8 +2070,8 @@ void AttributesConfigsTest::testArticulatedObjectJSONLoad() { ] } }, - "link_id_02_ao_name" : { - "subset_id_020_ao_name" : { + "link_set_02_ao_name" : { + "marker_set_020_ao_name" : { "markers" : { "0":[21.3, 2.4, 3.5], "1":[22.3, 3.4, 4.5], @@ -2070,7 +2079,7 @@ void AttributesConfigsTest::testArticulatedObjectJSONLoad() { "3":[24.3, 5.4, 6.5] } }, - "subset_id_021_ao_name" : { + "marker_set_021_ao_name" : { "markers" : [ [21.4, 2.5, 3.6], [22.4, 3.5, 4.6], @@ -2078,7 +2087,7 @@ void AttributesConfigsTest::testArticulatedObjectJSONLoad() { [24.4, 5.5, 6.6] ] }, - "subset_id_022_ao_name" : { + "marker_set_022_ao_name" : { "markers" : [ [221.4, 2.5, 3.6], [222.4, 3.5, 4.6], @@ -2088,9 +2097,9 @@ void AttributesConfigsTest::testArticulatedObjectJSONLoad() { } } }, - "marker_set_1_ao_name" : { - "link_id_10_ao_name" : { - "subset_id_100_ao_name" : { + "task_set_1_ao_name" : { + "link_set_10_ao_name" : { + "marker_set_100_ao_name" : { "markers" : [ [111.1, 12.2, 13.3], [112.1, 13.2, 14.3], @@ -2098,7 +2107,7 @@ void AttributesConfigsTest::testArticulatedObjectJSONLoad() { [114.1, 15.2, 16.3] ] }, - "subset_id_101_ao_name" : { + "marker_set_101_ao_name" : { "markers" : { "0":[111.2, 12.3, 13.4], "1":[112.2, 13.3, 14.4], @@ -2107,8 +2116,8 @@ void AttributesConfigsTest::testArticulatedObjectJSONLoad() { } } }, - "link_id_11_ao_name" : { - "subset_id_110_ao_name" : { + "link_set_11_ao_name" : { + "marker_set_110_ao_name" : { "markers" : { "0":[111.3, 12.4, 13.5], "1":[112.3, 13.4, 14.5], @@ -2116,7 +2125,7 @@ void AttributesConfigsTest::testArticulatedObjectJSONLoad() { "3":[114.3, 15.4, 16.5] } }, - "subset_id_111_ao_name" : { + "marker_set_111_ao_name" : { "markers" : [ [111.4, 12.5, 13.6], [112.4, 13.5, 14.6],