From 9c5d40076d775c579b9085766f136dfd254ff1a7 Mon Sep 17 00:00:00 2001 From: r13308365493 Date: Thu, 3 Jul 2025 16:44:21 +0800 Subject: [PATCH] Fix: update camera observation with env.step and Fixed the header file import problem and the application problem in ros after version update --- .../isaac/matterport/config/importer_cfg.py | 6 ++--- .../matterport/domains/matterport_importer.py | 12 ++++----- .../domains/matterport_raycast_camera.py | 6 ++--- .../domains/matterport_raycaster.py | 2 +- .../isaac/matterport/domains/raycaster_cfg.py | 4 +-- .../matterport/scripts/matterport_domains.py | 6 ++--- .../matterport/scripts/matterport_ext.py | 4 +-- .../viplanner/collectors/terrain_analysis.py | 8 +++--- .../collectors/terrain_analysis_cfg.py | 2 +- .../collectors/viewpoint_sampling.py | 12 ++++----- .../collectors/viewpoint_sampling_cfg.py | 2 +- .../omni/viplanner/config/base_cfg.py | 20 +++++++------- .../omni/viplanner/config/carla_cfg.py | 18 +++++++------ .../omni/viplanner/config/carla_class_cost.py | 2 +- .../omni/viplanner/config/matterport_cfg.py | 19 +++++++------- .../viplanner/config/matterport_class_cost.py | 2 +- .../omni/viplanner/config/warehouse_cfg.py | 13 +++++----- .../omni/viplanner/utils/unreal_importer.py | 12 ++++----- .../viplanner/utils/unreal_importer_cfg.py | 4 +-- .../viplanner_matterport_raycast_camera.py | 4 +-- .../omni/viplanner/viplanner/mdp/__init__.py | 2 +- .../mdp/actions/navigation_actions.py | 8 +++--- .../path_follower_command_generator.py | 14 +++++----- .../path_follower_command_generator_cfg.py | 6 ++--- .../viplanner/viplanner/mdp/observations.py | 6 ++--- .../viplanner/viplanner/mdp/terminations.py | 8 +++--- .../viplanner/viplanner/viplanner_algo.py | 4 +-- omniverse/standalone/viplanner_demo.py | 26 ++++++++++++++----- ros/planner/config/anymal_mount.yaml | 18 ++++++------- ros/planner/launch/viplanner.launch | 2 ++ .../src/waypoint_tool.cpp | 6 ++--- 31 files changed, 137 insertions(+), 121 deletions(-) diff --git a/omniverse/extension/omni.isaac.matterport/omni/isaac/matterport/config/importer_cfg.py b/omniverse/extension/omni.isaac.matterport/omni/isaac/matterport/config/importer_cfg.py index 133487e..87cd631 100644 --- a/omniverse/extension/omni.isaac.matterport/omni/isaac/matterport/config/importer_cfg.py +++ b/omniverse/extension/omni.isaac.matterport/omni/isaac/matterport/config/importer_cfg.py @@ -5,9 +5,9 @@ # SPDX-License-Identifier: BSD-3-Clause -from omni.isaac.core.utils import extensions -from omni.isaac.lab.terrains import TerrainImporterCfg -from omni.isaac.lab.utils import configclass +from isaacsim.core.utils import extensions +from isaaclab.terrains import TerrainImporterCfg +from isaaclab.utils import configclass from omni.isaac.matterport.domains import MatterportImporter from typing_extensions import Literal diff --git a/omniverse/extension/omni.isaac.matterport/omni/isaac/matterport/domains/matterport_importer.py b/omniverse/extension/omni.isaac.matterport/omni/isaac/matterport/domains/matterport_importer.py index 5db838f..48af372 100644 --- a/omniverse/extension/omni.isaac.matterport/omni/isaac/matterport/domains/matterport_importer.py +++ b/omniverse/extension/omni.isaac.matterport/omni/isaac/matterport/domains/matterport_importer.py @@ -15,19 +15,19 @@ # omni import carb -import omni.isaac.core.utils.prims as prim_utils -import omni.isaac.core.utils.stage as stage_utils +import isaacsim.core.utils.prims as prim_utils +import isaacsim.core.utils.stage as stage_utils # isaac-lab -import omni.isaac.lab.sim as sim_utils -from omni.isaac.core.simulation_context import SimulationContext -from omni.isaac.lab.terrains import TerrainImporter +import isaaclab.sim as sim_utils +from isaacsim.core.api.simulation_context import SimulationContext +from isaaclab.terrains import TerrainImporter if TYPE_CHECKING: from omni.isaac.matterport.config import MatterportImporterCfg # omniverse -from omni.isaac.core.utils import extensions +from isaacsim.core.utils import extensions extensions.enable_extension("omni.kit.asset_converter") import omni.kit.asset_converter as converter diff --git a/omniverse/extension/omni.isaac.matterport/omni/isaac/matterport/domains/matterport_raycast_camera.py b/omniverse/extension/omni.isaac.matterport/omni/isaac/matterport/domains/matterport_raycast_camera.py index 135fbb1..011d7e6 100644 --- a/omniverse/extension/omni.isaac.matterport/omni/isaac/matterport/domains/matterport_raycast_camera.py +++ b/omniverse/extension/omni.isaac.matterport/omni/isaac/matterport/domains/matterport_raycast_camera.py @@ -12,13 +12,13 @@ import carb import numpy as np -import omni.isaac.lab.utils.math as math_utils +import isaaclab.utils.math as math_utils import pandas as pd import torch import trimesh import warp as wp -from omni.isaac.lab.sensors import RayCasterCamera, RayCasterCameraCfg -from omni.isaac.lab.utils.warp import raycast_mesh +from isaaclab.sensors import RayCasterCamera, RayCasterCameraCfg +from isaaclab.utils.warp import raycast_mesh from omni.isaac.matterport.domains import DATA_DIR from tensordict import TensorDict diff --git a/omniverse/extension/omni.isaac.matterport/omni/isaac/matterport/domains/matterport_raycaster.py b/omniverse/extension/omni.isaac.matterport/omni/isaac/matterport/domains/matterport_raycaster.py index fc8faad..de2891f 100644 --- a/omniverse/extension/omni.isaac.matterport/omni/isaac/matterport/domains/matterport_raycaster.py +++ b/omniverse/extension/omni.isaac.matterport/omni/isaac/matterport/domains/matterport_raycaster.py @@ -13,7 +13,7 @@ import numpy as np import trimesh import warp as wp -from omni.isaac.lab.sensors.ray_caster import RayCaster +from isaaclab.sensors.ray_caster import RayCaster from omni.isaac.matterport.domains import DATA_DIR if TYPE_CHECKING: diff --git a/omniverse/extension/omni.isaac.matterport/omni/isaac/matterport/domains/raycaster_cfg.py b/omniverse/extension/omni.isaac.matterport/omni/isaac/matterport/domains/raycaster_cfg.py index 92960ad..009f83c 100644 --- a/omniverse/extension/omni.isaac.matterport/omni/isaac/matterport/domains/raycaster_cfg.py +++ b/omniverse/extension/omni.isaac.matterport/omni/isaac/matterport/domains/raycaster_cfg.py @@ -5,8 +5,8 @@ # SPDX-License-Identifier: BSD-3-Clause -from omni.isaac.lab.sensors.ray_caster import RayCasterCfg -from omni.isaac.lab.utils import configclass +from isaaclab.sensors.ray_caster import RayCasterCfg +from isaaclab.utils import configclass from .matterport_raycaster import MatterportRayCaster diff --git a/omniverse/extension/omni.isaac.matterport/omni/isaac/matterport/scripts/matterport_domains.py b/omniverse/extension/omni.isaac.matterport/omni/isaac/matterport/scripts/matterport_domains.py index fab4bbf..a4e8186 100644 --- a/omniverse/extension/omni.isaac.matterport/omni/isaac/matterport/scripts/matterport_domains.py +++ b/omniverse/extension/omni.isaac.matterport/omni/isaac/matterport/scripts/matterport_domains.py @@ -13,9 +13,9 @@ import numpy as np import omni import torch -from omni.isaac.lab.sensors.camera import CameraData -from omni.isaac.lab.sensors.ray_caster import RayCasterCfg -from omni.isaac.lab.sim import SimulationContext +from isaaclab.sensors.camera import CameraData +from isaaclab.sensors.ray_caster import RayCasterCfg +from isaaclab.sim import SimulationContext from omni.isaac.matterport.domains.matterport_raycast_camera import ( MatterportRayCasterCamera, ) diff --git a/omniverse/extension/omni.isaac.matterport/omni/isaac/matterport/scripts/matterport_ext.py b/omniverse/extension/omni.isaac.matterport/omni/isaac/matterport/scripts/matterport_ext.py index 051a327..99b5a0e 100644 --- a/omniverse/extension/omni.isaac.matterport/omni/isaac/matterport/scripts/matterport_ext.py +++ b/omniverse/extension/omni.isaac.matterport/omni/isaac/matterport/scripts/matterport_ext.py @@ -22,8 +22,8 @@ # isaac-core import omni.ui as ui -from omni.isaac.lab.sensors.ray_caster import RayCasterCameraCfg, patterns -from omni.isaac.lab.sim import SimulationCfg, SimulationContext +from isaaclab.sensors.ray_caster import RayCasterCameraCfg, patterns +from isaaclab.sim import SimulationCfg, SimulationContext from omni.isaac.matterport.domains import MatterportImporter # omni-isaac-ui diff --git a/omniverse/extension/omni.viplanner/omni/viplanner/collectors/terrain_analysis.py b/omniverse/extension/omni.viplanner/omni/viplanner/collectors/terrain_analysis.py index da200c2..3fc69f0 100644 --- a/omniverse/extension/omni.viplanner/omni/viplanner/collectors/terrain_analysis.py +++ b/omniverse/extension/omni.viplanner/omni/viplanner/collectors/terrain_analysis.py @@ -15,10 +15,10 @@ import scipy.spatial.transform as tf import torch from omni.isaac.core.utils.semantics import get_semantics -from omni.isaac.lab.scene import InteractiveScene -from omni.isaac.lab.sensors import RayCaster, RayCasterCamera -from omni.isaac.lab.sim import SimulationContext -from omni.isaac.lab.utils.warp import raycast_mesh +from isaaclab.scene import InteractiveScene +from isaaclab.sensors import RayCaster, RayCasterCamera +from isaaclab.sim import SimulationContext +from isaaclab.utils.warp import raycast_mesh from omni.isaac.matterport.domains import MatterportRayCaster, MatterportRayCasterCamera from omni.physx import get_physx_scene_query_interface from pxr import Gf, Usd, UsdGeom diff --git a/omniverse/extension/omni.viplanner/omni/viplanner/collectors/terrain_analysis_cfg.py b/omniverse/extension/omni.viplanner/omni/viplanner/collectors/terrain_analysis_cfg.py index ded2987..3ae407a 100644 --- a/omniverse/extension/omni.viplanner/omni/viplanner/collectors/terrain_analysis_cfg.py +++ b/omniverse/extension/omni.viplanner/omni/viplanner/collectors/terrain_analysis_cfg.py @@ -6,7 +6,7 @@ from __future__ import annotations -from omni.isaac.lab.utils import configclass +from isaaclab.utils import configclass from omni.viplanner.config import MatterportSemanticCostMapping diff --git a/omniverse/extension/omni.viplanner/omni/viplanner/collectors/viewpoint_sampling.py b/omniverse/extension/omni.viplanner/omni/viplanner/collectors/viewpoint_sampling.py index 164c19c..5867fe9 100644 --- a/omniverse/extension/omni.viplanner/omni/viplanner/collectors/viewpoint_sampling.py +++ b/omniverse/extension/omni.viplanner/omni/viplanner/collectors/viewpoint_sampling.py @@ -14,13 +14,13 @@ import cv2 import numpy as np -import omni.isaac.lab.utils.math as math_utils +import isaaclab.utils.math as math_utils import torch -from omni.isaac.lab.markers import VisualizationMarkers -from omni.isaac.lab.markers.config import GREEN_ARROW_X_MARKER_CFG -from omni.isaac.lab.scene import InteractiveScene -from omni.isaac.lab.sensors import Camera -from omni.isaac.lab.sim import SimulationContext +from isaaclab.markers import VisualizationMarkers +from isaaclab.markers.config import GREEN_ARROW_X_MARKER_CFG +from isaaclab.scene import InteractiveScene +from isaaclab.sensors import Camera +from isaaclab.sim import SimulationContext from viplanner.config import VIPlannerSemMetaHandler diff --git a/omniverse/extension/omni.viplanner/omni/viplanner/collectors/viewpoint_sampling_cfg.py b/omniverse/extension/omni.viplanner/omni/viplanner/collectors/viewpoint_sampling_cfg.py index 6571703..39d9549 100644 --- a/omniverse/extension/omni.viplanner/omni/viplanner/collectors/viewpoint_sampling_cfg.py +++ b/omniverse/extension/omni.viplanner/omni/viplanner/collectors/viewpoint_sampling_cfg.py @@ -4,7 +4,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -from omni.isaac.lab.utils import configclass +from isaaclab.utils import configclass from .terrain_analysis_cfg import TerrainAnalysisCfg diff --git a/omniverse/extension/omni.viplanner/omni/viplanner/config/base_cfg.py b/omniverse/extension/omni.viplanner/omni/viplanner/config/base_cfg.py index af1b6f5..d0d1575 100644 --- a/omniverse/extension/omni.viplanner/omni/viplanner/config/base_cfg.py +++ b/omniverse/extension/omni.viplanner/omni/viplanner/config/base_cfg.py @@ -7,15 +7,14 @@ import os import omni.viplanner.viplanner.mdp as mdp -from omni.isaac.lab.envs import ManagerBasedRLEnvCfg -from omni.isaac.lab.managers import EventTermCfg as EventTerm -from omni.isaac.lab.managers import ObservationGroupCfg as ObsGroup -from omni.isaac.lab.managers import ObservationTermCfg as ObsTerm -from omni.isaac.lab.managers import SceneEntityCfg -from omni.isaac.lab.managers import TerminationTermCfg as DoneTerm -from omni.isaac.lab.utils import configclass -from omni.isaac.lab.utils.assets import ISAACLAB_NUCLEUS_DIR - +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.managers import EventTermCfg as EventTerm +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR ## # MDP settings ## @@ -152,9 +151,10 @@ class CommandsCfg: vel_command: mdp.PathFollowerCommandGeneratorCfg = mdp.PathFollowerCommandGeneratorCfg( robot_attr="robot", - lookAheadDistance=1.0, + lookAheadDistance=2, debug_vis=True, maxSpeed=1.0, + resampling_time_range=(0.1, 0.3), ) diff --git a/omniverse/extension/omni.viplanner/omni/viplanner/config/carla_cfg.py b/omniverse/extension/omni.viplanner/omni/viplanner/config/carla_cfg.py index 6b02cae..390ea3e 100644 --- a/omniverse/extension/omni.viplanner/omni/viplanner/config/carla_cfg.py +++ b/omniverse/extension/omni.viplanner/omni/viplanner/config/carla_cfg.py @@ -6,18 +6,19 @@ import os -import omni.isaac.lab.sim as sim_utils -from omni.isaac.lab.assets import AssetBaseCfg -from omni.isaac.lab.scene import InteractiveSceneCfg -from omni.isaac.lab.sensors import CameraCfg, ContactSensorCfg, RayCasterCfg, patterns -from omni.isaac.lab.utils import configclass +import isaaclab.sim as sim_utils +from isaaclab.assets import AssetBaseCfg +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sensors import CameraCfg, ContactSensorCfg, RayCasterCfg, patterns +from isaaclab.utils import configclass from omni.viplanner.utils import UnRealImporterCfg ## # Pre-defined configs ## # isort: off -from omni.isaac.lab_assets.anymal import ANYMAL_C_CFG +from isaaclab_assets.robots.anymal import ANYMAL_C_CFG +from isaaclab_assets.robots.fourier import GR1T2_CFG from .base_cfg import ViPlannerBaseCfg from ..viplanner import DATA_DIR @@ -40,7 +41,7 @@ class TerrainSceneCfg(InteractiveSceneCfg): dynamic_friction=1.0, ), # NOTE: this path should be absolute to load the textures correctly - usd_path="${USER_PATH_TO_USD}/carla.usd", + usd_path="/home/fourier/vip_ws/src/viplanner/Carla_USD/carla.usd", groundplane=True, cw_config_file=os.path.join(DATA_DIR, "town01", "cw_multiply_cfg.yml"), sem_mesh_to_class_map=os.path.join(DATA_DIR, "town01", "keyword_mapping.yml"), @@ -108,13 +109,14 @@ class ViPlannerCarlaCfg(ViPlannerBaseCfg): """Configuration for the locomotion velocity-tracking environment.""" # Scene settings - scene: TerrainSceneCfg = TerrainSceneCfg(num_envs=1, env_spacing=1.0, replicate_physics=False) + scene: TerrainSceneCfg = TerrainSceneCfg(num_envs=1, env_spacing=1.0, replicate_physics=True) def __post_init__(self): """Post initialization.""" super().__post_init__() # adapt viewer self.viewer.eye = (133, 127.5, 8.5) + #self.sim.dt = 1.0 / 10.0 self.viewer.lookat = (125.5, 120, 1.0) # change ANYmal position self.scene.robot.init_state.pos = (125.5, 118.5, 0.8) diff --git a/omniverse/extension/omni.viplanner/omni/viplanner/config/carla_class_cost.py b/omniverse/extension/omni.viplanner/omni/viplanner/config/carla_class_cost.py index f102598..89cf583 100644 --- a/omniverse/extension/omni.viplanner/omni/viplanner/config/carla_class_cost.py +++ b/omniverse/extension/omni.viplanner/omni/viplanner/config/carla_class_cost.py @@ -4,7 +4,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -from omni.isaac.lab.utils import configclass +from isaaclab.utils import configclass OBSTACLE_COST = 2.0 ROAD_LOSS = 1.5 diff --git a/omniverse/extension/omni.viplanner/omni/viplanner/config/matterport_cfg.py b/omniverse/extension/omni.viplanner/omni/viplanner/config/matterport_cfg.py index bf25479..e93cbc8 100644 --- a/omniverse/extension/omni.viplanner/omni/viplanner/config/matterport_cfg.py +++ b/omniverse/extension/omni.viplanner/omni/viplanner/config/matterport_cfg.py @@ -4,15 +4,15 @@ # # SPDX-License-Identifier: BSD-3-Clause -import omni.isaac.lab.sim as sim_utils +import isaaclab.sim as sim_utils import omni.viplanner.viplanner.mdp as mdp -from omni.isaac.lab.assets import AssetBaseCfg -from omni.isaac.lab.managers import ObservationGroupCfg as ObsGroup -from omni.isaac.lab.managers import ObservationTermCfg as ObsTerm -from omni.isaac.lab.managers import SceneEntityCfg -from omni.isaac.lab.scene import InteractiveSceneCfg -from omni.isaac.lab.sensors import ContactSensorCfg, patterns, CameraCfg -from omni.isaac.lab.utils import configclass +from isaaclab.assets import AssetBaseCfg +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sensors import ContactSensorCfg, patterns, CameraCfg +from isaaclab.utils import configclass from omni.isaac.matterport.config import MatterportImporterCfg from omni.isaac.matterport.domains import MatterportRayCasterCfg from omni.viplanner.utils import VIPlannerMatterportRayCasterCameraCfg @@ -23,8 +23,7 @@ # Pre-defined configs ## # isort: off -from omni.isaac.lab_assets.anymal import ANYMAL_C_CFG - +from isaaclab_assets.robots.anymal import ANYMAL_C_CFG ## # Scene definition ## diff --git a/omniverse/extension/omni.viplanner/omni/viplanner/config/matterport_class_cost.py b/omniverse/extension/omni.viplanner/omni/viplanner/config/matterport_class_cost.py index 500e25b..4617881 100644 --- a/omniverse/extension/omni.viplanner/omni/viplanner/config/matterport_class_cost.py +++ b/omniverse/extension/omni.viplanner/omni/viplanner/config/matterport_class_cost.py @@ -4,7 +4,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -from omni.isaac.lab.utils import configclass +from isaaclab.utils import configclass OBSTACLE_COST = 1.0 TRAVERSABLE_COST = 0.0 diff --git a/omniverse/extension/omni.viplanner/omni/viplanner/config/warehouse_cfg.py b/omniverse/extension/omni.viplanner/omni/viplanner/config/warehouse_cfg.py index ae6c97e..57589bb 100644 --- a/omniverse/extension/omni.viplanner/omni/viplanner/config/warehouse_cfg.py +++ b/omniverse/extension/omni.viplanner/omni/viplanner/config/warehouse_cfg.py @@ -6,11 +6,11 @@ import os -import omni.isaac.lab.sim as sim_utils -from omni.isaac.lab.assets import AssetBaseCfg -from omni.isaac.lab.scene import InteractiveSceneCfg -from omni.isaac.lab.sensors import CameraCfg, ContactSensorCfg, RayCasterCfg, patterns -from omni.isaac.lab.utils import configclass +import isaaclab.sim as sim_utils +from isaaclab.assets import AssetBaseCfg +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sensors import CameraCfg, ContactSensorCfg, RayCasterCfg, patterns +from isaaclab.utils import configclass from omni.viplanner.utils import UnRealImporterCfg from ..viplanner import DATA_DIR @@ -20,8 +20,7 @@ # Pre-defined configs ## # isort: off -from omni.isaac.lab_assets.anymal import ANYMAL_C_CFG - +from isaaclab_assets.robots.anymal import ANYMAL_C_CFG ## # Scene definition diff --git a/omniverse/extension/omni.viplanner/omni/viplanner/utils/unreal_importer.py b/omniverse/extension/omni.viplanner/omni/viplanner/utils/unreal_importer.py index f46fca7..01c0803 100644 --- a/omniverse/extension/omni.viplanner/omni/viplanner/utils/unreal_importer.py +++ b/omniverse/extension/omni.viplanner/omni/viplanner/utils/unreal_importer.py @@ -12,14 +12,14 @@ import carb import numpy as np import omni -import omni.isaac.core.utils.prims as prim_utils -import omni.isaac.lab.sim as sim_utils +import isaacsim.core.utils.prims as prim_utils +import isaaclab.sim as sim_utils import trimesh import yaml -from omni.isaac.core.utils.semantics import add_update_semantics, remove_all_semantics -from omni.isaac.lab.terrains import TerrainImporter -from omni.isaac.lab.utils.assets import ISAAC_NUCLEUS_DIR -from omni.isaac.lab.utils.warp import convert_to_warp_mesh +from isaacsim.core.utils.semantics import add_update_semantics, remove_all_semantics +from isaaclab.terrains import TerrainImporter +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR +from isaaclab.utils.warp import convert_to_warp_mesh from pxr import Gf, Usd, UsdGeom if TYPE_CHECKING: diff --git a/omniverse/extension/omni.viplanner/omni/viplanner/utils/unreal_importer_cfg.py b/omniverse/extension/omni.viplanner/omni/viplanner/utils/unreal_importer_cfg.py index 317b407..43c641e 100644 --- a/omniverse/extension/omni.viplanner/omni/viplanner/utils/unreal_importer_cfg.py +++ b/omniverse/extension/omni.viplanner/omni/viplanner/utils/unreal_importer_cfg.py @@ -4,8 +4,8 @@ # # SPDX-License-Identifier: BSD-3-Clause -from omni.isaac.lab.terrains import TerrainImporterCfg -from omni.isaac.lab.utils import configclass +from isaaclab.terrains import TerrainImporterCfg +from isaaclab.utils import configclass from .unreal_importer import UnRealImporter diff --git a/omniverse/extension/omni.viplanner/omni/viplanner/utils/viplanner_matterport_raycast_camera.py b/omniverse/extension/omni.viplanner/omni/viplanner/utils/viplanner_matterport_raycast_camera.py index 5e30ed9..27ede95 100644 --- a/omniverse/extension/omni.viplanner/omni/viplanner/utils/viplanner_matterport_raycast_camera.py +++ b/omniverse/extension/omni.viplanner/omni/viplanner/utils/viplanner_matterport_raycast_camera.py @@ -6,8 +6,8 @@ import torch import yaml -from omni.isaac.lab.sensors.ray_caster import RayCasterCameraCfg -from omni.isaac.lab.utils.configclass import configclass +from isaaclab.sensors.ray_caster import RayCasterCameraCfg +from isaaclab.utils.configclass import configclass from omni.isaac.matterport.domains import MatterportRayCasterCamera from omni.viplanner.viplanner import DATA_DIR diff --git a/omniverse/extension/omni.viplanner/omni/viplanner/viplanner/mdp/__init__.py b/omniverse/extension/omni.viplanner/omni/viplanner/viplanner/mdp/__init__.py index 3d013af..79cacf7 100644 --- a/omniverse/extension/omni.viplanner/omni/viplanner/viplanner/mdp/__init__.py +++ b/omniverse/extension/omni.viplanner/omni/viplanner/viplanner/mdp/__init__.py @@ -6,7 +6,7 @@ """This sub-module contains the functions that are specific to the viplanner environments.""" -from omni.isaac.lab.envs.mdp import * # noqa: F401, F403 +from isaaclab.envs.mdp import * # noqa: F401, F403 from .actions import * # noqa: F401, F403 from .commands import * # noqa: F401, F403 diff --git a/omniverse/extension/omni.viplanner/omni/viplanner/viplanner/mdp/actions/navigation_actions.py b/omniverse/extension/omni.viplanner/omni/viplanner/viplanner/mdp/actions/navigation_actions.py index 84d42c2..b77ec02 100644 --- a/omniverse/extension/omni.viplanner/omni/viplanner/viplanner/mdp/actions/navigation_actions.py +++ b/omniverse/extension/omni.viplanner/omni/viplanner/viplanner/mdp/actions/navigation_actions.py @@ -9,10 +9,10 @@ from dataclasses import MISSING import torch -from omni.isaac.lab.envs import ManagerBasedRLEnv -from omni.isaac.lab.managers.action_manager import ActionTerm, ActionTermCfg -from omni.isaac.lab.utils import configclass -from omni.isaac.lab.utils.assets import check_file_path, read_file +from isaaclab.envs import ManagerBasedRLEnv +from isaaclab.managers.action_manager import ActionTerm, ActionTermCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import check_file_path, read_file # -- Navigation Action diff --git a/omniverse/extension/omni.viplanner/omni/viplanner/viplanner/mdp/commands/path_follower_command_generator.py b/omniverse/extension/omni.viplanner/omni/viplanner/viplanner/mdp/commands/path_follower_command_generator.py index 0ce4705..06e9e49 100644 --- a/omniverse/extension/omni.viplanner/omni/viplanner/viplanner/mdp/commands/path_follower_command_generator.py +++ b/omniverse/extension/omni.viplanner/omni/viplanner/viplanner/mdp/commands/path_follower_command_generator.py @@ -10,17 +10,17 @@ import math from typing import TYPE_CHECKING, Sequence -import omni.isaac.lab.utils.math as math_utils +import isaaclab.utils.math as math_utils import torch -from omni.isaac.lab.assets.articulation import Articulation -from omni.isaac.lab.envs import ManagerBasedRLEnv -from omni.isaac.lab.managers import CommandTerm -from omni.isaac.lab.markers import VisualizationMarkers -from omni.isaac.lab.markers.config import ( +from isaaclab.assets.articulation import Articulation +from isaaclab.envs import ManagerBasedRLEnv +from isaaclab.managers import CommandTerm +from isaaclab.markers import VisualizationMarkers +from isaaclab.markers.config import ( BLUE_ARROW_X_MARKER_CFG, GREEN_ARROW_X_MARKER_CFG, ) -from omni.isaac.lab.sim import SimulationContext +from isaaclab.sim import SimulationContext if TYPE_CHECKING: from .path_follower_command_generator_cfg import PathFollowerCommandGeneratorCfg diff --git a/omniverse/extension/omni.viplanner/omni/viplanner/viplanner/mdp/commands/path_follower_command_generator_cfg.py b/omniverse/extension/omni.viplanner/omni/viplanner/viplanner/mdp/commands/path_follower_command_generator_cfg.py index a336d63..adf08db 100644 --- a/omniverse/extension/omni.viplanner/omni/viplanner/viplanner/mdp/commands/path_follower_command_generator_cfg.py +++ b/omniverse/extension/omni.viplanner/omni/viplanner/viplanner/mdp/commands/path_follower_command_generator_cfg.py @@ -9,8 +9,8 @@ import math from dataclasses import MISSING -from omni.isaac.lab.managers import CommandTermCfg -from omni.isaac.lab.utils.configclass import configclass +from isaaclab.managers import CommandTermCfg +from isaaclab.utils.configclass import configclass from typing_extensions import Literal from .path_follower_command_generator import PathFollowerCommandGenerator @@ -46,7 +46,7 @@ class PathFollowerCommandGeneratorCfg(CommandTermCfg): """Gain for the yaw rate.""" stopYawRateGain: float = 7.0 # 3.5 """""" - maxYawRate: float = 90.0 * math.pi / 360 + maxYawRate: float = 90 * math.pi / 360 dirDiffThre: float = 0.7 stopDisThre: float = 0.2 slowDwnDisThre: float = 0.3 diff --git a/omniverse/extension/omni.viplanner/omni/viplanner/viplanner/mdp/observations.py b/omniverse/extension/omni.viplanner/omni/viplanner/viplanner/mdp/observations.py index 70513ab..2625981 100644 --- a/omniverse/extension/omni.viplanner/omni/viplanner/viplanner/mdp/observations.py +++ b/omniverse/extension/omni.viplanner/omni/viplanner/viplanner/mdp/observations.py @@ -15,15 +15,15 @@ from typing import TYPE_CHECKING import torch -from omni.isaac.lab.managers import SceneEntityCfg -from omni.isaac.lab.sensors.camera import CameraData +from isaaclab.managers import SceneEntityCfg +from isaaclab.sensors.camera import CameraData from viplanner.config import VIPlannerSemMetaHandler from .actions import NavigationAction if TYPE_CHECKING: - from omni.isaac.lab.envs.base_env import ManagerBasedEnv + from isaaclab.envs.base_env import ManagerBasedEnv # initialize viplanner config diff --git a/omniverse/extension/omni.viplanner/omni/viplanner/viplanner/mdp/terminations.py b/omniverse/extension/omni.viplanner/omni/viplanner/viplanner/mdp/terminations.py index 2ad4373..970b333 100644 --- a/omniverse/extension/omni.viplanner/omni/viplanner/viplanner/mdp/terminations.py +++ b/omniverse/extension/omni.viplanner/omni/viplanner/viplanner/mdp/terminations.py @@ -8,13 +8,13 @@ from typing import TYPE_CHECKING -import omni.isaac.core.utils.prims as prim_utils +import isaacsim.core.utils.prims as prim_utils import torch -from omni.isaac.lab.assets import Articulation -from omni.isaac.lab.managers import SceneEntityCfg +from isaaclab.assets import Articulation +from isaaclab.managers import SceneEntityCfg if TYPE_CHECKING: - from omni.isaac.lab.envs import ManagerBasedRLEnv + from isaaclab.envs import ManagerBasedRLEnv def at_goal( diff --git a/omniverse/extension/omni.viplanner/omni/viplanner/viplanner/viplanner_algo.py b/omniverse/extension/omni.viplanner/omni/viplanner/viplanner/viplanner_algo.py index a52465f..2fb45e3 100644 --- a/omniverse/extension/omni.viplanner/omni/viplanner/viplanner/viplanner_algo.py +++ b/omniverse/extension/omni.viplanner/omni/viplanner/viplanner/viplanner_algo.py @@ -7,7 +7,7 @@ import os import carb -import omni.isaac.lab.utils.math as math_utils +import isaaclab.utils.math as math_utils import torch import torchvision.transforms as transforms @@ -52,7 +52,7 @@ def __init__(self, model_dir: str, fear_threshold: float = 0.5, device: str = "c # setup waypoint display in Isaac # in headless mode, we cannot visualize the graph and omni.debug.draw is not available try: - import omni.isaac.debug_draw._debug_draw as omni_debug_draw + import isaacsim.util.debug_draw._debug_draw as omni_debug_draw self.draw = omni_debug_draw.acquire_debug_draw_interface() except ImportError: diff --git a/omniverse/standalone/viplanner_demo.py b/omniverse/standalone/viplanner_demo.py index 6af4521..cd1d274 100644 --- a/omniverse/standalone/viplanner_demo.py +++ b/omniverse/standalone/viplanner_demo.py @@ -13,7 +13,7 @@ import argparse # omni-isaac-lab -from omni.isaac.lab.app import AppLauncher +from isaaclab.app import AppLauncher # add argparse arguments parser = argparse.ArgumentParser(description="This script demonstrates how to use the camera sensor.") @@ -28,16 +28,19 @@ args_cli = parser.parse_args() args_cli.enable_cameras = True - +args_cli.no_vsync = True +args_cli.fixed_time_step = True +args_cli.async_rendering = True # launch omniverse app app_launcher = AppLauncher(args_cli) simulation_app = app_launcher.app + """Rest everything follows.""" -import omni.isaac.core.utils.prims as prim_utils +import isaacsim.core.utils.prims as prim_utils import torch -from omni.isaac.core.objects import VisualCuboid -from omni.isaac.lab.envs import ManagerBasedRLEnv +from isaacsim.core.api.objects import VisualCuboid +from isaaclab.envs import ManagerBasedRLEnv from omni.viplanner.config import ( ViPlannerCarlaCfg, ViPlannerMatterportCfg, @@ -98,7 +101,7 @@ def main(): goal_pos = prim_utils.get_prim_at_path("/World/goal").GetAttribute("xformOp:translate") # pause the simulator - # env.sim.pause() + #env.sim.pause() # load viplanner viplanner = VIPlannerAlgo(model_dir=args_cli.model_dir, device=env.device) @@ -112,6 +115,7 @@ def main(): # Simulate physics while simulation_app.is_running(): + print("Cam position:", obs["planner_transform"]["cam_position"]) with torch.inference_mode(): # If simulation is paused, then skip. if not env.sim.is_playing(): @@ -119,7 +123,16 @@ def main(): continue obs = env.step(action=paths.view(paths.shape[0], -1))[0] + robot_pos = env.scene["robot"].data.root_pos_w + robot_rot = env.scene["robot"].data.root_quat_w + cam_offset = torch.tensor([0.510, 0.0, 0.015], device=env.device) + cam_position = robot_pos + cam_offset + cam_orientation = robot_rot # 假设相机朝向与 base 保持一致 + + # 替换 obs 中的感知参考帧 + obs["planner_transform"]["cam_position"] = cam_position + obs["planner_transform"]["cam_orientation"] = cam_orientation # apply planner goals = torch.tensor(goal_pos.Get(), device=env.device).repeat(env.num_envs, 1) if torch.any( @@ -142,6 +155,7 @@ def main(): paths, obs["planner_transform"]["cam_position"], obs["planner_transform"]["cam_orientation"] ) + # draw path viplanner.debug_draw(paths, fear, goals) diff --git a/ros/planner/config/anymal_mount.yaml b/ros/planner/config/anymal_mount.yaml index 9c92cda..aab4c9e 100644 --- a/ros/planner/config/anymal_mount.yaml +++ b/ros/planner/config/anymal_mount.yaml @@ -7,22 +7,22 @@ overlap_ratio_thres: 0.80 depth_zero_ratio_thres: 0.6 # network model model_save: models -m2f_model_path: models/sem_model.pth -m2f_cfg_file: /root/git/mmdetection/configs/mask2former/mask2former_r50_8xb2-lsj-50e_coco-panoptic.py +m2f_model_path: /workspace/vip_ws/src/viplanner/ros/planner/models/mask2former_r50_8xb2-lsj-50e_coco-panoptic_20230118_125535-54df384a.pth +m2f_cfg_file: /workspace/mmdetection/configs/mask2former/mask2former_r50_8xb2-lsj-50e_coco-panoptic.py # ros topics -depth_topic: /depth_cam_mounted_front/depth/image_rect_raw -depth_info_topic: /depth_cam_mounted_front/depth/camera_info -rgb_topic: /depth_cam_mounted_front/color/image_raw/compressed -rgb_info_topic: /depth_cam_mounted_front/color/camera_info -mount_cam_frame: wide_angle_camera_rear_camera_parent # mounted camera is not part of the TF tree, specify its frame here +depth_topic: /camera/depth/image_rect_raw +depth_info_topic: /camera/depth/camera_info +rgb_topic: /camera/color/image_raw/compressed +rgb_info_topic: /camera/color/camera_info +mount_cam_frame: camera_link # mounted camera is not part of the TF tree, specify its frame here goal_topic: /mp_waypoint path_topic: /viplanner/path m2f_timer_topic: /viplanner/m2f_timer depth_uint_type: True compressed: True # frame ids -robot_id: base_inverted # also adjust in path_follower.launch -world_id: odom +robot_id: camera_link # also adjust in path_follower.launch +world_id: camera_init # fear reaction is_fear_act: False buffer_size: 3 diff --git a/ros/planner/launch/viplanner.launch b/ros/planner/launch/viplanner.launch index 79808bc..221efcd 100644 --- a/ros/planner/launch/viplanner.launch +++ b/ros/planner/launch/viplanner.launch @@ -20,5 +20,7 @@ + + diff --git a/ros/waypoint_rviz_plugin/src/waypoint_tool.cpp b/ros/waypoint_rviz_plugin/src/waypoint_tool.cpp index a0496c8..e0ec9b3 100644 --- a/ros/waypoint_rviz_plugin/src/waypoint_tool.cpp +++ b/ros/waypoint_rviz_plugin/src/waypoint_tool.cpp @@ -64,7 +64,7 @@ void WaypointTool::onPoseSet(double x, double y, double theta) pub_joy_.publish(joy); geometry_msgs::PointStamped waypoint_odom; - waypoint_odom.header.frame_id = "odom"; + waypoint_odom.header.frame_id = "camera_init"; waypoint_odom.header.stamp = joy.header.stamp; waypoint_odom.point.x = x; waypoint_odom.point.y = y; @@ -76,10 +76,10 @@ void WaypointTool::onPoseSet(double x, double y, double theta) // Wait for the transform to become available try { - geometry_msgs::TransformStamped transform = tf_buffer.lookupTransform("map", "base", ros::Time(0)); + geometry_msgs::TransformStamped transform = tf_buffer.lookupTransform("camera_init", "camera_link", ros::Time(0)); geometry_msgs::PointStamped waypoint_map; tf2::doTransform(waypoint_odom, waypoint_map, transform); - waypoint_map.header.frame_id = "map"; + waypoint_map.header.frame_id = "camera_init"; waypoint_map.header.stamp = ros::Time::now(); // Print out the transformed point coordinates