walk these ways bug fixed and realsense launch files with isaac compression nodes are added.

Go2Py/control/walk_these_ways.py

@@ -255,12 +255,12 @@ class WalkTheseWaysAgent:
         for i, policy_joint_name in enumerate(policy_joint_names):
             id = np.where([name == policy_joint_name for name in unitree_joint_names])[0][0]
             policy_to_unitree_map.append((i, id))
-        self.policy_to_unitree_map = np.array(policy_to_unitree_map)
+        self.policy_to_unitree_map = np.array(policy_to_unitree_map).astype(np.uint32)
 
         for i, unitree_joint_name in enumerate(unitree_joint_names):
             id = np.where([name == unitree_joint_name for name in policy_joint_names])[0][0]
             unitree_to_policy_map.append((i, id))
-        self.unitree_to_policy_map = np.array(unitree_to_policy_map)
+        self.unitree_to_policy_map = np.array(unitree_to_policy_map).astype(np.uint32)
 
         self.default_dof_pos = np.array(
             [
@@ -343,8 +343,8 @@ class WalkTheseWaysAgent:
         joint_state = self.robot.getJointStates()
         if joint_state is not None:
             self.gravity_vector = self.robot.getGravityInBody()
-            self.dof_pos = joint_state['q'][self.unitree_to_policy_map[:, 1]]
-            self.dof_vel = joint_state['dq'][self.unitree_to_policy_map[:, 1]]
+            self.dof_pos = np.array(joint_state['q'])[self.unitree_to_policy_map[:, 1]]
+            self.dof_vel = np.array(joint_state['dq'])[self.unitree_to_policy_map[:, 1]]
 
         if reset_timer:
             self.reset_gait_indices()

deploy/launch_files/realsense-h264.launch.py

@@ -0,0 +1,75 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+import launch
+from launch.actions import ExecuteProcess
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+
+
+def generate_launch_description():
+    # RealSense
+    # realsense_config_file_path = os.path.join(
+    #     get_package_share_directory('isaac_ros_h264_encoder'),
+    #     'config', 'realsense.yaml'
+    # )
+    realsense_config_file_path = 'realsense.yaml'
+    realsense_node = ComposableNode(
+        package='realsense2_camera',
+        plugin='realsense2_camera::RealSenseNodeFactory',
+        parameters=[realsense_config_file_path],
+        remappings=[
+            ('infra1/image_rect_raw', 'go2/realsense/left/image_rect_raw_mono'),
+            ('infra2/image_rect_raw', 'go2/realsense/right/image_rect_raw_mono'),
+            ('color/image_raw',       'go2/realsense/color/image_raw_mono'),
+            ('infra1/camera_info',    'go2/realsense/left/camera_info'),
+            ('infra2/camera_info',    'go2/realsense/right/camera_info'),
+            ('color/camera_info',     'go2/realsense/color/camera_info'),
+            ('depth/camera_info',     'go2/realsense/depth/camera_info'),
+            ('depth/color/points',     'go2/realsense/depth/color/points'),
+            ('depth/image_rect_raw',     'go2/realsense/depth/image_rect_raw'),
+            ('extrinsics/depth_to_accel',     'go2/realsense/extrinsics/depth_to_accel'),
+            ('extrinsics/depth_to_color',     'go2/realsense/extrinsics/depth_to_color'),
+            ('extrinsics/depth_to_gyro',     'go2/realsense/extrinsics/depth_to_gyro'),
+            ('imu',     'go2/realsense/imu'),
+
+        ]
+    )
+    color_format_converter_node = ComposableNode(
+        package='isaac_ros_image_proc',
+        plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode',
+        name='image_format_node_color',
+        parameters=[{
+                'encoding_desired': 'rgb8',
+        }],
+        remappings=[
+            ('image_raw', 'go2/realsense/color/image_raw_mono'),
+            ('image', 'go2/realsense/color/image_raw')]
+    )
+
+    color_encoder_node = ComposableNode(
+        package='isaac_ros_h264_encoder',
+        plugin='nvidia::isaac_ros::h264_encoder::EncoderNode',
+        name='color_encoder_node',
+        parameters=[{
+            'input_width': 640,
+            'input_height': 480,
+        }],
+        remappings=[
+            ('image_raw', 'go2/realsense/color/image_raw_mono'),
+            ('image_compressed', 'go2/realsense/color/image_raw_compressed')]
+    )
+
+    container = ComposableNodeContainer(
+        name='encoder_container',
+        namespace='encoder',
+        package='rclcpp_components',
+        executable='component_container_mt',
+        composable_node_descriptions=[realsense_node,
+                                      color_format_converter_node,
+                                      color_encoder_node],
+        output='screen'
+    )
+
+    return (launch.LaunchDescription([container]))

deploy/launch_files/realsense.yaml

@@ -0,0 +1,36 @@
+device_type: ''
+serial_no: ''
+usb_port_id: ''
+
+rgb_camera:
+  profile: '640x480x60'
+color_qos: "RELIABILITY_QOS_POLICY"
+
+depth_module:
+  profile: '640x480x15'
+  emitter_enabled: 1
+  emitter_on_off: false
+depth_qos: "SENSOR_DATA"
+depth_info_qos: "SYSTEM_DEFAULT"
+
+infra_qos: "SENSOR_DATA"
+
+enable_accel: true
+enable_color: true
+enable_depth: true
+enable_gyro: true
+enable_infra1: false
+enable_infra2: false
+
+pointcloud:
+  enable: true
+
+pointcloud_texture_index: 0
+pointcloud_texture_stream: RS2_STREAM_ANY
+
+enable_sync: true
+align_depth: false
+
+unite_imu_method: 2
+gyro_fps: 400
+accel_fps: 200

docker/Dockerfile.go2py

@@ -18,4 +18,6 @@ RUN --mount=type=cache,target=/var/cache/apt \
     --mount=type=bind,source=scripts/install-unitree-ros2.sh,target=/tmp/install-unitree-ros2.sh \
     bash /tmp/install-unitree-ros2.sh
 
+RUN apt install -y ros-humble-isaac-ros-h264-decoder ros-humble-isaac-ros-h264-encoder ros-humble-realsense2-camera
+
 USER $USERNAME