From 1d1228bb177ede273fe6818b12c64ab25d6c6f2a Mon Sep 17 00:00:00 2001 From: john Date: Tue, 21 May 2024 18:03:06 +0900 Subject: [PATCH] Add base_link TF --- .../stride/simulator/backends/ros2_backend.py | 67 +++++++++++++++++++ .../vehicles/quadrupedrobot/quadrupedrobot.py | 2 + .../stride/simulator/vehicles/vehicle.py | 1 + 3 files changed, 70 insertions(+) diff --git a/exts/stride.simulator/stride/simulator/backends/ros2_backend.py b/exts/stride.simulator/stride/simulator/backends/ros2_backend.py index 4567bb2..86d68a7 100644 --- a/exts/stride.simulator/stride/simulator/backends/ros2_backend.py +++ b/exts/stride.simulator/stride/simulator/backends/ros2_backend.py @@ -3,6 +3,7 @@ import numpy as np import struct +import rclpy.logging from stride.simulator.backends.backend import Backend from omni.isaac.core.utils.extensions import disable_extension, enable_extension @@ -26,7 +27,11 @@ PoseStamped, TwistStamped, AccelStamped, + TransformStamped, ) +from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster +from tf2_ros.transform_broadcaster import TransformBroadcaster + # set environment variable to use ROS2 os.environ["RMW_IMPLEMENTATION"] = "rmw_cyclonedds_cpp" @@ -68,6 +73,7 @@ def __init__(self, node_name: str): # Start the actual ROS2 setup here if not rclpy.ok(): # Check if the ROS2 context is already initialized rclpy.init() + print("ROS2 context initialized") self.node = rclpy.create_node(node_name) # Create publishers for the state of the vehicle in ENU @@ -90,6 +96,53 @@ def __init__(self, node_name: str): PointCloud2, node_name + "/sensors/points", 10 ) + # Create a static transform broadcaster for the base_link to map transform + self.tf_static_broadcaster = StaticTransformBroadcaster(self.node) + + #Initialize the transform from base_link to map + self.send_static_transform() + + # Create a transform broadcaster for the base_link to map transform + self.tf_broadcaster = TransformBroadcaster(self.node) + + def send_static_transform(self): + """ + send the static transform from base_link to vehicle_link + """ + t = TransformStamped() + t.header.stamp = self.node.get_clock().now().to_msg() + t.header.frame_id = 'base_link' + t.child_frame_id = 'base_link_frd' + # TODO(Jeong) : if the state update function is implemented, the position and orientation should be updated + # Converts from FLU to FRD + t.transform.translation.x = 0.0 + t.transform.translation.y = 0.0 + t.transform.translation.z = 0.0 + t.transform.rotation.x = 1.0 + t.transform.rotation.y = 0.0 + t.transform.rotation.z = 0.0 + t.transform.rotation.w = 0.0 + + self.tf_static_broadcaster.sendTransform(t) + + # Create the transform from map, i.e inertial frame (ROS standard) to map_ned (standard in airborn or marine vehicles) + t = TransformStamped() + t.header.stamp = self.node.get_clock().now().to_msg() + t.header.frame_id = "map" + t.child_frame_id = "map_ned" + + # Converts ENU to NED + t.transform.translation.x = 0.0 + t.transform.translation.y = 0.0 + t.transform.translation.z = 0.0 + t.transform.rotation.x = -0.7071068 + t.transform.rotation.y = -0.7071068 + t.transform.rotation.z = 0.0 + t.transform.rotation.w = 0.0 + + self.tf_static_broadcaster.sendTransform(t) + + def update(self, dt: float): """ Method that when implemented, should be used to update the state of the backend and the information being @@ -253,3 +306,17 @@ def update_state(self, state): self.twist_pub.publish(twist) self.twist_inertial_pub.publish(twist_inertial) self.accel_pub.publish(accel) + + t = TransformStamped() + t.header.stamp = self.node.get_clock().now().to_msg() + t.header.frame_id = "map" + t.child_frame_id = "base_link" + t.transform.translation.x = state.position[0] + t.transform.translation.y = state.position[1] + t.transform.translation.z = state.position[2] + t.transform.rotation.x = state.attitude[0] + t.transform.rotation.y = state.attitude[1] + t.transform.rotation.z = state.attitude[2] + t.transform.rotation.w = state.attitude[3] + self.tf_broadcaster.sendTransform(t) + diff --git a/exts/stride.simulator/stride/simulator/vehicles/quadrupedrobot/quadrupedrobot.py b/exts/stride.simulator/stride/simulator/vehicles/quadrupedrobot/quadrupedrobot.py index e3e39f6..f91c158 100644 --- a/exts/stride.simulator/stride/simulator/vehicles/quadrupedrobot/quadrupedrobot.py +++ b/exts/stride.simulator/stride/simulator/vehicles/quadrupedrobot/quadrupedrobot.py @@ -293,3 +293,5 @@ def _hit_report_callback(self, hit): if "/World/layout/GroundPlane" in current_hit_body: self._query_info.append(hit.distance) return True + + # TODO(Jeong) : Implement the function for the joint positions and velocities related to the quadrupedrobot \ No newline at end of file diff --git a/exts/stride.simulator/stride/simulator/vehicles/vehicle.py b/exts/stride.simulator/stride/simulator/vehicles/vehicle.py index 3449f1b..9adcfb3 100644 --- a/exts/stride.simulator/stride/simulator/vehicles/vehicle.py +++ b/exts/stride.simulator/stride/simulator/vehicles/vehicle.py @@ -187,6 +187,7 @@ def update_state(self, dt: float): # Get the body frame interface of the vehicle # (this will be the frame used to get the position, orientation, etc.) body = self._world.dc_interface.get_rigid_body(self._stage_prefix + "/base") + # TODO(Jeong) : Implement the following methods for the joint positions and velocities # Get the current position and orientation in the inertial frame pose = self._world.dc_interface.get_rigid_body_pose(body)