From d686d155dd5878a448fd253966bdc8427a5b4734 Mon Sep 17 00:00:00 2001 From: Ayush Ghosh Date: Tue, 4 Jun 2024 14:50:19 -0700 Subject: [PATCH] Updates for Isaac Sim 4.0.0 --- CHANGELOG.md | 15 + foxy_ws/fastdds.xml | 2 +- foxy_ws/src/custom_message/package.xml | 13 +- .../src/isaac_ros2_messages/CMakeLists.txt | 8 +- foxy_ws/src/isaac_ros2_messages/package.xml | 8 +- .../srv/GetPrimAttribute.srv | 7 + .../srv/GetPrimAttributes.srv | 7 + .../src/isaac_ros2_messages/srv/GetPrims.srv | 6 + .../srv/SetPrimAttribute.srv | 6 + foxy_ws/src/isaac_tutorials/CMakeLists.txt | 1 + foxy_ws/src/isaac_tutorials/package.xml | 7 +- .../isaac_tutorials/rviz2/carter_stereo.rviz | 42 +- .../scripts/ros2_ackermann_publisher.py | 56 +++ foxy_ws/src/isaacsim/CMakeLists.txt | 18 + .../isaacsim/launch/run_isaacsim.launch.py | 90 ++++ foxy_ws/src/isaacsim/package.xml | 30 ++ .../isaacsim/scripts/open_isaacsim_stage.py | 84 ++++ foxy_ws/src/isaacsim/scripts/run_isaacsim.py | 184 +++++++ .../launch/carter_navigation.launch.py | 4 +- .../carter_navigation_individual.launch.py | 2 +- .../carter_navigation_isaacsim.launch.py | 125 +++++ ...robot_carter_navigation_hospital.launch.py | 4 +- ...e_robot_carter_navigation_office.launch.py | 4 +- .../navigation/carter_navigation/package.xml | 6 +- .../params/carter_navigation_params.yaml | 6 +- ...ulti_robot_carter_navigation_params_1.yaml | 6 +- ...ulti_robot_carter_navigation_params_2.yaml | 6 +- ...ulti_robot_carter_navigation_params_3.yaml | 6 +- ...ulti_robot_carter_navigation_params_1.yaml | 6 +- ...ulti_robot_carter_navigation_params_2.yaml | 6 +- ...ulti_robot_carter_navigation_params_3.yaml | 6 +- .../isaac_ros_navigation_goal.launch.py | 2 +- .../isaac_ros_navigation_goal/package.xml | 6 +- humble_ws/fastdds.xml | 2 +- humble_ws/src/custom_message/package.xml | 13 +- .../src/isaac_ros2_messages/CMakeLists.txt | 8 +- humble_ws/src/isaac_ros2_messages/package.xml | 8 +- .../srv/GetPrimAttribute.srv | 7 + .../srv/GetPrimAttributes.srv | 7 + .../src/isaac_ros2_messages/srv/GetPrims.srv | 6 + .../srv/SetPrimAttribute.srv | 6 + humble_ws/src/isaac_tutorials/CMakeLists.txt | 1 + humble_ws/src/isaac_tutorials/package.xml | 8 +- .../isaac_tutorials/rviz2/carter_stereo.rviz | 42 +- .../scripts/ros2_ackermann_publisher.py | 56 +++ humble_ws/src/isaacsim/CMakeLists.txt | 18 + .../isaacsim/launch/run_isaacsim.launch.py | 90 ++++ humble_ws/src/isaacsim/package.xml | 30 ++ .../isaacsim/scripts/open_isaacsim_stage.py | 84 ++++ .../src/isaacsim/scripts/run_isaacsim.py | 184 +++++++ .../launch/carter_navigation.launch.py | 4 +- .../carter_navigation_individual.launch.py | 2 +- .../carter_navigation_isaacsim.launch.py | 125 +++++ ...robot_carter_navigation_hospital.launch.py | 4 +- ...e_robot_carter_navigation_office.launch.py | 4 +- .../navigation/carter_navigation/package.xml | 6 +- .../params/carter_navigation_params.yaml | 6 +- ...ulti_robot_carter_navigation_params_1.yaml | 6 +- ...ulti_robot_carter_navigation_params_2.yaml | 6 +- ...ulti_robot_carter_navigation_params_3.yaml | 6 +- ...ulti_robot_carter_navigation_params_1.yaml | 6 +- ...ulti_robot_carter_navigation_params_2.yaml | 6 +- ...ulti_robot_carter_navigation_params_3.yaml | 6 +- .../rviz2/carter_navigation.rviz | 40 +- .../isaac_ros_navigation_goal.launch.py | 2 +- .../isaac_ros_navigation_goal/package.xml | 6 +- noetic_ws/src/cortex_control/package.xml | 14 +- .../src/cortex_control_franka/package.xml | 15 +- noetic_ws/src/isaac_moveit/package.xml | 6 +- noetic_ws/src/isaac_ros_messages/package.xml | 6 +- noetic_ws/src/isaac_tutorials/CMakeLists.txt | 2 + noetic_ws/src/isaac_tutorials/package.xml | 7 +- .../scripts/ros_ackermann_publisher.py | 38 ++ noetic_ws/src/isaac_vins/CMakeLists.txt | 207 -------- .../config/isaac_a1/isaac_left.yaml | 16 - .../config/isaac_a1/isaac_right.yaml | 16 - .../config/isaac_a1/vins_fusion_isaac_a1.yaml | 71 --- .../isaac_vins/launch/isaac_a1_vins.launch | 18 - noetic_ws/src/isaac_vins/package.xml | 38 -- .../rviz/isaac_vins_rviz_config.rviz | 468 ------------------ .../src/navigation/carter_2dnav/package.xml | 6 +- .../navigation/carter_description/package.xml | 6 +- .../isaac_ros_navigation_goal/package.xml | 6 +- 83 files changed, 1507 insertions(+), 1017 deletions(-) create mode 100644 CHANGELOG.md create mode 100644 foxy_ws/src/isaac_ros2_messages/srv/GetPrimAttribute.srv create mode 100644 foxy_ws/src/isaac_ros2_messages/srv/GetPrimAttributes.srv create mode 100644 foxy_ws/src/isaac_ros2_messages/srv/GetPrims.srv create mode 100644 foxy_ws/src/isaac_ros2_messages/srv/SetPrimAttribute.srv create mode 100644 foxy_ws/src/isaac_tutorials/scripts/ros2_ackermann_publisher.py create mode 100644 foxy_ws/src/isaacsim/CMakeLists.txt create mode 100644 foxy_ws/src/isaacsim/launch/run_isaacsim.launch.py create mode 100644 foxy_ws/src/isaacsim/package.xml create mode 100644 foxy_ws/src/isaacsim/scripts/open_isaacsim_stage.py create mode 100755 foxy_ws/src/isaacsim/scripts/run_isaacsim.py create mode 100644 foxy_ws/src/navigation/carter_navigation/launch/carter_navigation_isaacsim.launch.py create mode 100644 humble_ws/src/isaac_ros2_messages/srv/GetPrimAttribute.srv create mode 100644 humble_ws/src/isaac_ros2_messages/srv/GetPrimAttributes.srv create mode 100644 humble_ws/src/isaac_ros2_messages/srv/GetPrims.srv create mode 100644 humble_ws/src/isaac_ros2_messages/srv/SetPrimAttribute.srv create mode 100644 humble_ws/src/isaac_tutorials/scripts/ros2_ackermann_publisher.py create mode 100644 humble_ws/src/isaacsim/CMakeLists.txt create mode 100644 humble_ws/src/isaacsim/launch/run_isaacsim.launch.py create mode 100644 humble_ws/src/isaacsim/package.xml create mode 100644 humble_ws/src/isaacsim/scripts/open_isaacsim_stage.py create mode 100755 humble_ws/src/isaacsim/scripts/run_isaacsim.py create mode 100644 humble_ws/src/navigation/carter_navigation/launch/carter_navigation_isaacsim.launch.py create mode 100755 noetic_ws/src/isaac_tutorials/scripts/ros_ackermann_publisher.py delete mode 100644 noetic_ws/src/isaac_vins/CMakeLists.txt delete mode 100644 noetic_ws/src/isaac_vins/config/isaac_a1/isaac_left.yaml delete mode 100644 noetic_ws/src/isaac_vins/config/isaac_a1/isaac_right.yaml delete mode 100644 noetic_ws/src/isaac_vins/config/isaac_a1/vins_fusion_isaac_a1.yaml delete mode 100644 noetic_ws/src/isaac_vins/launch/isaac_a1_vins.launch delete mode 100644 noetic_ws/src/isaac_vins/package.xml delete mode 100644 noetic_ws/src/isaac_vins/rviz/isaac_vins_rviz_config.rviz diff --git a/CHANGELOG.md b/CHANGELOG.md new file mode 100644 index 0000000..fcba158 --- /dev/null +++ b/CHANGELOG.md @@ -0,0 +1,15 @@ +# Changelog + +## [1.0.0] - 2024-05-28 + +### Added +- Ackermann publisher script for Ackermann Steering tutorial [Noetic, Foxy, Humble] +- New `isaacsim` package to enable running Isaac Sim as a ROS2 node or from a ROS2 launch file! [Foxy, Humble] +- `isaac_ros2_messages` service interfaces for listing prims and manipulate their attributes [Foxy, Humble] + +### Removed +- Removed support for quadruped VINS Fusion example [Noetic] + +## [0.1.0] - 2023-12-18 +### Added +- Noetic, Foxy, Humble workspaces for Isaac Sim 2023.1.1 diff --git a/foxy_ws/fastdds.xml b/foxy_ws/fastdds.xml index 2782469..83bcc53 100644 --- a/foxy_ws/fastdds.xml +++ b/foxy_ws/fastdds.xml @@ -1,6 +1,6 @@ -Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. +Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. NVIDIA CORPORATION and its licensors retain all intellectual property and proprietary rights in and to this software, related documentation and any modifications thereto. Any use, reproduction, disclosure or diff --git a/foxy_ws/src/custom_message/package.xml b/foxy_ws/src/custom_message/package.xml index 2fb3b7f..74c3230 100644 --- a/foxy_ws/src/custom_message/package.xml +++ b/foxy_ws/src/custom_message/package.xml @@ -4,8 +4,17 @@ custom_message 0.0.0 Custom Message Sample Package - isaac sim - TODO: License declaration + Isaac Sim + + Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. + NVIDIA CORPORATION and its licensors retain all intellectual property + and proprietary rights in and to this software, related documentation + and any modifications thereto. Any use, reproduction, disclosure or + distribution of this software and related documentation without an express + license agreement from NVIDIA CORPORATION is strictly prohibited. + + https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html + https://forums.developer.nvidia.com/c/omniverse/simulation/69 ament_cmake diff --git a/foxy_ws/src/isaac_ros2_messages/CMakeLists.txt b/foxy_ws/src/isaac_ros2_messages/CMakeLists.txt index 7eb4e12..a61aebf 100644 --- a/foxy_ws/src/isaac_ros2_messages/CMakeLists.txt +++ b/foxy_ws/src/isaac_ros2_messages/CMakeLists.txt @@ -47,7 +47,11 @@ find_package(geometry_msgs REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "srv/IsaacPose.srv" + "srv/GetPrims.srv" + "srv/GetPrimAttributes.srv" + "srv/GetPrimAttribute.srv" + "srv/SetPrimAttribute.srv" DEPENDENCIES builtin_interfaces std_msgs geometry_msgs - ) +) - ament_package() +ament_package() diff --git a/foxy_ws/src/isaac_ros2_messages/package.xml b/foxy_ws/src/isaac_ros2_messages/package.xml index d825753..fdd3557 100644 --- a/foxy_ws/src/isaac_ros2_messages/package.xml +++ b/foxy_ws/src/isaac_ros2_messages/package.xml @@ -2,11 +2,11 @@ isaac_ros2_messages - 0.1.0 + 0.2.0 ROS2 messages for isaac ros2 bridge - isaac sim + Isaac Sim - Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. + Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. NVIDIA CORPORATION and its licensors retain all intellectual property and proprietary rights in and to this software, related documentation and any modifications thereto. Any use, reproduction, disclosure or @@ -14,7 +14,7 @@ license agreement from NVIDIA CORPORATION is strictly prohibited. https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html - https://forums.developer.nvidia.com/c/agx-autonomous-machines/isaac/simulation + https://forums.developer.nvidia.com/c/omniverse/simulation/69 ament_cmake diff --git a/foxy_ws/src/isaac_ros2_messages/srv/GetPrimAttribute.srv b/foxy_ws/src/isaac_ros2_messages/srv/GetPrimAttribute.srv new file mode 100644 index 0000000..10adbc2 --- /dev/null +++ b/foxy_ws/src/isaac_ros2_messages/srv/GetPrimAttribute.srv @@ -0,0 +1,7 @@ +string path # prim path +string attribute # attribute name +--- +string value # attribute value (as JSON) +string type # attribute type +bool success # indicate a successful execution of the service +string message # informational, e.g. for error messages \ No newline at end of file diff --git a/foxy_ws/src/isaac_ros2_messages/srv/GetPrimAttributes.srv b/foxy_ws/src/isaac_ros2_messages/srv/GetPrimAttributes.srv new file mode 100644 index 0000000..26eb58a --- /dev/null +++ b/foxy_ws/src/isaac_ros2_messages/srv/GetPrimAttributes.srv @@ -0,0 +1,7 @@ +string path # prim path +--- +string[] names # list of attribute base names (name used to Get or Set an attribute) +string[] displays # list of attribute display names (name displayed in Property tab) +string[] types # list of attribute data types +bool success # indicate a successful execution of the service +string message # informational, e.g. for error messages \ No newline at end of file diff --git a/foxy_ws/src/isaac_ros2_messages/srv/GetPrims.srv b/foxy_ws/src/isaac_ros2_messages/srv/GetPrims.srv new file mode 100644 index 0000000..7afbab6 --- /dev/null +++ b/foxy_ws/src/isaac_ros2_messages/srv/GetPrims.srv @@ -0,0 +1,6 @@ +string path # get prims at path +--- +string[] paths # list of prim paths +string[] types # prim type names +bool success # indicate a successful execution of the service +string message # informational, e.g. for error messages \ No newline at end of file diff --git a/foxy_ws/src/isaac_ros2_messages/srv/SetPrimAttribute.srv b/foxy_ws/src/isaac_ros2_messages/srv/SetPrimAttribute.srv new file mode 100644 index 0000000..c70a5ee --- /dev/null +++ b/foxy_ws/src/isaac_ros2_messages/srv/SetPrimAttribute.srv @@ -0,0 +1,6 @@ +string path # prim path +string attribute # attribute name +string value # attribute value (as JSON) +--- +bool success # indicate a successful execution of the service +string message # informational, e.g. for error messages \ No newline at end of file diff --git a/foxy_ws/src/isaac_tutorials/CMakeLists.txt b/foxy_ws/src/isaac_tutorials/CMakeLists.txt index 76e0d50..4d17ff7 100644 --- a/foxy_ws/src/isaac_tutorials/CMakeLists.txt +++ b/foxy_ws/src/isaac_tutorials/CMakeLists.txt @@ -12,6 +12,7 @@ install(DIRECTORY # Install Python executables install(PROGRAMS scripts/ros2_publisher.py + scripts/ros2_ackermann_publisher.py DESTINATION lib/${PROJECT_NAME} ) ament_package() diff --git a/foxy_ws/src/isaac_tutorials/package.xml b/foxy_ws/src/isaac_tutorials/package.xml index 3314cc6..a6b5c97 100644 --- a/foxy_ws/src/isaac_tutorials/package.xml +++ b/foxy_ws/src/isaac_tutorials/package.xml @@ -7,9 +7,9 @@ The isaac_tutorials package - isaac sim + Isaac Sim - Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. + Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. NVIDIA CORPORATION and its licensors retain all intellectual property and proprietary rights in and to this software, related documentation and any modifications thereto. Any use, reproduction, disclosure or @@ -17,7 +17,7 @@ license agreement from NVIDIA CORPORATION is strictly prohibited. https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html - https://forums.developer.nvidia.com/c/agx-autonomous-machines/isaac/simulation + https://forums.developer.nvidia.com/c/omniverse/simulation/69 ament_cmake @@ -25,6 +25,7 @@ launch_ros joint_state_publisher + ackermann_msgs rviz2 turtlebot3 diff --git a/foxy_ws/src/isaac_tutorials/rviz2/carter_stereo.rviz b/foxy_ws/src/isaac_tutorials/rviz2/carter_stereo.rviz index 0f16a3a..23355ad 100644 --- a/foxy_ws/src/isaac_tutorials/rviz2/carter_stereo.rviz +++ b/foxy_ws/src/isaac_tutorials/rviz2/carter_stereo.rviz @@ -57,17 +57,17 @@ Visualization Manager: Value: true front_3d_lidar: Value: true - front_stereo_camera:imu: + front_stereo_camera_imu: Value: true - front_stereo_camera:left_rgb: + front_stereo_camera_left_optical: Value: true - front_stereo_camera:right_rgb: + front_stereo_camera_right_optical: Value: true - left_stereo_camera:imu: + left_stereo_camera_imu: Value: true - left_stereo_camera:left_rgb: + left_stereo_camera_left_optical: Value: true - left_stereo_camera:right_rgb: + left_stereo_camera_right_optical: Value: true odom: Value: true @@ -79,11 +79,11 @@ Visualization Manager: Value: true rear_stereo_camera:right_rgb: Value: true - right_stereo_camera:imu: + right_stereo_camera_imu: Value: true - right_stereo_camera:left_rgb: + right_stereo_camera_left_optical: Value: true - right_stereo_camera:right_rgb: + right_stereo_camera_right_optical: Value: true Marker Scale: 1 Name: TF @@ -99,17 +99,17 @@ Visualization Manager: {} front_3d_lidar: {} - front_stereo_camera:imu: + front_stereo_camera_imu: {} - front_stereo_camera:left_rgb: + front_stereo_camera_left_optical: {} - front_stereo_camera:right_rgb: + front_stereo_camera_right_optical: {} - left_stereo_camera:imu: + left_stereo_camera_imu: {} - left_stereo_camera:left_rgb: + left_stereo_camera_left_optical: {} - left_stereo_camera:right_rgb: + left_stereo_camera_right_optical: {} rear_2d_lidar: {} @@ -119,11 +119,11 @@ Visualization Manager: {} rear_stereo_camera:right_rgb: {} - right_stereo_camera:imu: + right_stereo_camera_imu: {} - right_stereo_camera:left_rgb: + right_stereo_camera_left_optical: {} - right_stereo_camera:right_rgb: + right_stereo_camera_right_optical: {} Update Interval: 0 Value: true @@ -177,7 +177,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /front_stereo_camera/left_rgb/image_raw + Value: /front_stereo_camera/left/image_raw Value: true - Class: rviz_default_plugins/Image Enabled: true @@ -191,7 +191,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /front_stereo_camera/right_rgb/image_raw + Value: /front_stereo_camera/right/image_raw Value: true - Alpha: 1 Autocompute Intensity Bounds: true @@ -222,7 +222,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /front_3d_lidar/point_cloud + Value: /front_3d_lidar/lidar_points Use Fixed Frame: true Use rainbow: true Value: true diff --git a/foxy_ws/src/isaac_tutorials/scripts/ros2_ackermann_publisher.py b/foxy_ws/src/isaac_tutorials/scripts/ros2_ackermann_publisher.py new file mode 100644 index 0000000..abcb420 --- /dev/null +++ b/foxy_ws/src/isaac_tutorials/scripts/ros2_ackermann_publisher.py @@ -0,0 +1,56 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2020-2024, NVIDIA CORPORATION. All rights reserved. +# +# NVIDIA CORPORATION and its licensors retain all intellectual property +# and proprietary rights in and to this software, related documentation +# and any modifications thereto. Any use, reproduction, disclosure or +# distribution of this software and related documentation without an express +# license agreement from NVIDIA CORPORATION is strictly prohibited. + +import rclpy +from rclpy.node import Node + +from ackermann_msgs.msg import AckermannDriveStamped +import numpy as np + +class MinimalPublisher(Node): + + def __init__(self): + super().__init__('test_ackermann') + self.publisher_ = self.create_publisher(AckermannDriveStamped, 'ackermann_cmd', 10) + timer_period = 0.05 # seconds + self.timer = self.create_timer(timer_period, self.timer_callback) + self.i = 0 + + def timer_callback(self): + msg = AckermannDriveStamped() + + # Only command forklift using acceleration and steering angle + degrees1 = np.arange(0, 60) + degrees2 = np.arange(-60, 0) + degrees = np.concatenate((degrees1, degrees1[::-1], degrees2[::-1], degrees2)) + msg.header.frame_id = "forklift" + msg.header.stamp = self.get_clock().now().to_msg() + msg.drive.steering_angle = 0.0174533 * (degrees[self.i % degrees.size]) + + msg.drive.acceleration = float(degrees[self.i % degrees.size]) + + self.publisher_.publish(msg) + self.i += 1 + + +def main(args=None): + + rclpy.init(args=args) + + minimal_publisher = MinimalPublisher() + + rclpy.spin(minimal_publisher) + + minimal_publisher.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/foxy_ws/src/isaacsim/CMakeLists.txt b/foxy_ws/src/isaacsim/CMakeLists.txt new file mode 100644 index 0000000..968b438 --- /dev/null +++ b/foxy_ws/src/isaacsim/CMakeLists.txt @@ -0,0 +1,18 @@ +cmake_minimum_required(VERSION 3.5) +project(isaacsim) + +find_package(ament_cmake REQUIRED) +find_package(rclpy REQUIRED) + +install(DIRECTORY + scripts + launch + DESTINATION share/${PROJECT_NAME}) + +# Install Python executables +install(PROGRAMS + scripts/run_isaacsim.py + scripts/open_isaacsim_stage.py + DESTINATION lib/${PROJECT_NAME} +) +ament_package() diff --git a/foxy_ws/src/isaacsim/launch/run_isaacsim.launch.py b/foxy_ws/src/isaacsim/launch/run_isaacsim.launch.py new file mode 100644 index 0000000..249b9d3 --- /dev/null +++ b/foxy_ws/src/isaacsim/launch/run_isaacsim.launch.py @@ -0,0 +1,90 @@ +## Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. +## NVIDIA CORPORATION and its licensors retain all intellectual property +## and proprietary rights in and to this software, related documentation +## and any modifications thereto. Any use, reproduction, disclosure or +## distribution of this software and related documentation without an express +## license agreement from NVIDIA CORPORATION is strictly prohibited. + +from launch import LaunchDescription +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument, ExecuteProcess, OpaqueFunction +from launch.substitutions import LaunchConfiguration, Command, PythonExpression +from launch.conditions import IfCondition +from launch_ros.substitutions import FindPackageShare + + +# Declare all launch arguments corresponding to the bash script options +launch_args = [ + DeclareLaunchArgument('version', default_value='4.0.0', description='Specify the version of Isaac Sim to use. Isaac Sim will be run from default install root folder for the specified version. Leave empty to use latest version of Isaac Sim.'), + + DeclareLaunchArgument('install_path', default_value='', description='If Isaac Sim is insalled in a non-default location, provide a specific path to Isaac Sim installation root folder. (If defined, "version" parameter will be ignored)'), + + DeclareLaunchArgument('use_internal_libs', default_value='false', description='Set to true if you wish to use internal ROS libraries shipped with Isaac Sim.'), + + DeclareLaunchArgument('dds_type', default_value='fastdds', description='Set to "fastdds" or "cyclonedds" (Cyclone only supported for ROS Humble) to run Isaac Sim with a specific dds type.'), + + DeclareLaunchArgument('gui', default_value='', description='Provide the path to a usd file to open it when starting Isaac Sim in standard gui mode. If left empty, Isaac Sim will open an empty stage in standard gui mode.'), + + DeclareLaunchArgument('standalone', default_value='', description='Provide the path to the python file to open it and start Isaac Sim in standalone workflow. If left empty, Isaac Sim will open an empty stage in standard Gui mode.'), + + DeclareLaunchArgument('play_sim_on_start', default_value='false', description='If enabled and Isaac Sim will start playing the scene after it is loaded. (Only applicable when in standard gui mode and loading a scene)'), + + DeclareLaunchArgument('ros_distro', default_value='foxy', description='Provide ROS version to use. Only Humble and Foxy is supported.'), + + DeclareLaunchArgument('ros_installation_path', default_value='', description='If ROS is installed in a non-default location (as in not under /opt/ros/), provide the path to your main setup.bash file for your ROS install. (/path/to/custom/ros/install/setup.bash)'), + +] + +# List of parameters to check +parameters_to_check = [ + 'version', 'install_path', 'use_internal_libs', 'dds_type', + 'standalone', 'play_sim_on_start', 'ros_distro', 'ros_installation_path', 'gui' +] + +def launch_setup(context): + + def add_parameter_argument(param_name, command_list): + param_value = LaunchConfiguration(param_name).perform(context) # Here you'll get the runtime config value + + if param_value != '': + if param_value == 'true': + return command_list + ['--' + param_name.replace('_', '-')] + elif param_value == 'false': + return command_list + else: + return command_list + ['--' + param_name.replace('_', '-'), param_value,] + else: + return command_list + + bash_command = [] + + # Add parameters to the command list if they are set + for param in parameters_to_check: + bash_command = add_parameter_argument(param, bash_command) + + # Run isaac sim as a ROS2 node with default parameters. Parameters can be overridden here or via launch arguments from other launch files. + isaacsim_node = Node( + package='isaacsim', executable='run_isaacsim.py', + name='isaacsim', output="screen", + parameters=[{ + 'version': LaunchConfiguration('version'), + 'install_path': LaunchConfiguration('install_path'), + 'use_internal_libs': LaunchConfiguration('use_internal_libs'), + 'dds_type': LaunchConfiguration('dds_type'), + 'gui': LaunchConfiguration('gui'), + 'standalone': LaunchConfiguration('standalone'), + 'play_sim_on_start': LaunchConfiguration('play_sim_on_start'), + 'ros_distro': LaunchConfiguration('ros_distro'), + 'ros_installation_path': LaunchConfiguration('ros_installation_path') + }] + ) + return [isaacsim_node] + + +# Create and return the launch description with all declared arguments and the execute launch_setup +def generate_launch_description(): + opfunc = OpaqueFunction(function = launch_setup) + ld = LaunchDescription(launch_args) + ld.add_action(opfunc) + return ld diff --git a/foxy_ws/src/isaacsim/package.xml b/foxy_ws/src/isaacsim/package.xml new file mode 100644 index 0000000..d5f2f87 --- /dev/null +++ b/foxy_ws/src/isaacsim/package.xml @@ -0,0 +1,30 @@ + + + + isaacsim + 0.1.0 + + The isaacsim package that contains the script which can used to launch Isaac Sim as a ROS2 node from a launch file. + + + Isaac Sim + + Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. + NVIDIA CORPORATION and its licensors retain all intellectual property + and proprietary rights in and to this software, related documentation + and any modifications thereto. Any use, reproduction, disclosure or + distribution of this software and related documentation without an express + license agreement from NVIDIA CORPORATION is strictly prohibited. + + https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html + https://forums.developer.nvidia.com/c/omniverse/simulation/69 + + ament_cmake + + launch + launch_ros + + + ament_cmake + + diff --git a/foxy_ws/src/isaacsim/scripts/open_isaacsim_stage.py b/foxy_ws/src/isaacsim/scripts/open_isaacsim_stage.py new file mode 100644 index 0000000..2fdc67a --- /dev/null +++ b/foxy_ws/src/isaacsim/scripts/open_isaacsim_stage.py @@ -0,0 +1,84 @@ +"""Script to open a USD stage in Isaac Sim (in standard gui mode). This script along with its arguments are automatically passed to Isaac Sim via the ROS2 launch workflow""" + +import carb +import argparse +import omni.usd +import asyncio +import omni.client +import omni.kit.async_engine +import omni.timeline + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument('--path', type=str, required=True, help='The path to USD stage') + + # Add the --start-on-play option + # If --start-on-play is specified, it sets the value to True + parser.add_argument('--start-on-play', action='store_true', + help='If present, to true.') + + try: + options = parser.parse_args() + except Exception as e: + carb.log_error(str(e)) + return + + omni.kit.async_engine.run_coroutine(open_stage_async(options.path, options.start_on_play)) + +async def open_stage_async(path: str, start_on_play: bool): + timeline_interface = None + if start_on_play: + timeline_interface = omni.timeline.get_timeline_interface() + + async def _open_stage_internal(path): + is_stage_with_session = False + try: + import omni.kit.usd.layers as layers + live_session_name = layers.get_live_session_name_from_shared_link(path) + is_stage_with_session = live_session_name is not None + except Exception: + pass + + if is_stage_with_session: + # Try to open the stage with specified live session. + (success, error) = await layers.get_live_syncing().open_stage_with_live_session_async(path) + else: + # Otherwise, use normal stage open. + (success, error) = await omni.usd.get_context().open_stage_async(path) + + if not success: + carb.log_error(f"Failed to open stage {path}: {error}.") + else: + if timeline_interface is not None: + await omni.kit.app.get_app().next_update_async() + await omni.kit.app.get_app().next_update_async() + timeline_interface.play() + print("Stage loaded and simulation is playing.") + pass + result, _ = await omni.client.stat_async(path) + if result == omni.client.Result.OK: + await _open_stage_internal(path) + + return + + broken_url = omni.client.break_url(path) + if broken_url.scheme == 'omniverse': + # Attempt to connect to nucleus server before opening stage + try: + from omni.kit.widget.nucleus_connector import get_nucleus_connector + nucleus_connector = get_nucleus_connector() + except Exception: + carb.log_warn("Open stage: Could not import Nucleus connector.") + return + + server_url = omni.client.make_url(scheme='omniverse', host=broken_url.host) + nucleus_connector.connect( + broken_url.host, server_url, + on_success_fn=lambda *_: asyncio.ensure_future(_open_stage_internal(path)), + on_failed_fn=lambda *_: carb.log_error(f"Open stage: Failed to connect to server '{server_url}'.") + ) + else: + carb.log_warn(f"Open stage: Could not open non-existent url '{path}'.") + + +main() diff --git a/foxy_ws/src/isaacsim/scripts/run_isaacsim.py b/foxy_ws/src/isaacsim/scripts/run_isaacsim.py new file mode 100755 index 0000000..d61de3a --- /dev/null +++ b/foxy_ws/src/isaacsim/scripts/run_isaacsim.py @@ -0,0 +1,184 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2020-2024, NVIDIA CORPORATION. All rights reserved. +# +# NVIDIA CORPORATION and its licensors retain all intellectual property +# and proprietary rights in and to this software, related documentation +# and any modifications thereto. Any use, reproduction, disclosure or +# distribution of this software and related documentation without an express +# license agreement from NVIDIA CORPORATION is strictly prohibited. + +import rclpy +from rclpy.node import Node +import argparse +import os +import subprocess +from subprocess import run +import signal +import sys +import atexit +import psutil + +# Default values +defaults = { + "isaac_sim_version": "4.0.0", + "isaac_sim_path": "", + "use_internal_libs": False, + "dds_type": "fastdds", + "gui": "", + "standalone": "", + "play_sim_on_start": False, + "ros_distro_var": "foxy", + "ros_installation_path": "" +} + +# List to keep track of subprocesses +subprocesses = [] + +def signal_handler(sig, frame): + print('Ctrl+C received, shutting down...') + isaac_sim_shutdown() + sys.exit(0) + +def isaac_sim_shutdown(): + + for proc_id in subprocesses: + + if sys.platform == "win32": + os.kill(proc_id, signal.SIGTERM) + + else: + os.killpg(os.getpgid(proc_id), signal.SIGKILL) + + + print("All subprocesses terminated.") + +# Register the signal handler for SIGINT +signal.signal(signal.SIGINT, signal_handler) + +def version_ge(v1, v2): + return tuple(map(int, (v1.split(".")))) >= tuple(map(int, (v2.split(".")))) + +def version_gt(v1, v2): + return tuple(map(int, (v1.split(".")))) > tuple(map(int, (v2.split(".")))) + +def update_env_vars(version_to_remove, specified_path_to_remove, env_var_name): + env_var_value = os.environ.get(env_var_name, "") + new_env_var_value = [] + for path in env_var_value.split(os.pathsep): + if not version_to_remove in path and not path.startswith(specified_path_to_remove): + new_env_var_value.append(path) + os.environ[env_var_name] = os.pathsep.join(new_env_var_value) + +class IsaacSimLauncherNode(Node): + def __init__(self): + super().__init__('isaac_sim_launcher_node') + self.declare_parameters( + namespace='', + parameters=[ + ('version', defaults['isaac_sim_version']), + ('install_path', defaults['isaac_sim_path']), + ('use_internal_libs', defaults['use_internal_libs']), + ('dds_type', defaults['dds_type']), + ('gui', defaults['gui']), + ('standalone', defaults['standalone']), + ('play_sim_on_start', defaults['play_sim_on_start']), + ('ros_distro', defaults['ros_distro_var']), + ('ros_installation_path', defaults['ros_installation_path']), + ] + ) + self.execute_launch() + + def execute_launch(self): + args = argparse.Namespace() + args.version = self.get_parameter('version').get_parameter_value().string_value + args.install_path = self.get_parameter('install_path').get_parameter_value().string_value + args.use_internal_libs = self.get_parameter('use_internal_libs').get_parameter_value().bool_value + args.dds_type = self.get_parameter('dds_type').get_parameter_value().string_value + args.gui = self.get_parameter('gui').get_parameter_value().string_value + args.standalone = self.get_parameter('standalone').get_parameter_value().string_value + args.play_sim_on_start = self.get_parameter('play_sim_on_start').get_parameter_value().bool_value + args.ros_distro = self.get_parameter('ros_distro').get_parameter_value().string_value + args.ros_installation_path = self.get_parameter('ros_installation_path').get_parameter_value().string_value + + filepath_root = "" + + if args.install_path != "": + filepath_root = args.install_path + else: + # If custom Isaac Sim Installation folder not given, use the default path using version number provided. + home_var = "USERPROFILE" if sys.platform == "win32" else "HOME" + home_path = os.getenv(home_var) + + if version_ge(args.version, "4.0.0") and not version_gt(args.version, "2021.2.0"): + if sys.platform == "win32": + filepath_root = os.path.join(home_path, "AppData", "Local", "ov", "pkg", f"isaac-sim-{args.version}") + else: + filepath_root = os.path.join(home_path, ".local", "share", "ov", "pkg", f"isaac-sim-{args.version}") + elif version_ge(args.version, "2021.2.1") and not version_ge(args.version, "2023.1.2"): + if sys.platform == "win32": + filepath_root = os.path.join(home_path, "AppData", "Local", "ov", "pkg", f"isaac_sim-{args.version}") + else: + filepath_root = os.path.join(home_path, ".local", "share", "ov", "pkg", f"isaac_sim-{args.version}") + else: + print(f"Unsupported Isaac Sim version: {args.version}") + sys.exit(0) + + os.environ["ROS_DISTRO"] = args.ros_distro + + if args.use_internal_libs: + if sys.platform == "win32": + print("use_internal_libs parameter is not supported in Windows") + sys.exit(0) + else: + os.environ["LD_LIBRARY_PATH"] = f"{os.getenv('LD_LIBRARY_PATH')}:{filepath_root}/exts/omni.isaac.ros2_bridge/{args.ros_distro}/lib" + specific_path_to_remove = f"/opt/ros/{args.ros_distro}" + version_to_remove = "foxy" if args.ros_distro == "humble" else "humble" + update_env_vars(version_to_remove, specific_path_to_remove, "LD_LIBRARY_PATH") + update_env_vars(version_to_remove, specific_path_to_remove, "PYTHONPATH") + update_env_vars(version_to_remove, specific_path_to_remove, "PATH") + + elif args.ros_installation_path: + # If a custom ros installation path is provided + + if sys.platform == "win32": + proc = subprocess.Popen(f"call {args.ros_installation_path}", shell=True, start_new_session=True) + subprocesses.append(proc.pid) + else: + proc = subprocess.Popen(f"source {args.ros_installation_path}", shell=True, start_new_session=True) + subprocesses.append(proc.pid) + + os.environ["RMW_IMPLEMENTATION"] = "rmw_cyclonedds_cpp" if args.dds_type == "cyclonedds" else "rmw_fastrtps_cpp" + + play_sim_on_start_arg = "--start-on-play" if args.play_sim_on_start else "" + + if args.standalone != "": + executable_path = os.path.join(filepath_root, "python.sh" if sys.platform != "win32" else "python.bat") + + proc = subprocess.Popen(f"{executable_path} {args.standalone}", shell=True, start_new_session=True) + subprocesses.append(proc.pid) + else: + executable_command = f'{os.path.join(filepath_root, "isaac-sim.sh" if sys.platform != "win32" else "isaac-sim.bat")} --/isaac/startup/ros_bridge_extension=omni.isaac.ros2_bridge' + + if args.gui != "": + script_dir = os.path.dirname(__file__) + file_arg = os.path.join(script_dir, "open_isaacsim_stage.py") + f" --path {args.gui} {play_sim_on_start_arg}" + command = f"{executable_command} --exec '{file_arg}'" + proc = subprocess.Popen(command, shell=True, start_new_session=True) + subprocesses.append(proc.pid) + else: + proc = subprocess.Popen(executable_command, shell=True, start_new_session=True) + subprocesses.append(proc.pid) + +def main(args=None): + rclpy.init(args=args) + isaac_sim_launcher_node = IsaacSimLauncherNode() + rclpy.spin(isaac_sim_launcher_node) + + # Ensure all subprocesses are terminated before exiting + isaac_sim_shutdown() + isaac_sim_launcher_node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/foxy_ws/src/navigation/carter_navigation/launch/carter_navigation.launch.py b/foxy_ws/src/navigation/carter_navigation/launch/carter_navigation.launch.py index 7cf93e6..3a49d58 100644 --- a/foxy_ws/src/navigation/carter_navigation/launch/carter_navigation.launch.py +++ b/foxy_ws/src/navigation/carter_navigation/launch/carter_navigation.launch.py @@ -1,4 +1,4 @@ -## Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. +## Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. ## NVIDIA CORPORATION and its licensors retain all intellectual property ## and proprietary rights in and to this software, related documentation ## and any modifications thereto. Any use, reproduction, disclosure or @@ -57,7 +57,7 @@ def generate_launch_description(): Node( package='pointcloud_to_laserscan', executable='pointcloud_to_laserscan_node', - remappings=[('cloud_in', ['/front_3d_lidar/point_cloud']), + remappings=[('cloud_in', ['/front_3d_lidar/lidar_points']), ('scan', ['/scan'])], parameters=[{ 'target_frame': 'front_3d_lidar', diff --git a/foxy_ws/src/navigation/carter_navigation/launch/carter_navigation_individual.launch.py b/foxy_ws/src/navigation/carter_navigation/launch/carter_navigation_individual.launch.py index 6885664..b8736b6 100644 --- a/foxy_ws/src/navigation/carter_navigation/launch/carter_navigation_individual.launch.py +++ b/foxy_ws/src/navigation/carter_navigation/launch/carter_navigation_individual.launch.py @@ -1,4 +1,4 @@ -## Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. +## Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. ## NVIDIA CORPORATION and its licensors retain all intellectual property ## and proprietary rights in and to this software, related documentation ## and any modifications thereto. Any use, reproduction, disclosure or diff --git a/foxy_ws/src/navigation/carter_navigation/launch/carter_navigation_isaacsim.launch.py b/foxy_ws/src/navigation/carter_navigation/launch/carter_navigation_isaacsim.launch.py new file mode 100644 index 0000000..cb55f2b --- /dev/null +++ b/foxy_ws/src/navigation/carter_navigation/launch/carter_navigation_isaacsim.launch.py @@ -0,0 +1,125 @@ +## Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. +## NVIDIA CORPORATION and its licensors retain all intellectual property +## and proprietary rights in and to this software, related documentation +## and any modifications thereto. Any use, reproduction, disclosure or +## distribution of this software and related documentation without an express +## license agreement from NVIDIA CORPORATION is strictly prohibited. + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch.actions import RegisterEventHandler, ExecuteProcess +from launch.event_handlers import OnProcessStart, OnProcessIO +from launch.substitutions import FindExecutable + +def generate_launch_description(): + + use_sim_time = LaunchConfiguration("use_sim_time", default="True") + + map_dir = LaunchConfiguration( + "map", + default=os.path.join( + get_package_share_directory("carter_navigation"), "maps", "carter_warehouse_navigation.yaml" + ), + ) + + param_dir = LaunchConfiguration( + "params_file", + default=os.path.join( + get_package_share_directory("carter_navigation"), "params", "carter_navigation_params.yaml" + ), + ) + + + nav2_bringup_launch_dir = os.path.join(get_package_share_directory("nav2_bringup"), "launch") + + rviz_config_dir = os.path.join(get_package_share_directory("carter_navigation"), "rviz2", "carter_navigation.rviz") + + ld_automatic_goal = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory("isaac_ros_navigation_goal"), "launch", "isaac_ros_navigation_goal.launch.py" + ), + ] + ), + ) + + + + def execute_second_node_if_condition_met(event, second_node_action): + output = event.text.decode().strip() + # Look for fully loaded message from Isaac Sim. Only applicable in Gui mode. + if "Stage loaded and simulation is playing." in output: + # Log a message indicating the condition has been met + print("Condition met, launching the second node.") + + return second_node_action + + + return LaunchDescription( + [ + # Declaring the Isaac Sim scene path. 'gui' launch argument is already used withing run_isaac_sim.launch.py + DeclareLaunchArgument("gui", default_value='omniverse://localhost/NVIDIA/Assets/Isaac/4.0/Isaac/Samples/ROS2/Scenario/carter_warehouse_navigation.usd', description="Path to isaac sim scene"), + + # Include Isaac Sim launch file from isaacsim package with given launch parameters. + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory("isaacsim"), "launch", "run_isaacsim.launch.py" + ), + ] + ), + launch_arguments={ + 'version': '4.0.0', + 'play_sim_on_start': 'true', + }.items(), + ), + + DeclareLaunchArgument("map", default_value=map_dir, description="Full path to map file to load"), + DeclareLaunchArgument( + "params_file", default_value=param_dir, description="Full path to param file to load" + ), + DeclareLaunchArgument( + "use_sim_time", default_value="true", description="Use simulation (Omniverse Isaac Sim) clock if true" + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(nav2_bringup_launch_dir, "rviz_launch.py")), + launch_arguments={"namespace": "", "use_namespace": "False", "rviz_config": rviz_config_dir}.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource([nav2_bringup_launch_dir, "/bringup_launch.py"]), + launch_arguments={"map": map_dir, "use_sim_time": use_sim_time, "params_file": param_dir}.items(), + ), + Node( + package='pointcloud_to_laserscan', executable='pointcloud_to_laserscan_node', + remappings=[('cloud_in', ['/front_3d_lidar/lidar_points']), + ('scan', ['/scan'])], + parameters=[{ + 'target_frame': 'front_3d_lidar', + 'transform_tolerance': 0.01, + 'min_height': -0.4, + 'max_height': 1.5, + 'angle_min': -1.5708, # -M_PI/2 + 'angle_max': 1.5708, # M_PI/2 + 'angle_increment': 0.0087, # M_PI/360.0 + 'scan_time': 0.3333, + 'range_min': 0.05, + 'range_max': 100.0, + 'use_inf': True, + 'inf_epsilon': 1.0, + # 'concurrency_level': 1, + }], + name='pointcloud_to_laserscan' + ), + + # Launch automatic goal generator node when Isaac Sim has finished loading. + RegisterEventHandler( + OnProcessIO( + on_stdout=lambda event: execute_second_node_if_condition_met(event, ld_automatic_goal) + ) + ), + ] + ) diff --git a/foxy_ws/src/navigation/carter_navigation/launch/multiple_robot_carter_navigation_hospital.launch.py b/foxy_ws/src/navigation/carter_navigation/launch/multiple_robot_carter_navigation_hospital.launch.py index 4ec283b..b190dae 100644 --- a/foxy_ws/src/navigation/carter_navigation/launch/multiple_robot_carter_navigation_hospital.launch.py +++ b/foxy_ws/src/navigation/carter_navigation/launch/multiple_robot_carter_navigation_hospital.launch.py @@ -1,4 +1,4 @@ -## Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. +## Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. ## NVIDIA CORPORATION and its licensors retain all intellectual property ## and proprietary rights in and to this software, related documentation ## and any modifications thereto. Any use, reproduction, disclosure or @@ -132,7 +132,7 @@ def generate_launch_description(): Node( package='pointcloud_to_laserscan', executable='pointcloud_to_laserscan_node', - remappings=[('cloud_in', ['front_3d_lidar/point_cloud']), + remappings=[('cloud_in', ['front_3d_lidar/lidar_points']), ('scan', ['scan'])], parameters=[{ 'target_frame': 'front_3d_lidar', diff --git a/foxy_ws/src/navigation/carter_navigation/launch/multiple_robot_carter_navigation_office.launch.py b/foxy_ws/src/navigation/carter_navigation/launch/multiple_robot_carter_navigation_office.launch.py index 1fc777d..620f3f5 100644 --- a/foxy_ws/src/navigation/carter_navigation/launch/multiple_robot_carter_navigation_office.launch.py +++ b/foxy_ws/src/navigation/carter_navigation/launch/multiple_robot_carter_navigation_office.launch.py @@ -1,4 +1,4 @@ -## Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. +## Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. ## NVIDIA CORPORATION and its licensors retain all intellectual property ## and proprietary rights in and to this software, related documentation ## and any modifications thereto. Any use, reproduction, disclosure or @@ -132,7 +132,7 @@ def generate_launch_description(): Node( package='pointcloud_to_laserscan', executable='pointcloud_to_laserscan_node', - remappings=[('cloud_in', ['front_3d_lidar/point_cloud']), + remappings=[('cloud_in', ['front_3d_lidar/lidar_points']), ('scan', ['scan'])], parameters=[{ 'target_frame': 'front_3d_lidar', diff --git a/foxy_ws/src/navigation/carter_navigation/package.xml b/foxy_ws/src/navigation/carter_navigation/package.xml index 9dccd82..e13f017 100644 --- a/foxy_ws/src/navigation/carter_navigation/package.xml +++ b/foxy_ws/src/navigation/carter_navigation/package.xml @@ -7,9 +7,9 @@ The carter_navigation package - isaac sim + Isaac Sim - Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. + Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. NVIDIA CORPORATION and its licensors retain all intellectual property and proprietary rights in and to this software, related documentation and any modifications thereto. Any use, reproduction, disclosure or @@ -17,7 +17,7 @@ license agreement from NVIDIA CORPORATION is strictly prohibited. https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html - https://forums.developer.nvidia.com/c/agx-autonomous-machines/isaac/simulation + https://forums.developer.nvidia.com/c/omniverse/simulation/69 ament_cmake diff --git a/foxy_ws/src/navigation/carter_navigation/params/carter_navigation_params.yaml b/foxy_ws/src/navigation/carter_navigation/params/carter_navigation_params.yaml index 390ad30..0957b13 100644 --- a/foxy_ws/src/navigation/carter_navigation/params/carter_navigation_params.yaml +++ b/foxy_ws/src/navigation/carter_navigation/params/carter_navigation_params.yaml @@ -53,7 +53,7 @@ bt_navigator: use_sim_time: True global_frame: map robot_base_frame: base_link - odom_topic: /odom + odom_topic: /chassis/odom default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml" plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node @@ -92,7 +92,7 @@ velocity_smoother: velocity_timeout: 1.0 # max_accel: [1.0, 0.0, 1.25] # max_decel: [-1.0, 0.0, -1.25] - odom_topic: "odom" + odom_topic: "chassis/odom" odom_duration: 0.1 controller_server: @@ -183,7 +183,7 @@ local_costmap: unknown_threshold: 15 observation_sources: pointcloud pointcloud: # no frame set, uses frame from message - topic: /front_3d_lidar/point_cloud + topic: /front_3d_lidar/lidar_points max_obstacle_height: 2.0 min_obstacle_height: 0.1 obstacle_max_range: 10.0 diff --git a/foxy_ws/src/navigation/carter_navigation/params/hospital/multi_robot_carter_navigation_params_1.yaml b/foxy_ws/src/navigation/carter_navigation/params/hospital/multi_robot_carter_navigation_params_1.yaml index d6db418..7f9525c 100644 --- a/foxy_ws/src/navigation/carter_navigation/params/hospital/multi_robot_carter_navigation_params_1.yaml +++ b/foxy_ws/src/navigation/carter_navigation/params/hospital/multi_robot_carter_navigation_params_1.yaml @@ -54,7 +54,7 @@ bt_navigator: enable_groot_monitoring: False global_frame: map robot_base_frame: base_link - odom_topic: odom + odom_topic: chassis/odom default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml" plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node @@ -94,7 +94,7 @@ velocity_smoother: velocity_timeout: 1.0 # max_accel: [1.0, 0.0, 1.25] # max_decel: [-1.0, 0.0, -1.25] - odom_topic: "odom" + odom_topic: "chassis/odom" odom_duration: 0.1 controller_server: @@ -185,7 +185,7 @@ local_costmap: unknown_threshold: 15 observation_sources: pointcloud pointcloud: # no frame set, uses frame from message - topic: /carter1/front_3d_lidar/point_cloud + topic: /carter1/front_3d_lidar/lidar_points max_obstacle_height: 2.0 min_obstacle_height: 0.1 obstacle_max_range: 10.0 diff --git a/foxy_ws/src/navigation/carter_navigation/params/hospital/multi_robot_carter_navigation_params_2.yaml b/foxy_ws/src/navigation/carter_navigation/params/hospital/multi_robot_carter_navigation_params_2.yaml index dffd3b6..21879b2 100644 --- a/foxy_ws/src/navigation/carter_navigation/params/hospital/multi_robot_carter_navigation_params_2.yaml +++ b/foxy_ws/src/navigation/carter_navigation/params/hospital/multi_robot_carter_navigation_params_2.yaml @@ -54,7 +54,7 @@ bt_navigator: enable_groot_monitoring: False global_frame: map robot_base_frame: base_link - odom_topic: odom + odom_topic: chassis/odom default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml" plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node @@ -94,7 +94,7 @@ velocity_smoother: velocity_timeout: 1.0 # max_accel: [1.0, 0.0, 1.25] # max_decel: [-1.0, 0.0, -1.25] - odom_topic: "odom" + odom_topic: "chassis/odom" odom_duration: 0.1 controller_server: @@ -185,7 +185,7 @@ local_costmap: unknown_threshold: 15 observation_sources: pointcloud pointcloud: # no frame set, uses frame from message - topic: /carter2/front_3d_lidar/point_cloud + topic: /carter2/front_3d_lidar/lidar_points max_obstacle_height: 2.0 min_obstacle_height: 0.1 obstacle_max_range: 10.0 diff --git a/foxy_ws/src/navigation/carter_navigation/params/hospital/multi_robot_carter_navigation_params_3.yaml b/foxy_ws/src/navigation/carter_navigation/params/hospital/multi_robot_carter_navigation_params_3.yaml index 5b12de6..5c9db80 100644 --- a/foxy_ws/src/navigation/carter_navigation/params/hospital/multi_robot_carter_navigation_params_3.yaml +++ b/foxy_ws/src/navigation/carter_navigation/params/hospital/multi_robot_carter_navigation_params_3.yaml @@ -54,7 +54,7 @@ bt_navigator: enable_groot_monitoring: False global_frame: map robot_base_frame: base_link - odom_topic: odom + odom_topic: chassis/odom default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml" plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node @@ -94,7 +94,7 @@ velocity_smoother: velocity_timeout: 1.0 # max_accel: [1.0, 0.0, 1.25] # max_decel: [-1.0, 0.0, -1.25] - odom_topic: "odom" + odom_topic: "chassis/odom" odom_duration: 0.1 controller_server: @@ -185,7 +185,7 @@ local_costmap: unknown_threshold: 15 observation_sources: pointcloud pointcloud: # no frame set, uses frame from message - topic: /carter3/front_3d_lidar/point_cloud + topic: /carter3/front_3d_lidar/lidar_points max_obstacle_height: 2.0 min_obstacle_height: 0.1 obstacle_max_range: 10.0 diff --git a/foxy_ws/src/navigation/carter_navigation/params/office/multi_robot_carter_navigation_params_1.yaml b/foxy_ws/src/navigation/carter_navigation/params/office/multi_robot_carter_navigation_params_1.yaml index be3f2b0..7460877 100644 --- a/foxy_ws/src/navigation/carter_navigation/params/office/multi_robot_carter_navigation_params_1.yaml +++ b/foxy_ws/src/navigation/carter_navigation/params/office/multi_robot_carter_navigation_params_1.yaml @@ -54,7 +54,7 @@ bt_navigator: enable_groot_monitoring: False global_frame: map robot_base_frame: base_link - odom_topic: odom + odom_topic: chassis/odom default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml" plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node @@ -94,7 +94,7 @@ velocity_smoother: velocity_timeout: 1.0 # max_accel: [1.0, 0.0, 1.25] # max_decel: [-1.0, 0.0, -1.25] - odom_topic: "odom" + odom_topic: "chassis/odom" odom_duration: 0.1 controller_server: @@ -185,7 +185,7 @@ local_costmap: unknown_threshold: 15 observation_sources: pointcloud pointcloud: # no frame set, uses frame from message - topic: /carter1/front_3d_lidar/point_cloud + topic: /carter1/front_3d_lidar/lidar_points max_obstacle_height: 2.0 min_obstacle_height: 0.1 obstacle_max_range: 10.0 diff --git a/foxy_ws/src/navigation/carter_navigation/params/office/multi_robot_carter_navigation_params_2.yaml b/foxy_ws/src/navigation/carter_navigation/params/office/multi_robot_carter_navigation_params_2.yaml index d912dae..9438be1 100644 --- a/foxy_ws/src/navigation/carter_navigation/params/office/multi_robot_carter_navigation_params_2.yaml +++ b/foxy_ws/src/navigation/carter_navigation/params/office/multi_robot_carter_navigation_params_2.yaml @@ -54,7 +54,7 @@ bt_navigator: enable_groot_monitoring: False global_frame: map robot_base_frame: base_link - odom_topic: odom + odom_topic: chassis/odom default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml" plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node @@ -94,7 +94,7 @@ velocity_smoother: velocity_timeout: 1.0 # max_accel: [1.0, 0.0, 1.25] # max_decel: [-1.0, 0.0, -1.25] - odom_topic: "odom" + odom_topic: "chassis/odom" odom_duration: 0.1 controller_server: @@ -185,7 +185,7 @@ local_costmap: unknown_threshold: 15 observation_sources: pointcloud pointcloud: # no frame set, uses frame from message - topic: /carter2/front_3d_lidar/point_cloud + topic: /carter2/front_3d_lidar/lidar_points max_obstacle_height: 2.0 min_obstacle_height: 0.1 obstacle_max_range: 10.0 diff --git a/foxy_ws/src/navigation/carter_navigation/params/office/multi_robot_carter_navigation_params_3.yaml b/foxy_ws/src/navigation/carter_navigation/params/office/multi_robot_carter_navigation_params_3.yaml index a614246..59a3e2c 100644 --- a/foxy_ws/src/navigation/carter_navigation/params/office/multi_robot_carter_navigation_params_3.yaml +++ b/foxy_ws/src/navigation/carter_navigation/params/office/multi_robot_carter_navigation_params_3.yaml @@ -54,7 +54,7 @@ bt_navigator: enable_groot_monitoring: False global_frame: map robot_base_frame: base_link - odom_topic: odom + odom_topic: chassis/odom default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml" plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node @@ -94,7 +94,7 @@ velocity_smoother: velocity_timeout: 1.0 # max_accel: [1.0, 0.0, 1.25] # max_decel: [-1.0, 0.0, -1.25] - odom_topic: "odom" + odom_topic: "chassis/odom" odom_duration: 0.1 controller_server: @@ -185,7 +185,7 @@ local_costmap: unknown_threshold: 15 observation_sources: pointcloud pointcloud: # no frame set, uses frame from message - topic: /carter3/front_3d_lidar/point_cloud + topic: /carter3/front_3d_lidar/lidar_points max_obstacle_height: 2.0 min_obstacle_height: 0.1 obstacle_max_range: 10.0 diff --git a/foxy_ws/src/navigation/isaac_ros_navigation_goal/launch/isaac_ros_navigation_goal.launch.py b/foxy_ws/src/navigation/isaac_ros_navigation_goal/launch/isaac_ros_navigation_goal.launch.py index 2a63f0a..4f3d712 100644 --- a/foxy_ws/src/navigation/isaac_ros_navigation_goal/launch/isaac_ros_navigation_goal.launch.py +++ b/foxy_ws/src/navigation/isaac_ros_navigation_goal/launch/isaac_ros_navigation_goal.launch.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. +# Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. # # NVIDIA CORPORATION and its licensors retain all intellectual property # and proprietary rights in and to this software, related documentation diff --git a/foxy_ws/src/navigation/isaac_ros_navigation_goal/package.xml b/foxy_ws/src/navigation/isaac_ros_navigation_goal/package.xml index 42c960d..cd69e67 100644 --- a/foxy_ws/src/navigation/isaac_ros_navigation_goal/package.xml +++ b/foxy_ws/src/navigation/isaac_ros_navigation_goal/package.xml @@ -5,9 +5,9 @@ 0.1.0 Package to set goals for navigation stack. - isaac sim + Isaac Sim - Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. + Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. NVIDIA CORPORATION and its licensors retain all intellectual property and proprietary rights in and to this software, related documentation and any modifications thereto. Any use, reproduction, disclosure or @@ -15,7 +15,7 @@ license agreement from NVIDIA CORPORATION is strictly prohibited. https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html - https://forums.developer.nvidia.com/c/agx-autonomous-machines/isaac/simulation + https://forums.developer.nvidia.com/c/omniverse/simulation/69 https://developer.nvidia.com/isaac-ros-gems/ rclpy diff --git a/humble_ws/fastdds.xml b/humble_ws/fastdds.xml index 2782469..83bcc53 100644 --- a/humble_ws/fastdds.xml +++ b/humble_ws/fastdds.xml @@ -1,6 +1,6 @@ -Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. +Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. NVIDIA CORPORATION and its licensors retain all intellectual property and proprietary rights in and to this software, related documentation and any modifications thereto. Any use, reproduction, disclosure or diff --git a/humble_ws/src/custom_message/package.xml b/humble_ws/src/custom_message/package.xml index 2fb3b7f..f91f9e2 100644 --- a/humble_ws/src/custom_message/package.xml +++ b/humble_ws/src/custom_message/package.xml @@ -4,8 +4,17 @@ custom_message 0.0.0 Custom Message Sample Package - isaac sim - TODO: License declaration + Isaac Sim + + Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. + NVIDIA CORPORATION and its licensors retain all intellectual property + and proprietary rights in and to this software, related documentation + and any modifications thereto. Any use, reproduction, disclosure or + distribution of this software and related documentation without an express + license agreement from NVIDIA CORPORATION is strictly prohibited. + + https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html + https://forums.developer.nvidia.com/c/omniverse/simulation/69 ament_cmake diff --git a/humble_ws/src/isaac_ros2_messages/CMakeLists.txt b/humble_ws/src/isaac_ros2_messages/CMakeLists.txt index 7eb4e12..a61aebf 100644 --- a/humble_ws/src/isaac_ros2_messages/CMakeLists.txt +++ b/humble_ws/src/isaac_ros2_messages/CMakeLists.txt @@ -47,7 +47,11 @@ find_package(geometry_msgs REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "srv/IsaacPose.srv" + "srv/GetPrims.srv" + "srv/GetPrimAttributes.srv" + "srv/GetPrimAttribute.srv" + "srv/SetPrimAttribute.srv" DEPENDENCIES builtin_interfaces std_msgs geometry_msgs - ) +) - ament_package() +ament_package() diff --git a/humble_ws/src/isaac_ros2_messages/package.xml b/humble_ws/src/isaac_ros2_messages/package.xml index d825753..fdd3557 100644 --- a/humble_ws/src/isaac_ros2_messages/package.xml +++ b/humble_ws/src/isaac_ros2_messages/package.xml @@ -2,11 +2,11 @@ isaac_ros2_messages - 0.1.0 + 0.2.0 ROS2 messages for isaac ros2 bridge - isaac sim + Isaac Sim - Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. + Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. NVIDIA CORPORATION and its licensors retain all intellectual property and proprietary rights in and to this software, related documentation and any modifications thereto. Any use, reproduction, disclosure or @@ -14,7 +14,7 @@ license agreement from NVIDIA CORPORATION is strictly prohibited. https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html - https://forums.developer.nvidia.com/c/agx-autonomous-machines/isaac/simulation + https://forums.developer.nvidia.com/c/omniverse/simulation/69 ament_cmake diff --git a/humble_ws/src/isaac_ros2_messages/srv/GetPrimAttribute.srv b/humble_ws/src/isaac_ros2_messages/srv/GetPrimAttribute.srv new file mode 100644 index 0000000..10adbc2 --- /dev/null +++ b/humble_ws/src/isaac_ros2_messages/srv/GetPrimAttribute.srv @@ -0,0 +1,7 @@ +string path # prim path +string attribute # attribute name +--- +string value # attribute value (as JSON) +string type # attribute type +bool success # indicate a successful execution of the service +string message # informational, e.g. for error messages \ No newline at end of file diff --git a/humble_ws/src/isaac_ros2_messages/srv/GetPrimAttributes.srv b/humble_ws/src/isaac_ros2_messages/srv/GetPrimAttributes.srv new file mode 100644 index 0000000..26eb58a --- /dev/null +++ b/humble_ws/src/isaac_ros2_messages/srv/GetPrimAttributes.srv @@ -0,0 +1,7 @@ +string path # prim path +--- +string[] names # list of attribute base names (name used to Get or Set an attribute) +string[] displays # list of attribute display names (name displayed in Property tab) +string[] types # list of attribute data types +bool success # indicate a successful execution of the service +string message # informational, e.g. for error messages \ No newline at end of file diff --git a/humble_ws/src/isaac_ros2_messages/srv/GetPrims.srv b/humble_ws/src/isaac_ros2_messages/srv/GetPrims.srv new file mode 100644 index 0000000..7afbab6 --- /dev/null +++ b/humble_ws/src/isaac_ros2_messages/srv/GetPrims.srv @@ -0,0 +1,6 @@ +string path # get prims at path +--- +string[] paths # list of prim paths +string[] types # prim type names +bool success # indicate a successful execution of the service +string message # informational, e.g. for error messages \ No newline at end of file diff --git a/humble_ws/src/isaac_ros2_messages/srv/SetPrimAttribute.srv b/humble_ws/src/isaac_ros2_messages/srv/SetPrimAttribute.srv new file mode 100644 index 0000000..c70a5ee --- /dev/null +++ b/humble_ws/src/isaac_ros2_messages/srv/SetPrimAttribute.srv @@ -0,0 +1,6 @@ +string path # prim path +string attribute # attribute name +string value # attribute value (as JSON) +--- +bool success # indicate a successful execution of the service +string message # informational, e.g. for error messages \ No newline at end of file diff --git a/humble_ws/src/isaac_tutorials/CMakeLists.txt b/humble_ws/src/isaac_tutorials/CMakeLists.txt index 76e0d50..4d17ff7 100644 --- a/humble_ws/src/isaac_tutorials/CMakeLists.txt +++ b/humble_ws/src/isaac_tutorials/CMakeLists.txt @@ -12,6 +12,7 @@ install(DIRECTORY # Install Python executables install(PROGRAMS scripts/ros2_publisher.py + scripts/ros2_ackermann_publisher.py DESTINATION lib/${PROJECT_NAME} ) ament_package() diff --git a/humble_ws/src/isaac_tutorials/package.xml b/humble_ws/src/isaac_tutorials/package.xml index 5ef7804..770aa10 100644 --- a/humble_ws/src/isaac_tutorials/package.xml +++ b/humble_ws/src/isaac_tutorials/package.xml @@ -7,9 +7,9 @@ The isaac_tutorials package - isaac sim + Isaac Sim - Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. + Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. NVIDIA CORPORATION and its licensors retain all intellectual property and proprietary rights in and to this software, related documentation and any modifications thereto. Any use, reproduction, disclosure or @@ -17,14 +17,16 @@ license agreement from NVIDIA CORPORATION is strictly prohibited. https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html - https://forums.developer.nvidia.com/c/agx-autonomous-machines/isaac/simulation + https://forums.developer.nvidia.com/c/omniverse/simulation/69 ament_cmake + rqt_image_view launch launch_ros joint_state_publisher + ackermann_msgs rviz2 diff --git a/humble_ws/src/isaac_tutorials/rviz2/carter_stereo.rviz b/humble_ws/src/isaac_tutorials/rviz2/carter_stereo.rviz index 3274f6d..c904dcd 100644 --- a/humble_ws/src/isaac_tutorials/rviz2/carter_stereo.rviz +++ b/humble_ws/src/isaac_tutorials/rviz2/carter_stereo.rviz @@ -59,17 +59,17 @@ Visualization Manager: Value: true front_3d_lidar: Value: true - front_stereo_camera:imu: + front_stereo_camera_imu: Value: true - front_stereo_camera:left_rgb: + front_stereo_camera_left_optical: Value: true - front_stereo_camera:right_rgb: + front_stereo_camera_right_optical: Value: true - left_stereo_camera:imu: + left_stereo_camera_imu: Value: true - left_stereo_camera:left_rgb: + left_stereo_camera_left_optical: Value: true - left_stereo_camera:right_rgb: + left_stereo_camera_right_optical: Value: true odom: Value: true @@ -81,11 +81,11 @@ Visualization Manager: Value: true rear_stereo_camera:right_rgb: Value: true - right_stereo_camera:imu: + right_stereo_camera_imu: Value: true - right_stereo_camera:left_rgb: + right_stereo_camera_left_optical: Value: true - right_stereo_camera:right_rgb: + right_stereo_camera_right_optical: Value: true Marker Scale: 1 Name: TF @@ -101,17 +101,17 @@ Visualization Manager: {} front_3d_lidar: {} - front_stereo_camera:imu: + front_stereo_camera_imu: {} - front_stereo_camera:left_rgb: + front_stereo_camera_left_optical: {} - front_stereo_camera:right_rgb: + front_stereo_camera_right_optical: {} - left_stereo_camera:imu: + left_stereo_camera_imu: {} - left_stereo_camera:left_rgb: + left_stereo_camera_left_optical: {} - left_stereo_camera:right_rgb: + left_stereo_camera_right_optical: {} rear_2d_lidar: {} @@ -121,11 +121,11 @@ Visualization Manager: {} rear_stereo_camera:right_rgb: {} - right_stereo_camera:imu: + right_stereo_camera_imu: {} - right_stereo_camera:left_rgb: + right_stereo_camera_left_optical: {} - right_stereo_camera:right_rgb: + right_stereo_camera_right_optical: {} Update Interval: 0 Value: true @@ -180,7 +180,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /front_stereo_camera/left_rgb/image_raw + Value: /front_stereo_camera/left/image_raw Value: true - Class: rviz_default_plugins/Image Enabled: true @@ -194,7 +194,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /front_stereo_camera/right_rgb/image_raw + Value: /front_stereo_camera/right/image_raw Value: true - Alpha: 1 Autocompute Intensity Bounds: true @@ -226,7 +226,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /front_3d_lidar/point_cloud + Value: /front_3d_lidar/lidar_points Use Fixed Frame: true Use rainbow: true Value: true diff --git a/humble_ws/src/isaac_tutorials/scripts/ros2_ackermann_publisher.py b/humble_ws/src/isaac_tutorials/scripts/ros2_ackermann_publisher.py new file mode 100644 index 0000000..abcb420 --- /dev/null +++ b/humble_ws/src/isaac_tutorials/scripts/ros2_ackermann_publisher.py @@ -0,0 +1,56 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2020-2024, NVIDIA CORPORATION. All rights reserved. +# +# NVIDIA CORPORATION and its licensors retain all intellectual property +# and proprietary rights in and to this software, related documentation +# and any modifications thereto. Any use, reproduction, disclosure or +# distribution of this software and related documentation without an express +# license agreement from NVIDIA CORPORATION is strictly prohibited. + +import rclpy +from rclpy.node import Node + +from ackermann_msgs.msg import AckermannDriveStamped +import numpy as np + +class MinimalPublisher(Node): + + def __init__(self): + super().__init__('test_ackermann') + self.publisher_ = self.create_publisher(AckermannDriveStamped, 'ackermann_cmd', 10) + timer_period = 0.05 # seconds + self.timer = self.create_timer(timer_period, self.timer_callback) + self.i = 0 + + def timer_callback(self): + msg = AckermannDriveStamped() + + # Only command forklift using acceleration and steering angle + degrees1 = np.arange(0, 60) + degrees2 = np.arange(-60, 0) + degrees = np.concatenate((degrees1, degrees1[::-1], degrees2[::-1], degrees2)) + msg.header.frame_id = "forklift" + msg.header.stamp = self.get_clock().now().to_msg() + msg.drive.steering_angle = 0.0174533 * (degrees[self.i % degrees.size]) + + msg.drive.acceleration = float(degrees[self.i % degrees.size]) + + self.publisher_.publish(msg) + self.i += 1 + + +def main(args=None): + + rclpy.init(args=args) + + minimal_publisher = MinimalPublisher() + + rclpy.spin(minimal_publisher) + + minimal_publisher.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/humble_ws/src/isaacsim/CMakeLists.txt b/humble_ws/src/isaacsim/CMakeLists.txt new file mode 100644 index 0000000..968b438 --- /dev/null +++ b/humble_ws/src/isaacsim/CMakeLists.txt @@ -0,0 +1,18 @@ +cmake_minimum_required(VERSION 3.5) +project(isaacsim) + +find_package(ament_cmake REQUIRED) +find_package(rclpy REQUIRED) + +install(DIRECTORY + scripts + launch + DESTINATION share/${PROJECT_NAME}) + +# Install Python executables +install(PROGRAMS + scripts/run_isaacsim.py + scripts/open_isaacsim_stage.py + DESTINATION lib/${PROJECT_NAME} +) +ament_package() diff --git a/humble_ws/src/isaacsim/launch/run_isaacsim.launch.py b/humble_ws/src/isaacsim/launch/run_isaacsim.launch.py new file mode 100644 index 0000000..92a8b17 --- /dev/null +++ b/humble_ws/src/isaacsim/launch/run_isaacsim.launch.py @@ -0,0 +1,90 @@ +## Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. +## NVIDIA CORPORATION and its licensors retain all intellectual property +## and proprietary rights in and to this software, related documentation +## and any modifications thereto. Any use, reproduction, disclosure or +## distribution of this software and related documentation without an express +## license agreement from NVIDIA CORPORATION is strictly prohibited. + +from launch import LaunchDescription +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument, ExecuteProcess, OpaqueFunction +from launch.substitutions import LaunchConfiguration, Command, PythonExpression +from launch.conditions import IfCondition +from launch_ros.substitutions import FindPackageShare + + +# Declare all launch arguments corresponding to the bash script options +launch_args = [ + DeclareLaunchArgument('version', default_value='4.0.0', description='Specify the version of Isaac Sim to use. Isaac Sim will be run from default install root folder for the specified version. Leave empty to use latest version of Isaac Sim.'), + + DeclareLaunchArgument('install_path', default_value='', description='If Isaac Sim is insalled in a non-default location, provide a specific path to Isaac Sim installation root folder. (If defined, "version" parameter will be ignored)'), + + DeclareLaunchArgument('use_internal_libs', default_value='false', description='Set to true if you wish to use internal ROS libraries shipped with Isaac Sim.'), + + DeclareLaunchArgument('dds_type', default_value='fastdds', description='Set to "fastdds" or "cyclonedds" (Cyclone only supported for ROS Humble) to run Isaac Sim with a specific dds type.'), + + DeclareLaunchArgument('gui', default_value='', description='Provide the path to a usd file to open it when starting Isaac Sim in standard gui mode. If left empty, Isaac Sim will open an empty stage in standard gui mode.'), + + DeclareLaunchArgument('standalone', default_value='', description='Provide the path to the python file to open it and start Isaac Sim in standalone workflow. If left empty, Isaac Sim will open an empty stage in standard Gui mode.'), + + DeclareLaunchArgument('play_sim_on_start', default_value='false', description='If enabled and Isaac Sim will start playing the scene after it is loaded. (Only applicable when in standard gui mode and loading a scene)'), + + DeclareLaunchArgument('ros_distro', default_value='humble', description='Provide ROS version to use. Only Humble and Foxy is supported.'), + + DeclareLaunchArgument('ros_installation_path', default_value='', description='If ROS is installed in a non-default location (as in not under /opt/ros/), provide the path to your main setup.bash file for your ROS install. (/path/to/custom/ros/install/setup.bash)'), + +] + +# List of parameters to check +parameters_to_check = [ + 'version', 'install_path', 'use_internal_libs', 'dds_type', + 'standalone', 'play_sim_on_start', 'ros_distro', 'ros_installation_path', 'gui' +] + +def launch_setup(context): + + def add_parameter_argument(param_name, command_list): + param_value = LaunchConfiguration(param_name).perform(context) # Here you'll get the runtime config value + + if param_value != '': + if param_value == 'true': + return command_list + ['--' + param_name.replace('_', '-')] + elif param_value == 'false': + return command_list + else: + return command_list + ['--' + param_name.replace('_', '-'), param_value,] + else: + return command_list + + bash_command = [] + + # Add parameters to the command list if they are set + for param in parameters_to_check: + bash_command = add_parameter_argument(param, bash_command) + + # Run isaac sim as a ROS2 node with default parameters. Parameters can be overridden here or via launch arguments from other launch files. + isaacsim_node = Node( + package='isaacsim', executable='run_isaacsim.py', + name='isaacsim', output="screen", + parameters=[{ + 'version': LaunchConfiguration('version'), + 'install_path': LaunchConfiguration('install_path'), + 'use_internal_libs': LaunchConfiguration('use_internal_libs'), + 'dds_type': LaunchConfiguration('dds_type'), + 'gui': LaunchConfiguration('gui'), + 'standalone': LaunchConfiguration('standalone'), + 'play_sim_on_start': LaunchConfiguration('play_sim_on_start'), + 'ros_distro': LaunchConfiguration('ros_distro'), + 'ros_installation_path': LaunchConfiguration('ros_installation_path') + }] + ) + return [isaacsim_node] + + +# Create and return the launch description with all declared arguments and the execute launch_setup +def generate_launch_description(): + opfunc = OpaqueFunction(function = launch_setup) + ld = LaunchDescription(launch_args) + ld.add_action(opfunc) + return ld diff --git a/humble_ws/src/isaacsim/package.xml b/humble_ws/src/isaacsim/package.xml new file mode 100644 index 0000000..d5f2f87 --- /dev/null +++ b/humble_ws/src/isaacsim/package.xml @@ -0,0 +1,30 @@ + + + + isaacsim + 0.1.0 + + The isaacsim package that contains the script which can used to launch Isaac Sim as a ROS2 node from a launch file. + + + Isaac Sim + + Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. + NVIDIA CORPORATION and its licensors retain all intellectual property + and proprietary rights in and to this software, related documentation + and any modifications thereto. Any use, reproduction, disclosure or + distribution of this software and related documentation without an express + license agreement from NVIDIA CORPORATION is strictly prohibited. + + https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html + https://forums.developer.nvidia.com/c/omniverse/simulation/69 + + ament_cmake + + launch + launch_ros + + + ament_cmake + + diff --git a/humble_ws/src/isaacsim/scripts/open_isaacsim_stage.py b/humble_ws/src/isaacsim/scripts/open_isaacsim_stage.py new file mode 100644 index 0000000..2fdc67a --- /dev/null +++ b/humble_ws/src/isaacsim/scripts/open_isaacsim_stage.py @@ -0,0 +1,84 @@ +"""Script to open a USD stage in Isaac Sim (in standard gui mode). This script along with its arguments are automatically passed to Isaac Sim via the ROS2 launch workflow""" + +import carb +import argparse +import omni.usd +import asyncio +import omni.client +import omni.kit.async_engine +import omni.timeline + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument('--path', type=str, required=True, help='The path to USD stage') + + # Add the --start-on-play option + # If --start-on-play is specified, it sets the value to True + parser.add_argument('--start-on-play', action='store_true', + help='If present, to true.') + + try: + options = parser.parse_args() + except Exception as e: + carb.log_error(str(e)) + return + + omni.kit.async_engine.run_coroutine(open_stage_async(options.path, options.start_on_play)) + +async def open_stage_async(path: str, start_on_play: bool): + timeline_interface = None + if start_on_play: + timeline_interface = omni.timeline.get_timeline_interface() + + async def _open_stage_internal(path): + is_stage_with_session = False + try: + import omni.kit.usd.layers as layers + live_session_name = layers.get_live_session_name_from_shared_link(path) + is_stage_with_session = live_session_name is not None + except Exception: + pass + + if is_stage_with_session: + # Try to open the stage with specified live session. + (success, error) = await layers.get_live_syncing().open_stage_with_live_session_async(path) + else: + # Otherwise, use normal stage open. + (success, error) = await omni.usd.get_context().open_stage_async(path) + + if not success: + carb.log_error(f"Failed to open stage {path}: {error}.") + else: + if timeline_interface is not None: + await omni.kit.app.get_app().next_update_async() + await omni.kit.app.get_app().next_update_async() + timeline_interface.play() + print("Stage loaded and simulation is playing.") + pass + result, _ = await omni.client.stat_async(path) + if result == omni.client.Result.OK: + await _open_stage_internal(path) + + return + + broken_url = omni.client.break_url(path) + if broken_url.scheme == 'omniverse': + # Attempt to connect to nucleus server before opening stage + try: + from omni.kit.widget.nucleus_connector import get_nucleus_connector + nucleus_connector = get_nucleus_connector() + except Exception: + carb.log_warn("Open stage: Could not import Nucleus connector.") + return + + server_url = omni.client.make_url(scheme='omniverse', host=broken_url.host) + nucleus_connector.connect( + broken_url.host, server_url, + on_success_fn=lambda *_: asyncio.ensure_future(_open_stage_internal(path)), + on_failed_fn=lambda *_: carb.log_error(f"Open stage: Failed to connect to server '{server_url}'.") + ) + else: + carb.log_warn(f"Open stage: Could not open non-existent url '{path}'.") + + +main() diff --git a/humble_ws/src/isaacsim/scripts/run_isaacsim.py b/humble_ws/src/isaacsim/scripts/run_isaacsim.py new file mode 100755 index 0000000..478effe --- /dev/null +++ b/humble_ws/src/isaacsim/scripts/run_isaacsim.py @@ -0,0 +1,184 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2020-2024, NVIDIA CORPORATION. All rights reserved. +# +# NVIDIA CORPORATION and its licensors retain all intellectual property +# and proprietary rights in and to this software, related documentation +# and any modifications thereto. Any use, reproduction, disclosure or +# distribution of this software and related documentation without an express +# license agreement from NVIDIA CORPORATION is strictly prohibited. + +import rclpy +from rclpy.node import Node +import argparse +import os +import subprocess +from subprocess import run +import signal +import sys +import atexit +import psutil + +# Default values +defaults = { + "isaac_sim_version": "4.0.0", + "isaac_sim_path": "", + "use_internal_libs": False, + "dds_type": "fastdds", + "gui": "", + "standalone": "", + "play_sim_on_start": False, + "ros_distro_var": "humble", + "ros_installation_path": "" +} + +# List to keep track of subprocesses +subprocesses = [] + +def signal_handler(sig, frame): + print('Ctrl+C received, shutting down...') + isaac_sim_shutdown() + sys.exit(0) + +def isaac_sim_shutdown(): + + for proc_id in subprocesses: + + if sys.platform == "win32": + os.kill(proc_id, signal.SIGTERM) + + else: + os.killpg(os.getpgid(proc_id), signal.SIGKILL) + + + print("All subprocesses terminated.") + +# Register the signal handler for SIGINT +signal.signal(signal.SIGINT, signal_handler) + +def version_ge(v1, v2): + return tuple(map(int, (v1.split(".")))) >= tuple(map(int, (v2.split(".")))) + +def version_gt(v1, v2): + return tuple(map(int, (v1.split(".")))) > tuple(map(int, (v2.split(".")))) + +def update_env_vars(version_to_remove, specified_path_to_remove, env_var_name): + env_var_value = os.environ.get(env_var_name, "") + new_env_var_value = [] + for path in env_var_value.split(os.pathsep): + if not version_to_remove in path and not path.startswith(specified_path_to_remove): + new_env_var_value.append(path) + os.environ[env_var_name] = os.pathsep.join(new_env_var_value) + +class IsaacSimLauncherNode(Node): + def __init__(self): + super().__init__('isaac_sim_launcher_node') + self.declare_parameters( + namespace='', + parameters=[ + ('version', defaults['isaac_sim_version']), + ('install_path', defaults['isaac_sim_path']), + ('use_internal_libs', defaults['use_internal_libs']), + ('dds_type', defaults['dds_type']), + ('gui', defaults['gui']), + ('standalone', defaults['standalone']), + ('play_sim_on_start', defaults['play_sim_on_start']), + ('ros_distro', defaults['ros_distro_var']), + ('ros_installation_path', defaults['ros_installation_path']), + ] + ) + self.execute_launch() + + def execute_launch(self): + args = argparse.Namespace() + args.version = self.get_parameter('version').get_parameter_value().string_value + args.install_path = self.get_parameter('install_path').get_parameter_value().string_value + args.use_internal_libs = self.get_parameter('use_internal_libs').get_parameter_value().bool_value + args.dds_type = self.get_parameter('dds_type').get_parameter_value().string_value + args.gui = self.get_parameter('gui').get_parameter_value().string_value + args.standalone = self.get_parameter('standalone').get_parameter_value().string_value + args.play_sim_on_start = self.get_parameter('play_sim_on_start').get_parameter_value().bool_value + args.ros_distro = self.get_parameter('ros_distro').get_parameter_value().string_value + args.ros_installation_path = self.get_parameter('ros_installation_path').get_parameter_value().string_value + + filepath_root = "" + + if args.install_path != "": + filepath_root = args.install_path + else: + # If custom Isaac Sim Installation folder not given, use the default path using version number provided. + home_var = "USERPROFILE" if sys.platform == "win32" else "HOME" + home_path = os.getenv(home_var) + + if version_ge(args.version, "4.0.0") and not version_gt(args.version, "2021.2.0"): + if sys.platform == "win32": + filepath_root = os.path.join(home_path, "AppData", "Local", "ov", "pkg", f"isaac-sim-{args.version}") + else: + filepath_root = os.path.join(home_path, ".local", "share", "ov", "pkg", f"isaac-sim-{args.version}") + elif version_ge(args.version, "2021.2.1") and not version_ge(args.version, "2023.1.2"): + if sys.platform == "win32": + filepath_root = os.path.join(home_path, "AppData", "Local", "ov", "pkg", f"isaac_sim-{args.version}") + else: + filepath_root = os.path.join(home_path, ".local", "share", "ov", "pkg", f"isaac_sim-{args.version}") + else: + print(f"Unsupported Isaac Sim version: {args.version}") + sys.exit(0) + + os.environ["ROS_DISTRO"] = args.ros_distro + + if args.use_internal_libs: + if sys.platform == "win32": + print("use_internal_libs parameter is not supported in Windows") + sys.exit(0) + else: + os.environ["LD_LIBRARY_PATH"] = f"{os.getenv('LD_LIBRARY_PATH')}:{filepath_root}/exts/omni.isaac.ros2_bridge/{args.ros_distro}/lib" + specific_path_to_remove = f"/opt/ros/{args.ros_distro}" + version_to_remove = "foxy" if args.ros_distro == "humble" else "humble" + update_env_vars(version_to_remove, specific_path_to_remove, "LD_LIBRARY_PATH") + update_env_vars(version_to_remove, specific_path_to_remove, "PYTHONPATH") + update_env_vars(version_to_remove, specific_path_to_remove, "PATH") + + elif args.ros_installation_path: + # If a custom ros installation path is provided + + if sys.platform == "win32": + proc = subprocess.Popen(f"call {args.ros_installation_path}", shell=True, start_new_session=True) + subprocesses.append(proc.pid) + else: + proc = subprocess.Popen(f"source {args.ros_installation_path}", shell=True, start_new_session=True) + subprocesses.append(proc.pid) + + os.environ["RMW_IMPLEMENTATION"] = "rmw_cyclonedds_cpp" if args.dds_type == "cyclonedds" else "rmw_fastrtps_cpp" + + play_sim_on_start_arg = "--start-on-play" if args.play_sim_on_start else "" + + if args.standalone != "": + executable_path = os.path.join(filepath_root, "python.sh" if sys.platform != "win32" else "python.bat") + + proc = subprocess.Popen(f"{executable_path} {args.standalone}", shell=True, start_new_session=True) + subprocesses.append(proc.pid) + else: + executable_command = f'{os.path.join(filepath_root, "isaac-sim.sh" if sys.platform != "win32" else "isaac-sim.bat")} --/isaac/startup/ros_bridge_extension=omni.isaac.ros2_bridge' + + if args.gui != "": + script_dir = os.path.dirname(__file__) + file_arg = os.path.join(script_dir, "open_isaacsim_stage.py") + f" --path {args.gui} {play_sim_on_start_arg}" + command = f"{executable_command} --exec '{file_arg}'" + proc = subprocess.Popen(command, shell=True, start_new_session=True) + subprocesses.append(proc.pid) + else: + proc = subprocess.Popen(executable_command, shell=True, start_new_session=True) + subprocesses.append(proc.pid) + +def main(args=None): + rclpy.init(args=args) + isaac_sim_launcher_node = IsaacSimLauncherNode() + rclpy.spin(isaac_sim_launcher_node) + + # Ensure all subprocesses are terminated before exiting + isaac_sim_shutdown() + isaac_sim_launcher_node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/humble_ws/src/navigation/carter_navigation/launch/carter_navigation.launch.py b/humble_ws/src/navigation/carter_navigation/launch/carter_navigation.launch.py index 111ba96..11304cb 100644 --- a/humble_ws/src/navigation/carter_navigation/launch/carter_navigation.launch.py +++ b/humble_ws/src/navigation/carter_navigation/launch/carter_navigation.launch.py @@ -1,4 +1,4 @@ -## Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. +## Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. ## NVIDIA CORPORATION and its licensors retain all intellectual property ## and proprietary rights in and to this software, related documentation ## and any modifications thereto. Any use, reproduction, disclosure or @@ -58,7 +58,7 @@ def generate_launch_description(): Node( package='pointcloud_to_laserscan', executable='pointcloud_to_laserscan_node', - remappings=[('cloud_in', ['/front_3d_lidar/point_cloud']), + remappings=[('cloud_in', ['/front_3d_lidar/lidar_points']), ('scan', ['/scan'])], parameters=[{ 'target_frame': 'front_3d_lidar', diff --git a/humble_ws/src/navigation/carter_navigation/launch/carter_navigation_individual.launch.py b/humble_ws/src/navigation/carter_navigation/launch/carter_navigation_individual.launch.py index 6885664..b8736b6 100644 --- a/humble_ws/src/navigation/carter_navigation/launch/carter_navigation_individual.launch.py +++ b/humble_ws/src/navigation/carter_navigation/launch/carter_navigation_individual.launch.py @@ -1,4 +1,4 @@ -## Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. +## Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. ## NVIDIA CORPORATION and its licensors retain all intellectual property ## and proprietary rights in and to this software, related documentation ## and any modifications thereto. Any use, reproduction, disclosure or diff --git a/humble_ws/src/navigation/carter_navigation/launch/carter_navigation_isaacsim.launch.py b/humble_ws/src/navigation/carter_navigation/launch/carter_navigation_isaacsim.launch.py new file mode 100644 index 0000000..cb55f2b --- /dev/null +++ b/humble_ws/src/navigation/carter_navigation/launch/carter_navigation_isaacsim.launch.py @@ -0,0 +1,125 @@ +## Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. +## NVIDIA CORPORATION and its licensors retain all intellectual property +## and proprietary rights in and to this software, related documentation +## and any modifications thereto. Any use, reproduction, disclosure or +## distribution of this software and related documentation without an express +## license agreement from NVIDIA CORPORATION is strictly prohibited. + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch.actions import RegisterEventHandler, ExecuteProcess +from launch.event_handlers import OnProcessStart, OnProcessIO +from launch.substitutions import FindExecutable + +def generate_launch_description(): + + use_sim_time = LaunchConfiguration("use_sim_time", default="True") + + map_dir = LaunchConfiguration( + "map", + default=os.path.join( + get_package_share_directory("carter_navigation"), "maps", "carter_warehouse_navigation.yaml" + ), + ) + + param_dir = LaunchConfiguration( + "params_file", + default=os.path.join( + get_package_share_directory("carter_navigation"), "params", "carter_navigation_params.yaml" + ), + ) + + + nav2_bringup_launch_dir = os.path.join(get_package_share_directory("nav2_bringup"), "launch") + + rviz_config_dir = os.path.join(get_package_share_directory("carter_navigation"), "rviz2", "carter_navigation.rviz") + + ld_automatic_goal = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory("isaac_ros_navigation_goal"), "launch", "isaac_ros_navigation_goal.launch.py" + ), + ] + ), + ) + + + + def execute_second_node_if_condition_met(event, second_node_action): + output = event.text.decode().strip() + # Look for fully loaded message from Isaac Sim. Only applicable in Gui mode. + if "Stage loaded and simulation is playing." in output: + # Log a message indicating the condition has been met + print("Condition met, launching the second node.") + + return second_node_action + + + return LaunchDescription( + [ + # Declaring the Isaac Sim scene path. 'gui' launch argument is already used withing run_isaac_sim.launch.py + DeclareLaunchArgument("gui", default_value='omniverse://localhost/NVIDIA/Assets/Isaac/4.0/Isaac/Samples/ROS2/Scenario/carter_warehouse_navigation.usd', description="Path to isaac sim scene"), + + # Include Isaac Sim launch file from isaacsim package with given launch parameters. + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory("isaacsim"), "launch", "run_isaacsim.launch.py" + ), + ] + ), + launch_arguments={ + 'version': '4.0.0', + 'play_sim_on_start': 'true', + }.items(), + ), + + DeclareLaunchArgument("map", default_value=map_dir, description="Full path to map file to load"), + DeclareLaunchArgument( + "params_file", default_value=param_dir, description="Full path to param file to load" + ), + DeclareLaunchArgument( + "use_sim_time", default_value="true", description="Use simulation (Omniverse Isaac Sim) clock if true" + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(nav2_bringup_launch_dir, "rviz_launch.py")), + launch_arguments={"namespace": "", "use_namespace": "False", "rviz_config": rviz_config_dir}.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource([nav2_bringup_launch_dir, "/bringup_launch.py"]), + launch_arguments={"map": map_dir, "use_sim_time": use_sim_time, "params_file": param_dir}.items(), + ), + Node( + package='pointcloud_to_laserscan', executable='pointcloud_to_laserscan_node', + remappings=[('cloud_in', ['/front_3d_lidar/lidar_points']), + ('scan', ['/scan'])], + parameters=[{ + 'target_frame': 'front_3d_lidar', + 'transform_tolerance': 0.01, + 'min_height': -0.4, + 'max_height': 1.5, + 'angle_min': -1.5708, # -M_PI/2 + 'angle_max': 1.5708, # M_PI/2 + 'angle_increment': 0.0087, # M_PI/360.0 + 'scan_time': 0.3333, + 'range_min': 0.05, + 'range_max': 100.0, + 'use_inf': True, + 'inf_epsilon': 1.0, + # 'concurrency_level': 1, + }], + name='pointcloud_to_laserscan' + ), + + # Launch automatic goal generator node when Isaac Sim has finished loading. + RegisterEventHandler( + OnProcessIO( + on_stdout=lambda event: execute_second_node_if_condition_met(event, ld_automatic_goal) + ) + ), + ] + ) diff --git a/humble_ws/src/navigation/carter_navigation/launch/multiple_robot_carter_navigation_hospital.launch.py b/humble_ws/src/navigation/carter_navigation/launch/multiple_robot_carter_navigation_hospital.launch.py index 4ec283b..b190dae 100644 --- a/humble_ws/src/navigation/carter_navigation/launch/multiple_robot_carter_navigation_hospital.launch.py +++ b/humble_ws/src/navigation/carter_navigation/launch/multiple_robot_carter_navigation_hospital.launch.py @@ -1,4 +1,4 @@ -## Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. +## Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. ## NVIDIA CORPORATION and its licensors retain all intellectual property ## and proprietary rights in and to this software, related documentation ## and any modifications thereto. Any use, reproduction, disclosure or @@ -132,7 +132,7 @@ def generate_launch_description(): Node( package='pointcloud_to_laserscan', executable='pointcloud_to_laserscan_node', - remappings=[('cloud_in', ['front_3d_lidar/point_cloud']), + remappings=[('cloud_in', ['front_3d_lidar/lidar_points']), ('scan', ['scan'])], parameters=[{ 'target_frame': 'front_3d_lidar', diff --git a/humble_ws/src/navigation/carter_navigation/launch/multiple_robot_carter_navigation_office.launch.py b/humble_ws/src/navigation/carter_navigation/launch/multiple_robot_carter_navigation_office.launch.py index 1fc777d..620f3f5 100644 --- a/humble_ws/src/navigation/carter_navigation/launch/multiple_robot_carter_navigation_office.launch.py +++ b/humble_ws/src/navigation/carter_navigation/launch/multiple_robot_carter_navigation_office.launch.py @@ -1,4 +1,4 @@ -## Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. +## Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. ## NVIDIA CORPORATION and its licensors retain all intellectual property ## and proprietary rights in and to this software, related documentation ## and any modifications thereto. Any use, reproduction, disclosure or @@ -132,7 +132,7 @@ def generate_launch_description(): Node( package='pointcloud_to_laserscan', executable='pointcloud_to_laserscan_node', - remappings=[('cloud_in', ['front_3d_lidar/point_cloud']), + remappings=[('cloud_in', ['front_3d_lidar/lidar_points']), ('scan', ['scan'])], parameters=[{ 'target_frame': 'front_3d_lidar', diff --git a/humble_ws/src/navigation/carter_navigation/package.xml b/humble_ws/src/navigation/carter_navigation/package.xml index 2434aa6..fcb21ee 100644 --- a/humble_ws/src/navigation/carter_navigation/package.xml +++ b/humble_ws/src/navigation/carter_navigation/package.xml @@ -7,9 +7,9 @@ The carter_navigation package - isaac sim + Isaac Sim - Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. + Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. NVIDIA CORPORATION and its licensors retain all intellectual property and proprietary rights in and to this software, related documentation and any modifications thereto. Any use, reproduction, disclosure or @@ -17,7 +17,7 @@ license agreement from NVIDIA CORPORATION is strictly prohibited. https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html - https://forums.developer.nvidia.com/c/agx-autonomous-machines/isaac/simulation + https://forums.developer.nvidia.com/c/omniverse/simulation/69 ament_cmake diff --git a/humble_ws/src/navigation/carter_navigation/params/carter_navigation_params.yaml b/humble_ws/src/navigation/carter_navigation/params/carter_navigation_params.yaml index 38042bf..aadc983 100644 --- a/humble_ws/src/navigation/carter_navigation/params/carter_navigation_params.yaml +++ b/humble_ws/src/navigation/carter_navigation/params/carter_navigation_params.yaml @@ -60,7 +60,7 @@ bt_navigator: use_sim_time: True global_frame: map robot_base_frame: base_link - odom_topic: /odom + odom_topic: /chassis/odom bt_loop_duration: 20 default_server_timeout: 40 # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: @@ -127,7 +127,7 @@ velocity_smoother: velocity_timeout: 1.0 # max_accel: [1.0, 0.0, 1.25] # max_decel: [-1.0, 0.0, -1.25] - odom_topic: "odom" + odom_topic: "chassis/odom" odom_duration: 0.1 controller_server: @@ -232,7 +232,7 @@ local_costmap: unknown_threshold: 15 observation_sources: pointcloud pointcloud: # no frame set, uses frame from message - topic: /front_3d_lidar/point_cloud + topic: /front_3d_lidar/lidar_points max_obstacle_height: 2.0 min_obstacle_height: 0.1 obstacle_max_range: 10.0 diff --git a/humble_ws/src/navigation/carter_navigation/params/hospital/multi_robot_carter_navigation_params_1.yaml b/humble_ws/src/navigation/carter_navigation/params/hospital/multi_robot_carter_navigation_params_1.yaml index dac2245..161e2de 100644 --- a/humble_ws/src/navigation/carter_navigation/params/hospital/multi_robot_carter_navigation_params_1.yaml +++ b/humble_ws/src/navigation/carter_navigation/params/hospital/multi_robot_carter_navigation_params_1.yaml @@ -60,7 +60,7 @@ bt_navigator: use_sim_time: True global_frame: map robot_base_frame: base_link - odom_topic: odom + odom_topic: chassis/odom bt_loop_duration: 20 default_server_timeout: 40 # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: @@ -127,7 +127,7 @@ velocity_smoother: velocity_timeout: 1.0 # max_accel: [1.0, 0.0, 1.25] # max_decel: [-1.0, 0.0, -1.25] - odom_topic: "odom" + odom_topic: "chassis/odom" odom_duration: 0.1 controller_server: @@ -232,7 +232,7 @@ local_costmap: unknown_threshold: 15 observation_sources: pointcloud pointcloud: # no frame set, uses frame from message - topic: /carter1/front_3d_lidar/point_cloud + topic: /carter1/front_3d_lidar/lidar_points max_obstacle_height: 2.0 min_obstacle_height: 0.1 obstacle_max_range: 10.0 diff --git a/humble_ws/src/navigation/carter_navigation/params/hospital/multi_robot_carter_navigation_params_2.yaml b/humble_ws/src/navigation/carter_navigation/params/hospital/multi_robot_carter_navigation_params_2.yaml index 968e9c3..2473516 100644 --- a/humble_ws/src/navigation/carter_navigation/params/hospital/multi_robot_carter_navigation_params_2.yaml +++ b/humble_ws/src/navigation/carter_navigation/params/hospital/multi_robot_carter_navigation_params_2.yaml @@ -60,7 +60,7 @@ bt_navigator: use_sim_time: True global_frame: map robot_base_frame: base_link - odom_topic: odom + odom_topic: chassis/odom bt_loop_duration: 20 default_server_timeout: 40 # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: @@ -127,7 +127,7 @@ velocity_smoother: velocity_timeout: 1.0 # max_accel: [1.0, 0.0, 1.25] # max_decel: [-1.0, 0.0, -1.25] - odom_topic: "odom" + odom_topic: "chassis/odom" odom_duration: 0.1 controller_server: @@ -232,7 +232,7 @@ local_costmap: unknown_threshold: 15 observation_sources: pointcloud pointcloud: # no frame set, uses frame from message - topic: /carter2/front_3d_lidar/point_cloud + topic: /carter2/front_3d_lidar/lidar_points max_obstacle_height: 2.0 min_obstacle_height: 0.1 obstacle_max_range: 10.0 diff --git a/humble_ws/src/navigation/carter_navigation/params/hospital/multi_robot_carter_navigation_params_3.yaml b/humble_ws/src/navigation/carter_navigation/params/hospital/multi_robot_carter_navigation_params_3.yaml index 7281b52..a236741 100644 --- a/humble_ws/src/navigation/carter_navigation/params/hospital/multi_robot_carter_navigation_params_3.yaml +++ b/humble_ws/src/navigation/carter_navigation/params/hospital/multi_robot_carter_navigation_params_3.yaml @@ -60,7 +60,7 @@ bt_navigator: use_sim_time: True global_frame: map robot_base_frame: base_link - odom_topic: odom + odom_topic: chassis/odom bt_loop_duration: 20 default_server_timeout: 40 # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: @@ -127,7 +127,7 @@ velocity_smoother: velocity_timeout: 1.0 # max_accel: [1.0, 0.0, 1.25] # max_decel: [-1.0, 0.0, -1.25] - odom_topic: "odom" + odom_topic: "chassis/odom" odom_duration: 0.1 controller_server: @@ -232,7 +232,7 @@ local_costmap: unknown_threshold: 15 observation_sources: pointcloud pointcloud: # no frame set, uses frame from message - topic: /carter3/front_3d_lidar/point_cloud + topic: /carter3/front_3d_lidar/lidar_points max_obstacle_height: 2.0 min_obstacle_height: 0.1 obstacle_max_range: 10.0 diff --git a/humble_ws/src/navigation/carter_navigation/params/office/multi_robot_carter_navigation_params_1.yaml b/humble_ws/src/navigation/carter_navigation/params/office/multi_robot_carter_navigation_params_1.yaml index 0df5cf1..2b8c677 100644 --- a/humble_ws/src/navigation/carter_navigation/params/office/multi_robot_carter_navigation_params_1.yaml +++ b/humble_ws/src/navigation/carter_navigation/params/office/multi_robot_carter_navigation_params_1.yaml @@ -60,7 +60,7 @@ bt_navigator: use_sim_time: True global_frame: map robot_base_frame: base_link - odom_topic: odom + odom_topic: chassis/odom bt_loop_duration: 20 default_server_timeout: 40 # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: @@ -127,7 +127,7 @@ velocity_smoother: velocity_timeout: 1.0 # max_accel: [1.0, 0.0, 1.25] # max_decel: [-1.0, 0.0, -1.25] - odom_topic: "odom" + odom_topic: "chassis/odom" odom_duration: 0.1 controller_server: @@ -232,7 +232,7 @@ local_costmap: unknown_threshold: 15 observation_sources: pointcloud pointcloud: # no frame set, uses frame from message - topic: /carter1/front_3d_lidar/point_cloud + topic: /carter1/front_3d_lidar/lidar_points max_obstacle_height: 2.0 min_obstacle_height: 0.1 obstacle_max_range: 10.0 diff --git a/humble_ws/src/navigation/carter_navigation/params/office/multi_robot_carter_navigation_params_2.yaml b/humble_ws/src/navigation/carter_navigation/params/office/multi_robot_carter_navigation_params_2.yaml index 15ace4f..63da6b5 100644 --- a/humble_ws/src/navigation/carter_navigation/params/office/multi_robot_carter_navigation_params_2.yaml +++ b/humble_ws/src/navigation/carter_navigation/params/office/multi_robot_carter_navigation_params_2.yaml @@ -60,7 +60,7 @@ bt_navigator: use_sim_time: True global_frame: map robot_base_frame: base_link - odom_topic: odom + odom_topic: chassis/odom bt_loop_duration: 20 default_server_timeout: 40 # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: @@ -127,7 +127,7 @@ velocity_smoother: velocity_timeout: 1.0 # max_accel: [1.0, 0.0, 1.25] # max_decel: [-1.0, 0.0, -1.25] - odom_topic: "odom" + odom_topic: "chassis/odom" odom_duration: 0.1 controller_server: @@ -232,7 +232,7 @@ local_costmap: unknown_threshold: 15 observation_sources: pointcloud pointcloud: # no frame set, uses frame from message - topic: /carter2/front_3d_lidar/point_cloud + topic: /carter2/front_3d_lidar/lidar_points max_obstacle_height: 2.0 min_obstacle_height: 0.1 obstacle_max_range: 10.0 diff --git a/humble_ws/src/navigation/carter_navigation/params/office/multi_robot_carter_navigation_params_3.yaml b/humble_ws/src/navigation/carter_navigation/params/office/multi_robot_carter_navigation_params_3.yaml index 4805f2a..052cba2 100644 --- a/humble_ws/src/navigation/carter_navigation/params/office/multi_robot_carter_navigation_params_3.yaml +++ b/humble_ws/src/navigation/carter_navigation/params/office/multi_robot_carter_navigation_params_3.yaml @@ -60,7 +60,7 @@ bt_navigator: use_sim_time: True global_frame: map robot_base_frame: base_link - odom_topic: odom + odom_topic: chassis/odom bt_loop_duration: 20 default_server_timeout: 40 # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: @@ -127,7 +127,7 @@ velocity_smoother: velocity_timeout: 1.0 # max_accel: [1.0, 0.0, 1.25] # max_decel: [-1.0, 0.0, -1.25] - odom_topic: "odom" + odom_topic: "chassis/odom" odom_duration: 0.1 controller_server: @@ -232,7 +232,7 @@ local_costmap: unknown_threshold: 15 observation_sources: pointcloud pointcloud: # no frame set, uses frame from message - topic: /carter3/front_3d_lidar/point_cloud + topic: /carter3/front_3d_lidar/lidar_points max_obstacle_height: 2.0 min_obstacle_height: 0.1 obstacle_max_range: 10.0 diff --git a/humble_ws/src/navigation/carter_navigation/rviz2/carter_navigation.rviz b/humble_ws/src/navigation/carter_navigation/rviz2/carter_navigation.rviz index 6195c2a..79c3b79 100644 --- a/humble_ws/src/navigation/carter_navigation/rviz2/carter_navigation.rviz +++ b/humble_ws/src/navigation/carter_navigation/rviz2/carter_navigation.rviz @@ -88,17 +88,17 @@ Visualization Manager: Value: true front_3d_lidar: Value: true - front_stereo_camera:imu: + front_stereo_camera_imu: Value: true - front_stereo_camera:left_rgb: + front_stereo_camera_left_optical: Value: true - front_stereo_camera:right_rgb: + front_stereo_camera_right_optical: Value: true - left_stereo_camera:imu: + left_stereo_camera_imu: Value: true - left_stereo_camera:left_rgb: + left_stereo_camera_left_optical: Value: true - left_stereo_camera:right_rgb: + left_stereo_camera_right_optical: Value: true map: Value: true @@ -110,11 +110,11 @@ Visualization Manager: Value: true rear_stereo_camera:right_rgb: Value: true - right_stereo_camera:imu: + right_stereo_camera_imu: Value: true - right_stereo_camera:left_rgb: + right_stereo_camera_left_optical: Value: true - right_stereo_camera:right_rgb: + right_stereo_camera_right_optical: Value: true Marker Scale: 1 Name: TF @@ -133,17 +133,17 @@ Visualization Manager: {} front_3d_lidar: {} - front_stereo_camera:imu: + front_stereo_camera_imu: {} - front_stereo_camera:left_rgb: + front_stereo_camera_left_optical: {} - front_stereo_camera:right_rgb: + front_stereo_camera_right_optical: {} - left_stereo_camera:imu: + left_stereo_camera_imu: {} - left_stereo_camera:left_rgb: + left_stereo_camera_left_optical: {} - left_stereo_camera:right_rgb: + left_stereo_camera_right_optical: {} rear_stereo_camera:imu: {} @@ -151,11 +151,11 @@ Visualization Manager: {} rear_stereo_camera:right_rgb: {} - right_stereo_camera:imu: + right_stereo_camera_imu: {} - right_stereo_camera:left_rgb: + right_stereo_camera_left_optical: {} - right_stereo_camera:right_rgb: + right_stereo_camera_right_optical: {} Update Interval: 0 Value: true @@ -596,7 +596,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /front_3d_lidar/point_cloud + Value: /front_3d_lidar/lidar_points Use Fixed Frame: true Use rainbow: true Value: true @@ -612,7 +612,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /front_stereo_camera/left_rgb/image_raw + Value: /front_stereo_camera/left/image_raw Value: false Enabled: true Global Options: diff --git a/humble_ws/src/navigation/isaac_ros_navigation_goal/launch/isaac_ros_navigation_goal.launch.py b/humble_ws/src/navigation/isaac_ros_navigation_goal/launch/isaac_ros_navigation_goal.launch.py index 2a63f0a..4f3d712 100644 --- a/humble_ws/src/navigation/isaac_ros_navigation_goal/launch/isaac_ros_navigation_goal.launch.py +++ b/humble_ws/src/navigation/isaac_ros_navigation_goal/launch/isaac_ros_navigation_goal.launch.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. +# Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. # # NVIDIA CORPORATION and its licensors retain all intellectual property # and proprietary rights in and to this software, related documentation diff --git a/humble_ws/src/navigation/isaac_ros_navigation_goal/package.xml b/humble_ws/src/navigation/isaac_ros_navigation_goal/package.xml index b6a526a..bd666eb 100644 --- a/humble_ws/src/navigation/isaac_ros_navigation_goal/package.xml +++ b/humble_ws/src/navigation/isaac_ros_navigation_goal/package.xml @@ -5,9 +5,9 @@ 0.1.0 Package to set goals for navigation stack. - isaac sim + Isaac Sim - Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. + Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. NVIDIA CORPORATION and its licensors retain all intellectual property and proprietary rights in and to this software, related documentation and any modifications thereto. Any use, reproduction, disclosure or @@ -15,7 +15,7 @@ license agreement from NVIDIA CORPORATION is strictly prohibited. https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html - https://forums.developer.nvidia.com/c/agx-autonomous-machines/isaac/simulation + https://forums.developer.nvidia.com/c/omniverse/simulation/69 https://developer.nvidia.com/isaac-ros-gems/ rclpy diff --git a/noetic_ws/src/cortex_control/package.xml b/noetic_ws/src/cortex_control/package.xml index 13dd4ec..3459ecc 100644 --- a/noetic_ws/src/cortex_control/package.xml +++ b/noetic_ws/src/cortex_control/package.xml @@ -3,13 +3,17 @@ 1.0.0 NVIDIA Isaac Cortex Control ROS package. - Nathan Ratliff - Nathan Ratliff - - See LICENSE file. + Isaac Sim + Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. + NVIDIA CORPORATION and its licensors retain all intellectual property + and proprietary rights in and to this software, related documentation + and any modifications thereto. Any use, reproduction, disclosure or + distribution of this software and related documentation without an express + license agreement from NVIDIA CORPORATION is strictly prohibited. + https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html - https://forums.developer.nvidia.com/c/agx-autonomous-machines/isaac/simulation + https://forums.developer.nvidia.com/c/omniverse/simulation/69 catkin diff --git a/noetic_ws/src/cortex_control_franka/package.xml b/noetic_ws/src/cortex_control_franka/package.xml index eaaebf4..30307ca 100644 --- a/noetic_ws/src/cortex_control_franka/package.xml +++ b/noetic_ws/src/cortex_control_franka/package.xml @@ -3,11 +3,18 @@ cortex_control_franka 0.1.0 Cortex Control ROS extensions with Franka ROS dependencies. - Nathan Ratliff - BSD - + + Isaac Sim + + Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. + NVIDIA CORPORATION and its licensors retain all intellectual property + and proprietary rights in and to this software, related documentation + and any modifications thereto. Any use, reproduction, disclosure or + distribution of this software and related documentation without an express + license agreement from NVIDIA CORPORATION is strictly prohibited. + https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html - https://forums.developer.nvidia.com/c/agx-autonomous-machines/isaac/simulation + https://forums.developer.nvidia.com/c/omniverse/simulation/69 catkin diff --git a/noetic_ws/src/isaac_moveit/package.xml b/noetic_ws/src/isaac_moveit/package.xml index a6fd9bc..b095793 100644 --- a/noetic_ws/src/isaac_moveit/package.xml +++ b/noetic_ws/src/isaac_moveit/package.xml @@ -4,9 +4,9 @@ 0.1.0 The moveit samples package - isaac sim + Isaac Sim - Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. + Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. NVIDIA CORPORATION and its licensors retain all intellectual property and proprietary rights in and to this software, related documentation and any modifications thereto. Any use, reproduction, disclosure or @@ -14,7 +14,7 @@ license agreement from NVIDIA CORPORATION is strictly prohibited. https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html - https://forums.developer.nvidia.com/c/agx-autonomous-machines/isaac/simulation + https://forums.developer.nvidia.com/c/omniverse/simulation/69 catkin rospy diff --git a/noetic_ws/src/isaac_ros_messages/package.xml b/noetic_ws/src/isaac_ros_messages/package.xml index d4f4d74..94ce5dc 100644 --- a/noetic_ws/src/isaac_ros_messages/package.xml +++ b/noetic_ws/src/isaac_ros_messages/package.xml @@ -3,9 +3,9 @@ isaac_ros_messages 0.1.0 The isaac_ros_messages package - isaac sim + Isaac Sim - Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. + Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. NVIDIA CORPORATION and its licensors retain all intellectual property and proprietary rights in and to this software, related documentation and any modifications thereto. Any use, reproduction, disclosure or @@ -13,7 +13,7 @@ license agreement from NVIDIA CORPORATION is strictly prohibited. https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html - https://forums.developer.nvidia.com/c/agx-autonomous-machines/isaac/simulation + https://forums.developer.nvidia.com/c/omniverse/simulation/69 catkin geometry_msgs diff --git a/noetic_ws/src/isaac_tutorials/CMakeLists.txt b/noetic_ws/src/isaac_tutorials/CMakeLists.txt index eaef577..5ae6dcc 100644 --- a/noetic_ws/src/isaac_tutorials/CMakeLists.txt +++ b/noetic_ws/src/isaac_tutorials/CMakeLists.txt @@ -13,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS message_generation std_msgs geometry_msgs + ackermann_msgs ) @@ -164,6 +165,7 @@ include_directories( ## in contrast to setup.py, you can choose the destination catkin_install_python(PROGRAMS scripts/ros_publisher.py + scripts/ros_ackermann_publisher.py scripts/ros_service_client.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/noetic_ws/src/isaac_tutorials/package.xml b/noetic_ws/src/isaac_tutorials/package.xml index 123fb6c..5d1186a 100644 --- a/noetic_ws/src/isaac_tutorials/package.xml +++ b/noetic_ws/src/isaac_tutorials/package.xml @@ -4,9 +4,9 @@ 0.1.0 The isaac_tutorials package - isaac sim + Isaac Sim - Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. + Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. NVIDIA CORPORATION and its licensors retain all intellectual property and proprietary rights in and to this software, related documentation and any modifications thereto. Any use, reproduction, disclosure or @@ -14,10 +14,11 @@ license agreement from NVIDIA CORPORATION is strictly prohibited. https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html - https://forums.developer.nvidia.com/c/agx-autonomous-machines/isaac/simulation + https://forums.developer.nvidia.com/c/omniverse/simulation/69 message_generation message_runtime + ackermann_msgs catkin diff --git a/noetic_ws/src/isaac_tutorials/scripts/ros_ackermann_publisher.py b/noetic_ws/src/isaac_tutorials/scripts/ros_ackermann_publisher.py new file mode 100755 index 0000000..1980c6e --- /dev/null +++ b/noetic_ws/src/isaac_tutorials/scripts/ros_ackermann_publisher.py @@ -0,0 +1,38 @@ +#!/usr/bin/env python + +# Copyright (c) 2021-2024, NVIDIA CORPORATION. All rights reserved. +# +# NVIDIA CORPORATION and its licensors retain all intellectual property +# and proprietary rights in and to this software, related documentation +# and any modifications thereto. Any use, reproduction, disclosure or +# distribution of this software and related documentation without an express +# license agreement from NVIDIA CORPORATION is strictly prohibited. + +import rospy +from ackermann_msgs.msg import AckermannDriveStamped +import numpy as np + +rospy.init_node("test_rosbridge", anonymous=True) + +pub = rospy.Publisher("/ackermann_cmd", AckermannDriveStamped, queue_size=10) + +msg = AckermannDriveStamped() + +i = 0 + +rate = rospy.Rate(20) +while not rospy.is_shutdown(): + + # Only command forklift using acceleration and steering angle + degrees1 = np.arange(0, 60) + degrees2 = np.arange(-60, 0) + degrees = np.concatenate((degrees1, degrees1[::-1], degrees2[::-1], degrees2)) + msg.header.frame_id = "forklift" + msg.header.stamp = rospy.Time.now() + msg.drive.steering_angle = 0.0174533 * (degrees[i % degrees.size]) + msg.drive.acceleration = float(degrees[i % degrees.size]) + + pub.publish(msg) + + i += 1 + rate.sleep() diff --git a/noetic_ws/src/isaac_vins/CMakeLists.txt b/noetic_ws/src/isaac_vins/CMakeLists.txt deleted file mode 100644 index b42e8f0..0000000 --- a/noetic_ws/src/isaac_vins/CMakeLists.txt +++ /dev/null @@ -1,207 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(isaac_vins) - -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - roscpp - rospy - sensor_msgs - std_msgs -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# sensor_msgs# std_msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES a1_launch -# CATKIN_DEPENDS roscpp rospy sensor_msgs std_msgs -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/a1_launch.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/a1_launch_node.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_a1_launch.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/noetic_ws/src/isaac_vins/config/isaac_a1/isaac_left.yaml b/noetic_ws/src/isaac_vins/config/isaac_a1/isaac_left.yaml deleted file mode 100644 index 8a8f278..0000000 --- a/noetic_ws/src/isaac_vins/config/isaac_a1/isaac_left.yaml +++ /dev/null @@ -1,16 +0,0 @@ -%YAML:1.0 ---- -model_type: PINHOLE -camera_name: camera -image_width: 640 -image_height: 480 -distortion_parameters: - k1: 0.0 - k2: 0.0 - p1: 0.0 - p2: 0.0 -projection_parameters: - fx: 732.999267578125 - fy: 732.9993286132812 - cx: 320 - cy: 240 diff --git a/noetic_ws/src/isaac_vins/config/isaac_a1/isaac_right.yaml b/noetic_ws/src/isaac_vins/config/isaac_a1/isaac_right.yaml deleted file mode 100644 index 8a8f278..0000000 --- a/noetic_ws/src/isaac_vins/config/isaac_a1/isaac_right.yaml +++ /dev/null @@ -1,16 +0,0 @@ -%YAML:1.0 ---- -model_type: PINHOLE -camera_name: camera -image_width: 640 -image_height: 480 -distortion_parameters: - k1: 0.0 - k2: 0.0 - p1: 0.0 - p2: 0.0 -projection_parameters: - fx: 732.999267578125 - fy: 732.9993286132812 - cx: 320 - cy: 240 diff --git a/noetic_ws/src/isaac_vins/config/isaac_a1/vins_fusion_isaac_a1.yaml b/noetic_ws/src/isaac_vins/config/isaac_a1/vins_fusion_isaac_a1.yaml deleted file mode 100644 index 3814b14..0000000 --- a/noetic_ws/src/isaac_vins/config/isaac_a1/vins_fusion_isaac_a1.yaml +++ /dev/null @@ -1,71 +0,0 @@ -%YAML:1.0 - -#common parameters -#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam; -imu: 0 -num_of_cam: 2 - -imu_topic: "/isaac_a1/imu_data" -image0_topic: "/isaac_a1/camera_forward/camera_left/rgb" -image1_topic: "/isaac_a1/camera_forward/camera_right/rgb" -output_path: "~/output" - -cam0_calib: "isaac_left.yaml" -cam1_calib: "isaac_right.yaml" -image_width: 640 -image_height: 480 - - -# Extrinsic parameter between IMU and Camera. -estimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. - # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. - -body_T_cam0: !!opencv-matrix - rows: 4 - cols: 4 - dt: d - data: [ 0, 0, 1, 0.2693, - -1, 0, 0, 0.025, - 0, -1, 0, 0.067, - 0., 0., 0., 1. ] - -body_T_cam1: !!opencv-matrix - rows: 4 - cols: 4 - dt: d - data: [ 0, 0, 1, 0.2693, - -1, 0, 0, -0.025, - 0, -1, 0, 0.067, - 0., 0., 0., 1. ] - -#Multiple thread support -multiple_thread: 0 - -#feature traker paprameters -max_cnt: 150 # max feature number in feature tracking -min_dist: 10 # min distance between two features -freq: 15 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image -F_threshold: 1.0 # ransac threshold (pixel) -show_track: 1 # publish tracking image as topic -flow_back: 1 # perform forward and backward optical flow to improve feature tracking accuracy - -#optimization parameters -max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time -max_num_iterations: 8 # max solver itrations, to guarantee real time -keyframe_parallax: 10.0 # keyframe selection threshold (pixel) - -#imu parameters The more accurate parameters you provide, the better performance -acc_n: 0.5 # accelerometer measurement noise standard deviation. #0.2 0.04 -gyr_n: 0.1 # gyroscope measurement noise standard deviation. #0.05 0.004 -acc_w: 0.001 # accelerometer bias random work noise standard deviation. #0.002 -gyr_w: 0.0001 # gyroscope bias random work noise standard deviation. #4.0e-5 -g_norm: 9.805 # gravity magnitude - -#unsynchronization parameters -estimate_td: 0 # online estimate time offset between camera and imu -td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) - -#loop closure parameters -load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path' -pose_graph_save_path: "~/output/pose_graph/" # save and load path -save_image: 0 # save image in pose graph for visualization prupose; you can close this function by setting 0 diff --git a/noetic_ws/src/isaac_vins/launch/isaac_a1_vins.launch b/noetic_ws/src/isaac_vins/launch/isaac_a1_vins.launch deleted file mode 100644 index 76dda9a..0000000 --- a/noetic_ws/src/isaac_vins/launch/isaac_a1_vins.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/noetic_ws/src/isaac_vins/package.xml b/noetic_ws/src/isaac_vins/package.xml deleted file mode 100644 index 5948f3b..0000000 --- a/noetic_ws/src/isaac_vins/package.xml +++ /dev/null @@ -1,38 +0,0 @@ - - - isaac_vins - 1.0.1 - isaac sim visual inertia odometry package demo, currently supports Unitree A1 Quadruped - - isaac sim - - Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. - NVIDIA CORPORATION and its licensors retain all intellectual property - and proprietary rights in and to this software, related documentation - and any modifications thereto. Any use, reproduction, disclosure or - distribution of this software and related documentation without an express - license agreement from NVIDIA CORPORATION is strictly prohibited. - - https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html - https://forums.developer.nvidia.com/c/agx-autonomous-machines/isaac/simulation - - catkin - roscpp - rospy - sensor_msgs - std_msgs - roscpp - rospy - sensor_msgs - std_msgs - roscpp - rospy - sensor_msgs - std_msgs - - - - - - - diff --git a/noetic_ws/src/isaac_vins/rviz/isaac_vins_rviz_config.rviz b/noetic_ws/src/isaac_vins/rviz/isaac_vins_rviz_config.rviz deleted file mode 100644 index 6b96144..0000000 --- a/noetic_ws/src/isaac_vins/rviz/isaac_vins_rviz_config.rviz +++ /dev/null @@ -1,468 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /VIOGroup1 - - /VIOGroup1/VIOPath1 - - /VIOGroup1/CameraMarker1 - - /VIOGroup1/PointCloud1 - - /VIOGroup1/HistoryPointCloud1 - - /VIOGroup1/track_image1 - - /GlobalGroup1/CarModel1 - Splitter Ratio: 0.4651159942150116 - Tree Height: 1856 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: PointCloud - - Class: rviz/Displays - Help Height: 138 - Name: Displays - Property Tree Widget: - Expanded: ~ - Splitter Ratio: 0.5 - Tree Height: 270 -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 1 - Class: rviz/Axes - Enabled: true - Length: 0.5 - Name: Axes - Radius: 0.05000000074505806 - Reference Frame: - Show Trail: false - Value: true - - Class: rviz/Group - Displays: - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /loop_fusion/pose_graph - Name: loopLink - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /loop_fusion/camera_pose_visual - Name: loopCamera - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 204; 0; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.03999999910593033 - Name: loopPath - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Queue Size: 10 - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /loop_fusion/pose_graph_path - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: -0.00362956034950912 - Min Value: -0.15546569228172302 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud - Color: 255; 255; 255 - Color Transformer: FlatColor - Decay Time: 10 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: PointCloud - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 2 - Size (m): 0.009999999776482582 - Style: Points - Topic: /loop_fusion/margin_cloud_loop_rect - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud - Color: 0; 255; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: PointCloud - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 4 - Size (m): 0.009999999776482582 - Style: Points - Topic: /loop_fusion/point_cloud_loop_rect - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz/Image - Enabled: false - Image Topic: /loop_fusion/match_image - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: loop_match_Image - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: LoopGroup - - Class: rviz/Group - Displays: - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 25; 255; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: VIOPath - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Queue Size: 10 - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /vinsfusion/path - Unreliable: false - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /vinsfusion/camera_pose_visual - Name: CameraMarker - Namespaces: - CameraPoseVisualization: true - Queue Size: 100 - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: PointCloud - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 2 - Size (m): 0.009999999776482582 - Style: Points - Topic: /vinsfusion/point_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud - Color: 0; 255; 0 - Color Transformer: FlatColor - Decay Time: 100 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: HistoryPointCloud - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 1 - Size (m): 0.009999999776482582 - Style: Points - Topic: /vinsfusion/margin_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /vinsfusion/image_track - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: track_image - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Enabled: true - Name: VIOGroup - - Class: rviz/Group - Displays: - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 0; 0; 255 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: GloablPath - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Queue Size: 10 - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /globalEstimator/global_path - Unreliable: false - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /globalEstimator/car_model - Name: CarModel - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: true - Name: GlobalGroup - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: keyframe_pt - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares - Topic: /vinsfusion/keyframe_point - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /vinsfusion/key_poses - Name: key_poses - Namespaces: - key_poses: true - Queue Size: 100 - Value: true - - Angle Tolerance: 0.10000000149011612 - Class: rviz/Odometry - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: true - Keep: 100 - Name: keyframe_pose - Position Tolerance: 0.10000000149011612 - Queue Size: 10 - Shape: - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Color: 255; 25; 0 - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Value: Arrow - Topic: /vinsfusion/keyframe_pose - Unreliable: false - Value: true - Enabled: true - Global Options: - Background Color: 0; 0; 0 - Default Light: true - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/ThirdPersonFollower - Distance: 86.59434509277344 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.7853981852531433 - Focal Point: - X: -5.432063579559326 - Y: -4.7521138191223145 - Z: 0.38143399357795715 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 1.0797959566116333 - Target Frame: world - Yaw: 3.540623188018799 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 2230 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 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 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 3952 - X: 144 - Y: 0 - loop_match_Image: - collapsed: false - track_image: - collapsed: false diff --git a/noetic_ws/src/navigation/carter_2dnav/package.xml b/noetic_ws/src/navigation/carter_2dnav/package.xml index 0cdfcbb..72e5445 100644 --- a/noetic_ws/src/navigation/carter_2dnav/package.xml +++ b/noetic_ws/src/navigation/carter_2dnav/package.xml @@ -4,9 +4,9 @@ 0.1.0 The carter_2dnav package - isaac sim + Isaac Sim - Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. + Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. NVIDIA CORPORATION and its licensors retain all intellectual property and proprietary rights in and to this software, related documentation and any modifications thereto. Any use, reproduction, disclosure or @@ -14,7 +14,7 @@ license agreement from NVIDIA CORPORATION is strictly prohibited. https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html - https://forums.developer.nvidia.com/c/agx-autonomous-machines/isaac/simulation + https://forums.developer.nvidia.com/c/omniverse/simulation/69 catkin navigation diff --git a/noetic_ws/src/navigation/carter_description/package.xml b/noetic_ws/src/navigation/carter_description/package.xml index 5130233..3e2d912 100644 --- a/noetic_ws/src/navigation/carter_description/package.xml +++ b/noetic_ws/src/navigation/carter_description/package.xml @@ -4,9 +4,9 @@ 0.1.0 The carter_description package - isaac sim + Isaac Sim - Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. + Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. NVIDIA CORPORATION and its licensors retain all intellectual property and proprietary rights in and to this software, related documentation and any modifications thereto. Any use, reproduction, disclosure or @@ -14,7 +14,7 @@ license agreement from NVIDIA CORPORATION is strictly prohibited. https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html - https://forums.developer.nvidia.com/c/agx-autonomous-machines/isaac/simulation + https://forums.developer.nvidia.com/c/omniverse/simulation/69 catkin geometry_msgs diff --git a/noetic_ws/src/navigation/isaac_ros_navigation_goal/package.xml b/noetic_ws/src/navigation/isaac_ros_navigation_goal/package.xml index 67fbb90..c0acf71 100644 --- a/noetic_ws/src/navigation/isaac_ros_navigation_goal/package.xml +++ b/noetic_ws/src/navigation/isaac_ros_navigation_goal/package.xml @@ -4,9 +4,9 @@ 0.1.0 Package to set goals for navigation stack. - isaac sim + Isaac Sim - Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. + Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. NVIDIA CORPORATION and its licensors retain all intellectual property and proprietary rights in and to this software, related documentation and any modifications thereto. Any use, reproduction, disclosure or @@ -15,7 +15,7 @@ https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html - https://forums.developer.nvidia.com/c/agx-autonomous-machines/isaac/simulation + https://forums.developer.nvidia.com/c/omniverse/simulation/69 https://developer.nvidia.com/isaac-ros-gems/ catkin