feat: ros1 odometry graph

[kisisjrlly]

I have created a new ROSOdometry OmniGraph, which uses omni.isaac.core_nodes.IsaacComputeOdometry to get the drone's current odometry and publishes it to ROS1 Odometry topic using omni.isaac.ros_bridge.ROSPublishOdometry, but the Odometry message doesn't change as i use command rostopic echo /odom to observe, can anyone help me briefly analyze the reason? The test code and the results are shown as follows: ''' import torch import hydra from omegaconf import OmegaConf from omni_drones import CONFIG_PATH, init_simulation_app

@hydra.main(version_base=None, config_path=".", config_name="demo") def main(cfg): OmegaConf.resolve(cfg) simulation_app = init_simulation_app(cfg) print(OmegaConf.to_yaml(cfg))

import omni_drones.utils.scene as scene_utils
from omni.isaac.core.simulation_context import SimulationContext
from omni_drones.controllers import LeePositionController
from omni_drones.robots.drone import MultirotorBase
from omni_drones.utils.torch import euler_to_quaternion
from omni_drones.sensors.camera import Camera, PinholeCameraCfg
import dataclasses
from omni.isaac.core.utils import extensions
# enable ROS bridge extension
extensions.enable_extension("omni.isaac.ros_bridge")

sim = SimulationContext(
    stage_units_in_meters=1.0,
    physics_dt=cfg.sim.dt,
    rendering_dt=cfg.sim.dt,
    sim_params=cfg.sim,
    backend="torch",
    device=cfg.sim.device,
)
n = 1

drone_cls = MultirotorBase.REGISTRY[cfg.drone_model]
drone = drone_cls()

translations = torch.zeros(n, 3)
translations[:, 1] = torch.arange(n)
translations[:, 2] = 0.5
drone.spawn(translations=translations)

import omni.graph.core as og
# from pxr import Gf, Usd, UsdGeom
import usdrt.Sdf
camera_keys = og.Controller.Keys
odom_keys = og.Controller.Keys

drone_odom_primpath = "/World/envs/env_0/Hummingbird_0/ODOMGRAPH"
drone_baselink_path = "/World/envs/env_0/Hummingbird_0/base_link"

(drone_ros_odom_graph, _, _, _) = og.Controller.edit(
{
    "graph_path": drone_odom_primpath,
    "evaluator_name": "execution",
},
{
    odom_keys.CREATE_NODES: [
        ("OnTick", "omni.graph.action.OnTick"),
        ("ReadSimTime", "omni.isaac.core_nodes.IsaacReadSimulationTime"),
        ("computeOdom", "omni.isaac.core_nodes.IsaacComputeOdometry"),
        ("Ros1PublishOdometry", "omni.isaac.ros_bridge.ROS1PublishOdometry"),
    ],
    odom_keys.CONNECT: [
        ("OnTick.outputs:tick", "computeOdom.inputs:execIn"),
        ("computeOdom.outputs:execOut","Ros1PublishOdometry.inputs:execIn"),
        ("computeOdom.outputs:position","Ros1PublishOdometry.inputs:position"),
        ("computeOdom.outputs:orientation","Ros1PublishOdometry.inputs:orientation"),
        ("computeOdom.outputs:linearVelocity","Ros1PublishOdometry.inputs:linearVelocity"),
        ("computeOdom.outputs:angularVelocity","Ros1PublishOdometry.inputs:angularVelocity"),
        ("ReadSimTime.outputs:simulationTime","Ros1PublishOdometry.inputs:timeStamp"),
    ],
    odom_keys.SET_VALUES: [
        ("ReadSimTime.inputs:resetOnStop", False),
        ("ReadSimTime.inputs:swhFrameNumber", 0),
        ("Ros1PublishOdometry.inputs:nodeNamespace", ""),
        ("Ros1PublishOdometry.inputs:odomFrameId", "odom"),
        ("Ros1PublishOdometry.inputs:chassisFrameId", "base_link"),
        ("Ros1PublishOdometry.inputs:topicName", "odom"),
        ("Ros1PublishOdometry.inputs:queueSize", 1),
        ("computeOdom.inputs:chassisPrim", [usdrt.Sdf.Path(drone_baselink_path)]),
    ],
},
)

og.Controller.evaluate_sync(drone_ros_odom_graph)
scene_utils.design_scene()

sim.reset()
drone.initialize()

# let's fly a circular trajectory
radius = 1.5
omega = 1.
phase = torch.linspace(0, 2, n+1, device=sim.device)[:n]

def ref(t):
    _t = phase * torch.pi + t * omega
    pos = torch.stack([
        torch.cos(_t) * radius,
        torch.sin(_t) * radius,
        torch.ones(n, device=sim.device) * 1.5
    ], dim=-1)
    vel_xy = torch.stack([
        -torch.sin(_t) * radius * omega,
        torch.cos(_t) * radius * omega,
    ], dim=-1)
    yaw = torch.atan2(vel_xy[:, 1], vel_xy[:, 0])
    return pos, yaw

init_rpy = torch.zeros(n, 3, device=sim.device)
init_pos, init_rpy[:, 2] = ref(torch.tensor(0.0).to(sim.device))
init_rot = euler_to_quaternion(init_rpy)
init_vels = torch.zeros(n, 6, device=sim.device)

# create a position controller
# note: the controller is state-less (but holds its parameters)

controller = LeePositionController(g=9.81, uav_params=drone.params).to(sim.device)

def reset():
    drone._reset_idx(torch.tensor([0]))
    drone.set_world_poses(init_pos, init_rot)
    drone.set_velocities(init_vels)
    # flush the buffer so that the next getter invocation 
    # returns up-to-date values
    sim._physics_sim_view.flush() 

reset()
drone_state = drone.get_state()[..., :13].squeeze(0)

import time
# time.sleep(10)
from tqdm import tqdm
for i in tqdm(range(1000)):
    if sim.is_stopped():
        break
    if not sim.is_playing():
        sim.render()
        continue
    ref_pos, ref_yaw = ref((i % 1000)*cfg.sim.dt)
    action = controller(drone_state, target_pos=ref_pos, target_yaw=ref_yaw)
    drone.apply_action(action)
    sim.step(render=True)
    if i % 1000 == 0:
        reset()
    drone_state = drone.get_state()[..., :13].squeeze(0)
simulation_app.close()

if name == "main": main() '''