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
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()
React
Typescript
Django
Laravel
Facebook
Microsoft
Google
Alibaba
D3
Tencent