├── .gitignore ├── CHANGELOG.md ├── LICENSE ├── README.md ├── build_humble.sh ├── humble_ws ├── fastdds.xml └── src │ ├── ackermann_control │ └── cmdvel_to_ackermann │ │ ├── CMakeLists.txt │ │ ├── launch │ │ └── cmdvel_to_ackermann.launch.py │ │ ├── package.xml │ │ └── scripts │ │ ├── __init__.py │ │ └── cmdvel_to_ackermann.py │ ├── custom_message │ ├── CMakeLists.txt │ ├── msg │ │ └── SampleMsg.msg │ └── package.xml │ ├── isaac_ros2_messages │ ├── CMakeLists.txt │ ├── package.xml │ └── srv │ │ ├── GetPrimAttribute.srv │ │ ├── GetPrimAttributes.srv │ │ ├── GetPrims.srv │ │ ├── IsaacPose.srv │ │ └── SetPrimAttribute.srv │ ├── isaac_tutorials │ ├── CMakeLists.txt │ ├── package.xml │ ├── rviz2 │ │ ├── camera_lidar.rviz │ │ ├── camera_manual.rviz │ │ ├── carter_stereo.rviz │ │ ├── rtx_lidar.rviz │ │ └── turtle_stereo.rviz │ └── scripts │ │ ├── ros2_ackermann_publisher.py │ │ └── ros2_publisher.py │ ├── isaacsim │ ├── CMakeLists.txt │ ├── launch │ │ └── run_isaacsim.launch.py │ ├── package.xml │ └── scripts │ │ ├── open_isaacsim_stage.py │ │ └── run_isaacsim.py │ └── navigation │ ├── carter_navigation │ ├── CMakeLists.txt │ ├── launch │ │ ├── carter_navigation.launch.py │ │ ├── carter_navigation_individual.launch.py │ │ ├── carter_navigation_isaacsim.launch.py │ │ ├── multiple_robot_carter_navigation_hospital.launch.py │ │ └── multiple_robot_carter_navigation_office.launch.py │ ├── maps │ │ ├── carter_hospital_navigation.png │ │ ├── carter_hospital_navigation.yaml │ │ ├── carter_office_navigation.png │ │ ├── carter_office_navigation.yaml │ │ ├── carter_warehouse_navigation.png │ │ └── carter_warehouse_navigation.yaml │ ├── package.xml │ ├── params │ │ ├── carter_navigation_params.yaml │ │ ├── hospital │ │ │ ├── multi_robot_carter_navigation_params_1.yaml │ │ │ ├── multi_robot_carter_navigation_params_2.yaml │ │ │ └── multi_robot_carter_navigation_params_3.yaml │ │ └── office │ │ │ ├── multi_robot_carter_navigation_params_1.yaml │ │ │ ├── multi_robot_carter_navigation_params_2.yaml │ │ │ └── multi_robot_carter_navigation_params_3.yaml │ └── rviz2 │ │ ├── carter_navigation.rviz │ │ └── carter_navigation_namespaced.rviz │ ├── isaac_ros_navigation_goal │ ├── CMakeLists.txt │ ├── assets │ │ ├── carter_warehouse_navigation.png │ │ ├── carter_warehouse_navigation.yaml │ │ └── goals.txt │ ├── isaac_ros_navigation_goal │ │ ├── __init__.py │ │ ├── goal_generators │ │ │ ├── __init__.py │ │ │ ├── goal_generator.py │ │ │ ├── goal_reader.py │ │ │ └── random_goal_generator.py │ │ ├── obstacle_map.py │ │ └── set_goal.py │ ├── launch │ │ └── isaac_ros_navigation_goal.launch.py │ ├── package.xml │ ├── resource │ │ └── isaac_ros_navigation_goal │ ├── setup.cfg │ ├── setup.py │ └── test │ │ ├── test_flake8.py │ │ └── test_pep257.py │ └── iw_hub_navigation │ ├── CMakeLists.txt │ ├── launch │ ├── iw_hub_navigation.launch.py │ └── iw_hub_navigation_isaacsim.launch.py │ ├── maps │ ├── iw_hub_warehouse_navigation.png │ └── iw_hub_warehouse_navigation.yaml │ ├── package.xml │ ├── params │ └── iw_hub_navigation_params.yaml │ └── rviz2 │ └── iw_hub_navigation.rviz ├── noetic_ws └── src │ ├── cortex_control │ ├── CMakeLists.txt │ ├── config │ │ └── command_stream_interpolator.yaml │ ├── msg │ │ ├── CortexCommandAck.msg │ │ └── JointPosVelAccCommand.msg │ ├── package.xml │ ├── src │ │ └── cortex │ │ │ ├── control │ │ │ ├── builders.cpp │ │ │ ├── builders.h │ │ │ ├── command_stream_interpolator.cpp │ │ │ ├── command_stream_interpolator.h │ │ │ ├── command_stream_interpolator_main.cpp │ │ │ ├── joint_pos_vel_acc_command_publisher.cpp │ │ │ ├── joint_pos_vel_acc_command_publisher.h │ │ │ ├── rmpflow_commanded_joints_listener.cpp │ │ │ └── rmpflow_commanded_joints_listener.h │ │ │ ├── math │ │ │ ├── interpolation │ │ │ │ ├── cubic_position_interpolator.cpp │ │ │ │ ├── cubic_position_interpolator.h │ │ │ │ ├── incremental_interpolator.cpp │ │ │ │ ├── incremental_interpolator.h │ │ │ │ ├── interpolator.h │ │ │ │ ├── pos_vel_acc.h │ │ │ │ ├── quartic_interpolator.cpp │ │ │ │ ├── quartic_interpolator.h │ │ │ │ ├── quintic_interpolator.cpp │ │ │ │ ├── quintic_interpolator.h │ │ │ │ ├── smoothing_incremental_interpolator.cpp │ │ │ │ ├── smoothing_incremental_interpolator.h │ │ │ │ ├── time_scaled_interpolator.h │ │ │ │ └── trajectories.h │ │ │ ├── state.cpp │ │ │ └── state.h │ │ │ └── util │ │ │ ├── joint_state_listener.cpp │ │ │ ├── joint_state_listener.h │ │ │ ├── joint_state_publisher.cpp │ │ │ ├── joint_state_publisher.h │ │ │ ├── ros_message_listener.h │ │ │ ├── ros_util.cpp │ │ │ ├── ros_util.h │ │ │ ├── set_state_listener.h │ │ │ ├── stamped_state.cpp │ │ │ ├── stamped_state.h │ │ │ ├── state_listener.cpp │ │ │ ├── state_listener.h │ │ │ ├── string.cpp │ │ │ ├── string.h │ │ │ └── yaml.h │ └── srv │ │ └── MsgService.srv │ ├── cortex_control_franka │ ├── CMakeLists.txt │ ├── cfg │ │ ├── compliance_param.cfg │ │ └── desired_mass_param.cfg │ ├── config │ │ ├── command_stream_interpolator.yaml │ │ └── controller.yaml │ ├── include │ │ └── cortex │ │ │ └── control │ │ │ └── franka │ │ │ └── interpolated_command_stream_controller.h │ ├── launch │ │ ├── franka_control_cortex.launch │ │ └── joint_position_controller.launch │ ├── package.xml │ ├── plugin.xml │ └── src │ │ ├── interpolated_command_stream_controller.cpp │ │ └── python │ │ ├── franka_gripper_command_relay.py │ │ ├── franka_gripper_commander.py │ │ └── set_high_collision_thresholds.py │ ├── isaac_moveit │ ├── CMakeLists.txt │ ├── launch │ │ └── franka_isaac_execution.launch │ ├── package.xml │ ├── rviz │ │ └── isaac_moveit_config.rviz │ └── scripts │ │ └── panda_combined_joints_publisher.py │ ├── isaac_ros_messages │ ├── CMakeLists.txt │ ├── package.xml │ └── srv │ │ └── IsaacPose.srv │ ├── isaac_tutorials │ ├── CMakeLists.txt │ ├── launch │ │ └── apriltag_continuous_detection.launch │ ├── msg │ │ └── ContactSensor.msg │ ├── package.xml │ ├── rviz │ │ ├── apriltag_config.rviz │ │ ├── camera_lidar.rviz │ │ ├── camera_manual.rviz │ │ ├── carter_stereo.rviz │ │ ├── rtx_lidar.rviz │ │ └── turtlebot_config.rviz │ └── scripts │ │ ├── ros_ackermann_publisher.py │ │ ├── ros_publisher.py │ │ └── ros_service_client.py │ └── navigation │ ├── carter_2dnav │ ├── CMakeLists.txt │ ├── launch │ │ ├── amcl_robot_individual.launch │ │ ├── carter_navigation.launch │ │ ├── carter_slam_gmapping.launch │ │ ├── move_base_individual.launch │ │ └── multiple_robot_carter_navigation.launch │ ├── map │ │ ├── carter_hospital_navigation.png │ │ ├── carter_hospital_navigation.yaml │ │ ├── carter_office_navigation.png │ │ ├── carter_office_navigation.yaml │ │ ├── carter_warehouse_navigation.png │ │ └── carter_warehouse_navigation.yaml │ ├── package.xml │ ├── params │ │ ├── base_local_planner_params.yaml │ │ ├── costmap_common_params.yaml │ │ ├── global_costmap_params.yaml │ │ └── local_costmap_params.yaml │ └── rviz │ │ ├── carter_2dnav.rviz │ │ ├── carter_2dnav_multiple_robot.rviz │ │ └── carter_slam_gmapping.rviz │ ├── carter_description │ ├── CMakeLists.txt │ ├── package.xml │ └── urdf │ │ ├── carter.urdf │ │ └── meshes │ │ ├── caster_wheel.obj │ │ ├── chassis.obj │ │ ├── pivot.obj │ │ └── side_wheel.obj │ └── isaac_ros_navigation_goal │ ├── CMakeLists.txt │ ├── assets │ ├── carter_warehouse_navigation.png │ ├── carter_warehouse_navigation.yaml │ └── goals.txt │ ├── isaac_ros_navigation_goal │ ├── __init__.py │ ├── goal_generators │ │ ├── __init__.py │ │ ├── goal_generator.py │ │ ├── goal_reader.py │ │ └── random_goal_generator.py │ ├── obstacle_map.py │ └── set_goal.py │ ├── launch │ └── isaac_ros_navigation_goal.launch │ ├── package.xml │ └── setup.py └── ubuntu_20_humble_minimal.dockerfile /.gitignore: -------------------------------------------------------------------------------- 1 | noetic_ws/build 2 | noetic_ws/devel 3 | foxy_ws/build 4 | foxy_ws/install 5 | foxy_ws/log 6 | humble_ws/build 7 | humble_ws/install 8 | humble_ws/log -------------------------------------------------------------------------------- /CHANGELOG.md: -------------------------------------------------------------------------------- 1 | # Changelog 2 | 3 | 4 | ## [3.3.1] - 2025-01-24 5 | 6 | ### Changed 7 | - Updated fixed frame name to base_scan in `isaac_tutorials` rtx_lidar.rviz config file [Humble] 8 | - Added point cloud topic subscriber in `isaac_tutorials` camera_lidar.rviz config file [Humble] 9 | 10 | ## [3.3.0] - 2025-01-21 11 | 12 | ### Changed 13 | - Updated `isaacsim` run_isaacsim.py to use new commands for "webrtc" headless option [Humble] 14 | - Removed deprecated "native" headless option from `isaacsim` run_isaacsim.py [Humble] 15 | 16 | ## [3.2.3] - 2025-01-21 17 | 18 | ### Changed 19 | - Changed `isaacsim` run_isaacsim.py to have "humble" as default distro for ROS2 [Humble] 20 | 21 | ## [3.2.2] - 2024-01-20 22 | 23 | ### Changed 24 | - Updated ros2_ackeramnn_publisher.py to publish zero-velocity on interrupt [Humble] 25 | 26 | ## [3.2.1] - 2024-01-07 27 | 28 | ### Changed 29 | - Changed `isaac_tutorials` ros2 ackermann publisher message frame id to "ackeramnn" [Humble] 30 | 31 | ## [3.2.0] - 2024-12-16 32 | 33 | ### Changed 34 | - Bumped versions in `isaacsim` package to Isaac Sim version 4.5.0 [Humble] 35 | - Updated all asset paths referenced in ROS 2 packages to Isaac Sim 4.5 convention 36 | 37 | ## [3.1.1] - 2024-11-27 38 | 39 | ### Changed 40 | - Updated ros2_ackeramnn_publisher.py to publish velocity command [Humble] 41 | 42 | ## [3.1.0] - 2024-11-22 43 | 44 | ### Added 45 | - `cmdvel_to_ackermann` package for ackeramnn control [Humble] 46 | 47 | ## [3.0.1] - 2024-11-22 48 | 49 | ### Fixed 50 | - updated ubuntu_20_humble_minimal.dockerfile to build all packages [Humble] 51 | 52 | ## [3.0.0] - 2024-11-15 53 | 54 | ### Removed 55 | - Removed all Foxy packages and dockerfiles [Foxy] 56 | 57 | ## [2.1.0] - 2024-10-03 58 | 59 | ### Added 60 | - Option in `isaacsim` package to launch isaac-sim.sh with custom args [Humble] 61 | 62 | ## [2.0.0] - 2024-09-20 63 | 64 | ### Added 65 | - New `iw_hub_navigation` package to run Nav2 with new iw.hub robot in new environment [Foxy, Humble] 66 | - New RViz config for TurtleBot tutorials [Noetic] 67 | - New RViz config and launch file with Carter robot for SLAM with gmapping 68 | 69 | ### Changed 70 | - Bumped versions in `isaacsim` package to Isaac Sim version 4.2.0 [Foxy, Humble] 71 | - Changed `carter_2dnav` package to only use RTX Lidar [Noetic] 72 | - Updated dockerfiles to use setuptools 70.0.0 [Humble, Foxy] 73 | - Updated QoS settings for image subscribers in ``carter_stereo.rviz`` and ``carter_navigation.rviz`` config files. [Foxy, Humble] 74 | 75 | ## [1.1.0] - 2024-08-01 76 | 77 | ### Added 78 | - Option in `isaacsim` package to launch Isaac Sim in headless mode [Foxy, Humble] 79 | 80 | ### Changed 81 | - Bumped versions in `isaacsim` package to Isaac Sim version 4.1.0 [Foxy, Humble] 82 | 83 | 84 | ## [1.0.0] - 2024-05-28 85 | 86 | ### Added 87 | - Ackermann publisher script for Ackermann Steering tutorial [Noetic, Foxy, Humble] 88 | - New `isaacsim` package to enable running Isaac Sim as a ROS2 node or from a ROS2 launch file! [Foxy, Humble] 89 | - `isaac_ros2_messages` service interfaces for listing prims and manipulate their attributes [Foxy, Humble] 90 | 91 | ### Removed 92 | - Removed support for quadruped VINS Fusion example [Noetic] 93 | 94 | ## [0.1.0] - 2023-12-18 95 | ### Added 96 | - Noetic, Foxy, Humble workspaces for Isaac Sim 2023.1.1 97 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Isaac Sim ROS & ROS2 Workspaces 2 | 3 | This repository contains two workspaces: `noetic_ws` (ROS Noetic) and `humble_ws` (ROS2 Humble). 4 | 5 | [Click here for usage and installation instructions with Isaac Sim](https://docs.isaacsim.omniverse.nvidia.com/4.5.0/installation/install_ros.html) 6 | 7 | When cloning this repository, both workspaces are downloaded. Depending on which ROS distro you are using, follow the [setup instructions](https://docs.isaacsim.omniverse.nvidia.com/4.5.0/installation/install_ros.html#setting-up-workspaces) for building your specific workspace. -------------------------------------------------------------------------------- /build_humble.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -e 4 | SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" 5 | 6 | docker build . --network=host -f ubuntu_20_humble_minimal.dockerfile -t isaac_sim_ros:ubuntu_20_humble 7 | 8 | rm -rf build_ws/humble 9 | mkdir -p build_ws/humble 10 | 11 | pushd build_ws/humble 12 | 13 | docker cp $(docker create --rm isaac_sim_ros:ubuntu_20_humble):/workspace/humble_ws humble_ws 14 | 15 | docker cp $(docker create --rm isaac_sim_ros:ubuntu_20_humble):/workspace/build_ws isaac_sim_ros_ws 16 | 17 | popd -------------------------------------------------------------------------------- /humble_ws/fastdds.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. 4 | NVIDIA CORPORATION and its licensors retain all intellectual property 5 | and proprietary rights in and to this software, related documentation 6 | and any modifications thereto. Any use, reproduction, disclosure or 7 | distribution of this software and related documentation without an express 8 | license agreement from NVIDIA CORPORATION is strictly prohibited. 9 | 10 | 11 | 12 | 13 | 14 | UdpTransport 15 | UDPv4 16 | 17 | 18 | 19 | 20 | 21 | 22 | UdpTransport 23 | 24 | false 25 | 26 | 27 | -------------------------------------------------------------------------------- /humble_ws/src/ackermann_control/cmdvel_to_ackermann/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(cmdvel_to_ackermann) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(rclpy REQUIRED) 6 | find_package(ackermann_msgs REQUIRED) 7 | find_package(geometry_msgs REQUIRED) 8 | 9 | install(DIRECTORY 10 | scripts 11 | launch 12 | DESTINATION share/${PROJECT_NAME}) 13 | 14 | # Install Python executables 15 | install(PROGRAMS 16 | scripts/cmdvel_to_ackermann.py 17 | DESTINATION lib/${PROJECT_NAME} 18 | ) 19 | ament_package() 20 | -------------------------------------------------------------------------------- /humble_ws/src/ackermann_control/cmdvel_to_ackermann/launch/cmdvel_to_ackermann.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch.actions import DeclareLaunchArgument 3 | from launch_ros.actions import Node 4 | from launch.substitutions import LaunchConfiguration 5 | 6 | 7 | def generate_launch_description(): 8 | return LaunchDescription([ 9 | DeclareLaunchArgument("publish_period_ms", default_value="20", description="publishing dt in milliseconds"), 10 | DeclareLaunchArgument("track_width", default_value="0.24", description="wheel separation distance (m)"), 11 | DeclareLaunchArgument("acceleration", default_value="0.0", description="acceleration, 0 means change speed as quickly as possible (ms^-2)"), 12 | DeclareLaunchArgument("steering_velocity", default_value="0.0", description="delta steering angle, 0 means change angle as quickly as possible (radians/s)"), 13 | 14 | Node( 15 | package='cmdvel_to_ackermann', 16 | executable='cmdvel_to_ackermann.py', 17 | name='cmdvel_to_ackermann', 18 | output="screen", 19 | parameters=[{ 20 | 'publish_period_ms': LaunchConfiguration('publish_period_ms'), 21 | 'track_width': LaunchConfiguration('track_width'), 22 | 'acceleration': LaunchConfiguration('acceleration'), 23 | 'steering_velocity': LaunchConfiguration('steering_velocity') 24 | }] 25 | ), 26 | ]) 27 | -------------------------------------------------------------------------------- /humble_ws/src/ackermann_control/cmdvel_to_ackermann/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | cmdvel_to_ackermann 5 | 0.1.0 6 | This package converts command velocity (Twist Msg) to ackermann messages (AckermannDriveStamped Msg) 7 | Isaac Sim 8 | 9 | Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. 10 | NVIDIA CORPORATION and its licensors retain all intellectual property 11 | and proprietary rights in and to this software, related documentation 12 | and any modifications thereto. Any use, reproduction, disclosure or 13 | distribution of this software and related documentation without an express 14 | license agreement from NVIDIA CORPORATION is strictly prohibited. 15 | 16 | https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html 17 | https://forums.developer.nvidia.com/c/omniverse/simulation/69 18 | 19 | ament_cmake 20 | 21 | ackermann_msgs 22 | geometry_msgs 23 | launch 24 | 25 | 26 | ament_cmake 27 | 28 | 29 | -------------------------------------------------------------------------------- /humble_ws/src/ackermann_control/cmdvel_to_ackermann/scripts/__init__.py: -------------------------------------------------------------------------------- 1 | from cmdvel_to_ackermann import CmdvelToAckermann -------------------------------------------------------------------------------- /humble_ws/src/ackermann_control/cmdvel_to_ackermann/scripts/cmdvel_to_ackermann.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. 4 | # 5 | # NVIDIA CORPORATION and its licensors retain all intellectual property 6 | # and proprietary rights in and to this software, related documentation 7 | # and any modifications thereto. Any use, reproduction, disclosure or 8 | # distribution of this software and related documentation without an express 9 | # license agreement from NVIDIA CORPORATION is strictly prohibited. 10 | 11 | import math 12 | 13 | import rclpy 14 | from ackermann_msgs.msg import AckermannDriveStamped 15 | from geometry_msgs.msg import Twist 16 | from rclpy.node import Node 17 | 18 | 19 | class CmdvelToAckermann(Node): 20 | """Subscribe to the Twist message and converts it to AckermannDrive message and publish. 21 | """ 22 | 23 | def __init__(self): 24 | super().__init__('cmdvel_to_ackermann') 25 | 26 | self.declare_parameter('publish_period_ms', 20) 27 | self.declare_parameter('track_width', 0.24) 28 | self.declare_parameter('acceleration', 0.0) 29 | self.declare_parameter('steering_velocity', 0.0) 30 | 31 | self._cmd_vel_subscription = self.create_subscription(Twist, '/cmd_vel', 32 | self._cmd_vel_callback, 1) 33 | self._ackermann_publisher = self.create_publisher(AckermannDriveStamped, '/ackermann_cmd', 34 | 1) 35 | publish_period_ms = self.get_parameter( 36 | 'publish_period_ms').value / 1000 37 | self.create_timer(publish_period_ms, self._timer_callback) 38 | self.track_width = self.get_parameter("track_width").value 39 | self.acceleration = self.get_parameter("acceleration").value 40 | self.steering_velocity = self.get_parameter("steering_velocity").value 41 | 42 | self.get_logger().info(f"track_width: {self.track_width}") 43 | self.get_logger().info(f"acceleration: {self.acceleration}") 44 | self.get_logger().info(f"steering_velocity: {self.steering_velocity}") 45 | self._ackermann_msg = None 46 | 47 | 48 | def _convert_trans_rot_vel_to_steering_angle(self, v, omega) -> float: 49 | if omega == 0 or v == 0: 50 | if omega != 0: 51 | self.get_logger().warn( 52 | f'Invalid command for ackermann drive with zero vel {v} but non zero ' 53 | f'omega {omega}') 54 | return 0.0 55 | 56 | turning_radius = v / omega 57 | return math.atan(self.track_width / turning_radius) 58 | 59 | def _cmd_vel_callback(self, msg): 60 | self._ackermann_msg = AckermannDriveStamped() 61 | self._ackermann_msg.header.stamp = self.get_clock().now().to_msg() 62 | # Conversion logic (simplified example) 63 | self._ackermann_msg.drive.speed = msg.linear.x 64 | steering_angle = self._convert_trans_rot_vel_to_steering_angle( 65 | self._ackermann_msg.drive.speed, msg.angular.z) 66 | self._ackermann_msg.drive.steering_angle = steering_angle 67 | self._ackermann_msg.drive.acceleration = self.acceleration 68 | self._ackermann_msg.drive.steering_angle_velocity = self.steering_velocity 69 | 70 | def _timer_callback(self): 71 | if self._ackermann_msg: 72 | self._ackermann_publisher.publish(self._ackermann_msg) 73 | 74 | 75 | def main(args=None): 76 | rclpy.init(args=args) 77 | node = CmdvelToAckermann() 78 | rclpy.spin(node) 79 | node.destroy_node() 80 | rclpy.shutdown() 81 | 82 | 83 | if __name__ == '__main__': 84 | main() 85 | -------------------------------------------------------------------------------- /humble_ws/src/custom_message/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(custom_message) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | # find dependencies 19 | find_package(ament_cmake REQUIRED) 20 | find_package(std_msgs REQUIRED) 21 | find_package(rosidl_default_generators REQUIRED) 22 | 23 | rosidl_generate_interfaces(${PROJECT_NAME} 24 | "msg/SampleMsg.msg" 25 | DEPENDENCIES std_msgs 26 | ) 27 | # uncomment the following section in order to fill in 28 | # further dependencies manually. 29 | # find_package( REQUIRED) 30 | 31 | if(BUILD_TESTING) 32 | find_package(ament_lint_auto REQUIRED) 33 | # the following line skips the linter which checks for copyrights 34 | # uncomment the line when a copyright and license is not present in all source files 35 | #set(ament_cmake_copyright_FOUND TRUE) 36 | # the following line skips cpplint (only works in a git repo) 37 | # uncomment the line when this package is not in a git repo 38 | #set(ament_cmake_cpplint_FOUND TRUE) 39 | ament_lint_auto_find_test_dependencies() 40 | endif() 41 | 42 | ament_package() 43 | -------------------------------------------------------------------------------- /humble_ws/src/custom_message/msg/SampleMsg.msg: -------------------------------------------------------------------------------- 1 | std_msgs/String my_string 2 | int64 my_num 3 | -------------------------------------------------------------------------------- /humble_ws/src/custom_message/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | custom_message 5 | 0.0.0 6 | Custom Message Sample Package 7 | Isaac Sim 8 | 9 | Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. 10 | NVIDIA CORPORATION and its licensors retain all intellectual property 11 | and proprietary rights in and to this software, related documentation 12 | and any modifications thereto. Any use, reproduction, disclosure or 13 | distribution of this software and related documentation without an express 14 | license agreement from NVIDIA CORPORATION is strictly prohibited. 15 | 16 | https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html 17 | https://forums.developer.nvidia.com/c/omniverse/simulation/69 18 | 19 | ament_cmake 20 | 21 | std_msgs 22 | rosidl_default_generators 23 | rosidl_default_runtime 24 | rosidl_interface_packages 25 | 26 | ament_lint_auto 27 | ament_lint_common 28 | 29 | 30 | ament_cmake 31 | 32 | 33 | -------------------------------------------------------------------------------- /humble_ws/src/isaac_ros2_messages/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(isaac_ros2_messages) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | # find dependencies 19 | find_package(ament_cmake REQUIRED) 20 | # uncomment the following section in order to fill in 21 | # further dependencies manually. 22 | # find_package( REQUIRED) 23 | 24 | if(BUILD_TESTING) 25 | find_package(ament_lint_auto REQUIRED) 26 | # the following line skips the linter which checks for copyrights 27 | # uncomment the line when a copyright and license is not present in all source files 28 | #set(ament_cmake_copyright_FOUND TRUE) 29 | # the following line skips cpplint (only works in a git repo) 30 | # uncomment the line when this package is not in a git repo 31 | #set(ament_cmake_cpplint_FOUND TRUE) 32 | ament_lint_auto_find_test_dependencies() 33 | endif() 34 | 35 | 36 | find_package(builtin_interfaces REQUIRED) 37 | find_package(rosidl_default_generators REQUIRED) 38 | 39 | IF (WIN32) 40 | find_package(FastRTPS REQUIRED) 41 | ELSE() 42 | find_package(fastrtps REQUIRED) 43 | ENDIF() 44 | 45 | find_package(std_msgs REQUIRED) 46 | find_package(geometry_msgs REQUIRED) 47 | 48 | rosidl_generate_interfaces(${PROJECT_NAME} 49 | "srv/IsaacPose.srv" 50 | "srv/GetPrims.srv" 51 | "srv/GetPrimAttributes.srv" 52 | "srv/GetPrimAttribute.srv" 53 | "srv/SetPrimAttribute.srv" 54 | DEPENDENCIES builtin_interfaces std_msgs geometry_msgs 55 | ) 56 | 57 | ament_package() 58 | -------------------------------------------------------------------------------- /humble_ws/src/isaac_ros2_messages/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | isaac_ros2_messages 5 | 0.2.0 6 | ROS2 messages for isaac ros2 bridge 7 | Isaac Sim 8 | 9 | Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. 10 | NVIDIA CORPORATION and its licensors retain all intellectual property 11 | and proprietary rights in and to this software, related documentation 12 | and any modifications thereto. Any use, reproduction, disclosure or 13 | distribution of this software and related documentation without an express 14 | license agreement from NVIDIA CORPORATION is strictly prohibited. 15 | 16 | https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html 17 | https://forums.developer.nvidia.com/c/omniverse/simulation/69 18 | 19 | ament_cmake 20 | 21 | ament_lint_auto 22 | ament_lint_common 23 | 24 | rosidl_default_generators 25 | std_msgs 26 | geometry_msgs 27 | 28 | builtin_interfaces 29 | rosidl_default_runtime 30 | std_msgs 31 | geometry_msgs 32 | 33 | rosidl_interface_packages 34 | 35 | 36 | ament_cmake 37 | 38 | 39 | -------------------------------------------------------------------------------- /humble_ws/src/isaac_ros2_messages/srv/GetPrimAttribute.srv: -------------------------------------------------------------------------------- 1 | string path # prim path 2 | string attribute # attribute name 3 | --- 4 | string value # attribute value (as JSON) 5 | string type # attribute type 6 | bool success # indicate a successful execution of the service 7 | string message # informational, e.g. for error messages -------------------------------------------------------------------------------- /humble_ws/src/isaac_ros2_messages/srv/GetPrimAttributes.srv: -------------------------------------------------------------------------------- 1 | string path # prim path 2 | --- 3 | string[] names # list of attribute base names (name used to Get or Set an attribute) 4 | string[] displays # list of attribute display names (name displayed in Property tab) 5 | string[] types # list of attribute data types 6 | bool success # indicate a successful execution of the service 7 | string message # informational, e.g. for error messages -------------------------------------------------------------------------------- /humble_ws/src/isaac_ros2_messages/srv/GetPrims.srv: -------------------------------------------------------------------------------- 1 | string path # get prims at path 2 | --- 3 | string[] paths # list of prim paths 4 | string[] types # prim type names 5 | bool success # indicate a successful execution of the service 6 | string message # informational, e.g. for error messages -------------------------------------------------------------------------------- /humble_ws/src/isaac_ros2_messages/srv/IsaacPose.srv: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | string[] names 3 | geometry_msgs/Pose[] poses 4 | geometry_msgs/Twist[] velocities 5 | geometry_msgs/Vector3[] scales 6 | --- 7 | -------------------------------------------------------------------------------- /humble_ws/src/isaac_ros2_messages/srv/SetPrimAttribute.srv: -------------------------------------------------------------------------------- 1 | string path # prim path 2 | string attribute # attribute name 3 | string value # attribute value (as JSON) 4 | --- 5 | bool success # indicate a successful execution of the service 6 | string message # informational, e.g. for error messages -------------------------------------------------------------------------------- /humble_ws/src/isaac_tutorials/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(isaac_tutorials) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(rclpy REQUIRED) 6 | 7 | install(DIRECTORY 8 | rviz2 9 | scripts 10 | DESTINATION share/${PROJECT_NAME}) 11 | 12 | # Install Python executables 13 | install(PROGRAMS 14 | scripts/ros2_publisher.py 15 | scripts/ros2_ackermann_publisher.py 16 | DESTINATION lib/${PROJECT_NAME} 17 | ) 18 | ament_package() 19 | -------------------------------------------------------------------------------- /humble_ws/src/isaac_tutorials/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | isaac_tutorials 5 | 0.1.0 6 | 7 | The isaac_tutorials package 8 | 9 | 10 | Isaac Sim 11 | 12 | Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. 13 | NVIDIA CORPORATION and its licensors retain all intellectual property 14 | and proprietary rights in and to this software, related documentation 15 | and any modifications thereto. Any use, reproduction, disclosure or 16 | distribution of this software and related documentation without an express 17 | license agreement from NVIDIA CORPORATION is strictly prohibited. 18 | 19 | https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html 20 | https://forums.developer.nvidia.com/c/omniverse/simulation/69 21 | 22 | ament_cmake 23 | rqt_image_view 24 | 25 | launch 26 | launch_ros 27 | 28 | joint_state_publisher 29 | ackermann_msgs 30 | 31 | rviz2 32 | 33 | 34 | 35 | ament_cmake 36 | 37 | 38 | -------------------------------------------------------------------------------- /humble_ws/src/isaac_tutorials/scripts/ros2_ackermann_publisher.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright (c) 2020-2024, NVIDIA CORPORATION. All rights reserved. 4 | # 5 | # NVIDIA CORPORATION and its licensors retain all intellectual property 6 | # and proprietary rights in and to this software, related documentation 7 | # and any modifications thereto. Any use, reproduction, disclosure or 8 | # distribution of this software and related documentation without an express 9 | # license agreement from NVIDIA CORPORATION is strictly prohibited. 10 | 11 | import signal 12 | import sys 13 | 14 | import numpy as np 15 | 16 | import rclpy 17 | from rclpy.node import Node 18 | 19 | from ackermann_msgs.msg import AckermannDriveStamped 20 | 21 | class MinimalPublisher(Node): 22 | 23 | def __init__(self): 24 | super().__init__('test_ackermann') 25 | self.publisher_ = self.create_publisher(AckermannDriveStamped, 'ackermann_cmd', 10) 26 | timer_period = 0.05 # seconds 27 | self.timer = self.create_timer(timer_period, self.timer_callback) 28 | self.i = 0 29 | 30 | def timer_callback(self): 31 | msg = AckermannDriveStamped() 32 | 33 | # Only command ackermann drive robot using speed and steering angle 34 | degrees1 = np.arange(0, 60) 35 | degrees2 = np.arange(-60, 0) 36 | degrees = np.concatenate((degrees1, degrees1[::-1], degrees2[::-1], degrees2)) 37 | msg.header.frame_id = "ackermann" 38 | msg.header.stamp = self.get_clock().now().to_msg() 39 | msg.drive.steering_angle = 0.0174533 * (degrees[self.i % degrees.size]) 40 | 41 | msg.drive.speed = float(degrees[self.i % degrees.size]) 42 | msg.drive.acceleration = 1.0 43 | self.publisher_.publish(msg) 44 | self.i += 1 45 | 46 | def stop_robot(self): 47 | msg = AckermannDriveStamped() 48 | 49 | # Publish zero-velocity message 50 | msg.drive.steering_angle = 0.0 51 | msg.drive.speed = 0.0 52 | self.publisher_.publish(msg) 53 | 54 | 55 | def main(args=None): 56 | 57 | rclpy.init(args=args) 58 | 59 | minimal_publisher = MinimalPublisher() 60 | 61 | def signal_handler(sig, frame): 62 | minimal_publisher.stop_robot() 63 | minimal_publisher.destroy_node() 64 | rclpy.shutdown() 65 | sys.exit(0) 66 | 67 | # Register the SIGINT handler for Ctrl+C 68 | signal.signal(signal.SIGINT, signal_handler) 69 | 70 | try: 71 | rclpy.spin(minimal_publisher) 72 | except KeyboardInterrupt: 73 | pass 74 | 75 | 76 | if __name__ == '__main__': 77 | main() 78 | -------------------------------------------------------------------------------- /humble_ws/src/isaac_tutorials/scripts/ros2_publisher.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright (c) 2020-2022, NVIDIA CORPORATION. All rights reserved. 4 | # 5 | # NVIDIA CORPORATION and its licensors retain all intellectual property 6 | # and proprietary rights in and to this software, related documentation 7 | # and any modifications thereto. Any use, reproduction, disclosure or 8 | # distribution of this software and related documentation without an express 9 | # license agreement from NVIDIA CORPORATION is strictly prohibited. 10 | 11 | import rclpy 12 | from rclpy.node import Node 13 | from sensor_msgs.msg import JointState 14 | import numpy as np 15 | import time 16 | 17 | 18 | class TestROS2Bridge(Node): 19 | def __init__(self): 20 | 21 | super().__init__("test_ros2bridge") 22 | 23 | # Create the publisher. This publisher will publish a JointState message to the /joint_command topic. 24 | self.publisher_ = self.create_publisher(JointState, "joint_command", 10) 25 | 26 | # Create a JointState message 27 | self.joint_state = JointState() 28 | 29 | self.joint_state.name = [ 30 | "panda_joint1", 31 | "panda_joint2", 32 | "panda_joint3", 33 | "panda_joint4", 34 | "panda_joint5", 35 | "panda_joint6", 36 | "panda_joint7", 37 | "panda_finger_joint1", 38 | "panda_finger_joint2", 39 | ] 40 | 41 | num_joints = len(self.joint_state.name) 42 | 43 | # make sure kit's editor is playing for receiving messages 44 | self.joint_state.position = np.array([0.0] * num_joints, dtype=np.float64).tolist() 45 | self.default_joints = [0.0, -1.16, -0.0, -2.3, -0.0, 1.6, 1.1, 0.4, 0.4] 46 | 47 | # limiting the movements to a smaller range (this is not the range of the robot, just the range of the movement 48 | self.max_joints = np.array(self.default_joints) + 0.5 49 | self.min_joints = np.array(self.default_joints) - 0.5 50 | 51 | # position control the robot to wiggle around each joint 52 | self.time_start = time.time() 53 | 54 | timer_period = 0.05 # seconds 55 | self.timer = self.create_timer(timer_period, self.timer_callback) 56 | 57 | def timer_callback(self): 58 | self.joint_state.header.stamp = self.get_clock().now().to_msg() 59 | 60 | joint_position = ( 61 | np.sin(time.time() - self.time_start) * (self.max_joints - self.min_joints) * 0.5 + self.default_joints 62 | ) 63 | self.joint_state.position = joint_position.tolist() 64 | 65 | # Publish the message to the topic 66 | self.publisher_.publish(self.joint_state) 67 | 68 | 69 | def main(args=None): 70 | rclpy.init(args=args) 71 | 72 | ros2_publisher = TestROS2Bridge() 73 | 74 | rclpy.spin(ros2_publisher) 75 | 76 | # Destroy the node explicitly 77 | ros2_publisher.destroy_node() 78 | rclpy.shutdown() 79 | 80 | 81 | if __name__ == "__main__": 82 | main() 83 | -------------------------------------------------------------------------------- /humble_ws/src/isaacsim/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(isaacsim) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(rclpy REQUIRED) 6 | 7 | install(DIRECTORY 8 | scripts 9 | launch 10 | DESTINATION share/${PROJECT_NAME}) 11 | 12 | # Install Python executables 13 | install(PROGRAMS 14 | scripts/run_isaacsim.py 15 | scripts/open_isaacsim_stage.py 16 | DESTINATION lib/${PROJECT_NAME} 17 | ) 18 | ament_package() 19 | -------------------------------------------------------------------------------- /humble_ws/src/isaacsim/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | isaacsim 5 | 0.1.0 6 | 7 | The isaacsim package that contains the script which can used to launch Isaac Sim as a ROS2 node from a launch file. 8 | 9 | 10 | Isaac Sim 11 | 12 | Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. 13 | NVIDIA CORPORATION and its licensors retain all intellectual property 14 | and proprietary rights in and to this software, related documentation 15 | and any modifications thereto. Any use, reproduction, disclosure or 16 | distribution of this software and related documentation without an express 17 | license agreement from NVIDIA CORPORATION is strictly prohibited. 18 | 19 | https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html 20 | https://forums.developer.nvidia.com/c/omniverse/simulation/69 21 | 22 | ament_cmake 23 | 24 | launch 25 | launch_ros 26 | 27 | 28 | ament_cmake 29 | 30 | 31 | -------------------------------------------------------------------------------- /humble_ws/src/isaacsim/scripts/open_isaacsim_stage.py: -------------------------------------------------------------------------------- 1 | """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""" 2 | 3 | import carb 4 | import argparse 5 | import omni.usd 6 | import asyncio 7 | import omni.client 8 | import omni.kit.async_engine 9 | import omni.timeline 10 | 11 | def main(): 12 | parser = argparse.ArgumentParser() 13 | parser.add_argument('--path', type=str, required=True, help='The path to USD stage') 14 | 15 | # Add the --start-on-play option 16 | # If --start-on-play is specified, it sets the value to True 17 | parser.add_argument('--start-on-play', action='store_true', 18 | help='If present, to true.') 19 | 20 | try: 21 | options = parser.parse_args() 22 | except Exception as e: 23 | carb.log_error(str(e)) 24 | return 25 | 26 | omni.kit.async_engine.run_coroutine(open_stage_async(options.path, options.start_on_play)) 27 | 28 | async def open_stage_async(path: str, start_on_play: bool): 29 | timeline_interface = None 30 | if start_on_play: 31 | timeline_interface = omni.timeline.get_timeline_interface() 32 | 33 | async def _open_stage_internal(path): 34 | is_stage_with_session = False 35 | try: 36 | import omni.kit.usd.layers as layers 37 | live_session_name = layers.get_live_session_name_from_shared_link(path) 38 | is_stage_with_session = live_session_name is not None 39 | except Exception: 40 | pass 41 | 42 | if is_stage_with_session: 43 | # Try to open the stage with specified live session. 44 | (success, error) = await layers.get_live_syncing().open_stage_with_live_session_async(path) 45 | else: 46 | # Otherwise, use normal stage open. 47 | (success, error) = await omni.usd.get_context().open_stage_async(path) 48 | 49 | if not success: 50 | carb.log_error(f"Failed to open stage {path}: {error}.") 51 | else: 52 | if timeline_interface is not None: 53 | await omni.kit.app.get_app().next_update_async() 54 | await omni.kit.app.get_app().next_update_async() 55 | timeline_interface.play() 56 | print("Stage loaded and simulation is playing.") 57 | pass 58 | result, _ = await omni.client.stat_async(path) 59 | if result == omni.client.Result.OK: 60 | await _open_stage_internal(path) 61 | 62 | return 63 | 64 | broken_url = omni.client.break_url(path) 65 | if broken_url.scheme == 'omniverse': 66 | # Attempt to connect to nucleus server before opening stage 67 | try: 68 | from omni.kit.widget.nucleus_connector import get_nucleus_connector 69 | nucleus_connector = get_nucleus_connector() 70 | except Exception: 71 | carb.log_warn("Open stage: Could not import Nucleus connector.") 72 | return 73 | 74 | server_url = omni.client.make_url(scheme='omniverse', host=broken_url.host) 75 | nucleus_connector.connect( 76 | broken_url.host, server_url, 77 | on_success_fn=lambda *_: asyncio.ensure_future(_open_stage_internal(path)), 78 | on_failed_fn=lambda *_: carb.log_error(f"Open stage: Failed to connect to server '{server_url}'.") 79 | ) 80 | else: 81 | carb.log_warn(f"Open stage: Could not open non-existent url '{path}'.") 82 | 83 | 84 | main() 85 | -------------------------------------------------------------------------------- /humble_ws/src/navigation/carter_navigation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(carter_navigation) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | 6 | install(DIRECTORY 7 | launch 8 | params 9 | maps 10 | rviz2 11 | DESTINATION share/${PROJECT_NAME}) 12 | 13 | ament_package() 14 | -------------------------------------------------------------------------------- /humble_ws/src/navigation/carter_navigation/launch/carter_navigation.launch.py: -------------------------------------------------------------------------------- 1 | ## Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. 2 | ## NVIDIA CORPORATION and its licensors retain all intellectual property 3 | ## and proprietary rights in and to this software, related documentation 4 | ## and any modifications thereto. Any use, reproduction, disclosure or 5 | ## distribution of this software and related documentation without an express 6 | ## license agreement from NVIDIA CORPORATION is strictly prohibited. 7 | 8 | import os 9 | from ament_index_python.packages import get_package_share_directory 10 | from launch import LaunchDescription 11 | from launch.actions import DeclareLaunchArgument 12 | from launch.actions import IncludeLaunchDescription 13 | from launch.launch_description_sources import PythonLaunchDescriptionSource 14 | from launch.substitutions import LaunchConfiguration 15 | from launch_ros.actions import Node 16 | 17 | 18 | def generate_launch_description(): 19 | 20 | use_sim_time = LaunchConfiguration("use_sim_time", default="True") 21 | 22 | map_dir = LaunchConfiguration( 23 | "map", 24 | default=os.path.join( 25 | get_package_share_directory("carter_navigation"), "maps", "carter_warehouse_navigation.yaml" 26 | ), 27 | ) 28 | 29 | param_dir = LaunchConfiguration( 30 | "params_file", 31 | default=os.path.join( 32 | get_package_share_directory("carter_navigation"), "params", "carter_navigation_params.yaml" 33 | ), 34 | ) 35 | 36 | 37 | nav2_bringup_launch_dir = os.path.join(get_package_share_directory("nav2_bringup"), "launch") 38 | 39 | rviz_config_dir = os.path.join(get_package_share_directory("carter_navigation"), "rviz2", "carter_navigation.rviz") 40 | 41 | return LaunchDescription( 42 | [ 43 | DeclareLaunchArgument("map", default_value=map_dir, description="Full path to map file to load"), 44 | DeclareLaunchArgument( 45 | "params_file", default_value=param_dir, description="Full path to param file to load" 46 | ), 47 | DeclareLaunchArgument( 48 | "use_sim_time", default_value="true", description="Use simulation (Omniverse Isaac Sim) clock if true" 49 | ), 50 | IncludeLaunchDescription( 51 | PythonLaunchDescriptionSource(os.path.join(nav2_bringup_launch_dir, "rviz_launch.py")), 52 | launch_arguments={"namespace": "", "use_namespace": "False", "rviz_config": rviz_config_dir}.items(), 53 | ), 54 | IncludeLaunchDescription( 55 | PythonLaunchDescriptionSource([nav2_bringup_launch_dir, "/bringup_launch.py"]), 56 | launch_arguments={"map": map_dir, "use_sim_time": use_sim_time, "params_file": param_dir}.items(), 57 | ), 58 | 59 | Node( 60 | package='pointcloud_to_laserscan', executable='pointcloud_to_laserscan_node', 61 | remappings=[('cloud_in', ['/front_3d_lidar/lidar_points']), 62 | ('scan', ['/scan'])], 63 | parameters=[{ 64 | 'target_frame': 'front_3d_lidar', 65 | 'transform_tolerance': 0.01, 66 | 'min_height': -0.4, 67 | 'max_height': 1.5, 68 | 'angle_min': -1.5708, # -M_PI/2 69 | 'angle_max': 1.5708, # M_PI/2 70 | 'angle_increment': 0.0087, # M_PI/360.0 71 | 'scan_time': 0.3333, 72 | 'range_min': 0.05, 73 | 'range_max': 100.0, 74 | 'use_inf': True, 75 | 'inf_epsilon': 1.0, 76 | # 'concurrency_level': 1, 77 | }], 78 | name='pointcloud_to_laserscan' 79 | ) 80 | ] 81 | ) 82 | -------------------------------------------------------------------------------- /humble_ws/src/navigation/carter_navigation/launch/carter_navigation_individual.launch.py: -------------------------------------------------------------------------------- 1 | ## Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. 2 | ## NVIDIA CORPORATION and its licensors retain all intellectual property 3 | ## and proprietary rights in and to this software, related documentation 4 | ## and any modifications thereto. Any use, reproduction, disclosure or 5 | ## distribution of this software and related documentation without an express 6 | ## license agreement from NVIDIA CORPORATION is strictly prohibited. 7 | 8 | import os 9 | 10 | from ament_index_python.packages import get_package_share_directory 11 | 12 | from launch import LaunchDescription 13 | from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription 14 | from launch.conditions import IfCondition 15 | from launch.launch_description_sources import PythonLaunchDescriptionSource 16 | from launch.substitutions import LaunchConfiguration, PythonExpression, TextSubstitution 17 | from launch_ros.actions import Node 18 | 19 | 20 | def generate_launch_description(): 21 | 22 | # Get the launch directory 23 | nav2_launch_dir = os.path.join(get_package_share_directory("nav2_bringup"), "launch") 24 | 25 | # Create the launch configuration variables 26 | slam = LaunchConfiguration("slam") 27 | namespace = LaunchConfiguration("namespace") 28 | use_namespace = LaunchConfiguration("use_namespace") 29 | map_yaml_file = LaunchConfiguration("map") 30 | use_sim_time = LaunchConfiguration("use_sim_time") 31 | params_file = LaunchConfiguration("params_file") 32 | default_bt_xml_filename = LaunchConfiguration("default_bt_xml_filename") 33 | autostart = LaunchConfiguration("autostart") 34 | 35 | # Declare the launch arguments 36 | declare_namespace_cmd = DeclareLaunchArgument("namespace", default_value="", description="Top-level namespace") 37 | 38 | declare_use_namespace_cmd = DeclareLaunchArgument( 39 | "use_namespace", default_value="false", description="Whether to apply a namespace to the navigation stack" 40 | ) 41 | 42 | declare_slam_cmd = DeclareLaunchArgument("slam", default_value="False", description="Whether run a SLAM") 43 | 44 | declare_map_yaml_cmd = DeclareLaunchArgument( 45 | "map", 46 | default_value=os.path.join(nav2_launch_dir, "maps", "carter_warehouse_navigation.yaml"), 47 | description="Full path to map file to load", 48 | ) 49 | 50 | declare_use_sim_time_cmd = DeclareLaunchArgument( 51 | "use_sim_time", default_value="True", description="Use simulation (Isaac Sim) clock if true" 52 | ) 53 | 54 | declare_params_file_cmd = DeclareLaunchArgument( 55 | "params_file", 56 | default_value=os.path.join(nav2_launch_dir, "params", "nav2_params.yaml"), 57 | description="Full path to the ROS2 parameters file to use for all launched nodes", 58 | ) 59 | 60 | declare_bt_xml_cmd = DeclareLaunchArgument( 61 | "default_bt_xml_filename", 62 | default_value=os.path.join( 63 | get_package_share_directory("nav2_bt_navigator"), "behavior_trees", "navigate_w_replanning_and_recovery.xml" 64 | ), 65 | description="Full path to the behavior tree xml file to use", 66 | ) 67 | 68 | declare_autostart_cmd = DeclareLaunchArgument( 69 | "autostart", default_value="true", description="Automatically startup the nav2 stack" 70 | ) 71 | 72 | bringup_cmd = IncludeLaunchDescription( 73 | PythonLaunchDescriptionSource(os.path.join(nav2_launch_dir, "bringup_launch.py")), 74 | launch_arguments={ 75 | "namespace": namespace, 76 | "use_namespace": use_namespace, 77 | "slam": slam, 78 | "map": map_yaml_file, 79 | "use_sim_time": use_sim_time, 80 | "params_file": params_file, 81 | "default_bt_xml_filename": default_bt_xml_filename, 82 | "autostart": autostart, 83 | }.items(), 84 | ) 85 | 86 | # Create the launch description and populate 87 | ld = LaunchDescription() 88 | 89 | # Declare the launch options 90 | ld.add_action(declare_namespace_cmd) 91 | ld.add_action(declare_use_namespace_cmd) 92 | ld.add_action(declare_slam_cmd) 93 | ld.add_action(declare_map_yaml_cmd) 94 | ld.add_action(declare_use_sim_time_cmd) 95 | ld.add_action(declare_params_file_cmd) 96 | ld.add_action(declare_bt_xml_cmd) 97 | ld.add_action(declare_autostart_cmd) 98 | ld.add_action(bringup_cmd) 99 | 100 | return ld 101 | -------------------------------------------------------------------------------- /humble_ws/src/navigation/carter_navigation/maps/carter_hospital_navigation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isaac-sim/IsaacSim-ros_workspaces/435460aaeaa426e349fe885c60664b2bab5505bf/humble_ws/src/navigation/carter_navigation/maps/carter_hospital_navigation.png -------------------------------------------------------------------------------- /humble_ws/src/navigation/carter_navigation/maps/carter_hospital_navigation.yaml: -------------------------------------------------------------------------------- 1 | image: carter_hospital_navigation.png 2 | mode: trinary 3 | resolution: 0.05 4 | origin: [-49.625, -4.675, 0.0000] 5 | negate: 0 6 | occupied_thresh: 0.65 7 | free_thresh: 0.196 8 | -------------------------------------------------------------------------------- /humble_ws/src/navigation/carter_navigation/maps/carter_office_navigation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isaac-sim/IsaacSim-ros_workspaces/435460aaeaa426e349fe885c60664b2bab5505bf/humble_ws/src/navigation/carter_navigation/maps/carter_office_navigation.png -------------------------------------------------------------------------------- /humble_ws/src/navigation/carter_navigation/maps/carter_office_navigation.yaml: -------------------------------------------------------------------------------- 1 | image: carter_office_navigation.png 2 | mode: trinary 3 | resolution: 0.05 4 | origin: [-29.975, -39.975, 0.0000] 5 | negate: 0 6 | occupied_thresh: 0.65 7 | free_thresh: 0.196 8 | -------------------------------------------------------------------------------- /humble_ws/src/navigation/carter_navigation/maps/carter_warehouse_navigation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isaac-sim/IsaacSim-ros_workspaces/435460aaeaa426e349fe885c60664b2bab5505bf/humble_ws/src/navigation/carter_navigation/maps/carter_warehouse_navigation.png -------------------------------------------------------------------------------- /humble_ws/src/navigation/carter_navigation/maps/carter_warehouse_navigation.yaml: -------------------------------------------------------------------------------- 1 | image: carter_warehouse_navigation.png 2 | mode: trinary 3 | resolution: 0.05 4 | origin: [-11.975, -17.975, 0.0000] 5 | negate: 0 6 | occupied_thresh: 0.65 7 | free_thresh: 0.196 8 | -------------------------------------------------------------------------------- /humble_ws/src/navigation/carter_navigation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | carter_navigation 5 | 0.1.0 6 | 7 | The carter_navigation package 8 | 9 | 10 | Isaac Sim 11 | 12 | Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. 13 | NVIDIA CORPORATION and its licensors retain all intellectual property 14 | and proprietary rights in and to this software, related documentation 15 | and any modifications thereto. Any use, reproduction, disclosure or 16 | distribution of this software and related documentation without an express 17 | license agreement from NVIDIA CORPORATION is strictly prohibited. 18 | 19 | https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html 20 | https://forums.developer.nvidia.com/c/omniverse/simulation/69 21 | 22 | ament_cmake 23 | 24 | launch 25 | launch_ros 26 | 27 | joint_state_publisher 28 | 29 | rviz2 30 | 31 | navigation2 32 | nav2_amcl 33 | nav2_bringup 34 | nav2_bt_navigator 35 | nav2_costmap_2d 36 | nav2_core 37 | nav2_dwb_controller 38 | nav2_lifecycle_manager 39 | nav2_map_server 40 | nav2_behaviors 41 | nav2_planner 42 | nav2_msgs 43 | nav2_navfn_planner 44 | nav2_rviz_plugins 45 | nav2_behavior_tree 46 | nav2_util 47 | nav2_voxel_grid 48 | nav2_controller 49 | nav2_waypoint_follower 50 | pointcloud_to_laserscan 51 | 52 | 53 | 54 | ament_cmake 55 | 56 | 57 | -------------------------------------------------------------------------------- /humble_ws/src/navigation/isaac_ros_navigation_goal/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(isaac_ros_navigation_goal LANGUAGES PYTHON) 3 | 4 | # Default to C++17 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 17) 7 | endif() 8 | 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic) 11 | endif() 12 | 13 | execute_process(COMMAND uname -m COMMAND tr -d '\n' OUTPUT_VARIABLE ARCHITECTURE) 14 | message( STATUS "Architecture: ${ARCHITECTURE}" ) 15 | 16 | set(CUDA_MIN_VERSION "10.2") 17 | 18 | # Find dependencies 19 | find_package(ament_cmake REQUIRED) 20 | find_package(ament_cmake_auto REQUIRED) 21 | find_package(ament_cmake_python REQUIRED) 22 | find_package(rclpy REQUIRED) 23 | ament_auto_find_build_dependencies() 24 | 25 | # Install Python modules 26 | ament_python_install_package(${PROJECT_NAME}) 27 | 28 | # Install Python executables 29 | install(PROGRAMS 30 | isaac_ros_navigation_goal/set_goal.py 31 | DESTINATION lib/${PROJECT_NAME} 32 | ) 33 | 34 | ament_auto_package(INSTALL_TO_SHARE) 35 | -------------------------------------------------------------------------------- /humble_ws/src/navigation/isaac_ros_navigation_goal/assets/carter_warehouse_navigation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isaac-sim/IsaacSim-ros_workspaces/435460aaeaa426e349fe885c60664b2bab5505bf/humble_ws/src/navigation/isaac_ros_navigation_goal/assets/carter_warehouse_navigation.png -------------------------------------------------------------------------------- /humble_ws/src/navigation/isaac_ros_navigation_goal/assets/carter_warehouse_navigation.yaml: -------------------------------------------------------------------------------- 1 | image: carter_warehouse_navigation.png 2 | resolution: 0.05 3 | origin: [-11.975, -17.975, 0.0000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /humble_ws/src/navigation/isaac_ros_navigation_goal/assets/goals.txt: -------------------------------------------------------------------------------- 1 | 1 2 0 0 1 0 2 | 2 3 0 0 1 1 3 | 3.4 4.5 0.5 0.5 0.5 0.5 -------------------------------------------------------------------------------- /humble_ws/src/navigation/isaac_ros_navigation_goal/isaac_ros_navigation_goal/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isaac-sim/IsaacSim-ros_workspaces/435460aaeaa426e349fe885c60664b2bab5505bf/humble_ws/src/navigation/isaac_ros_navigation_goal/isaac_ros_navigation_goal/__init__.py -------------------------------------------------------------------------------- /humble_ws/src/navigation/isaac_ros_navigation_goal/isaac_ros_navigation_goal/goal_generators/__init__.py: -------------------------------------------------------------------------------- 1 | from .random_goal_generator import RandomGoalGenerator 2 | from .goal_reader import GoalReader 3 | -------------------------------------------------------------------------------- /humble_ws/src/navigation/isaac_ros_navigation_goal/isaac_ros_navigation_goal/goal_generators/goal_generator.py: -------------------------------------------------------------------------------- 1 | from abc import ABC, abstractmethod 2 | 3 | 4 | class GoalGenerator(ABC): 5 | """ 6 | Parent class for the Goal generators 7 | """ 8 | 9 | def __init__(self): 10 | pass 11 | 12 | @abstractmethod 13 | def generate_goal(self, max_num_of_trials=2000): 14 | """ 15 | Generate the goal. 16 | 17 | Parameters 18 | ---------- 19 | max_num_of_trials: maximum number of pose generations when generated pose keep is not a valid pose. 20 | 21 | Returns 22 | ------- 23 | [List][Pose]: Pose in format [pose.x,pose.y,orientaion.x,orientaion.y,orientaion.z,orientaion.w] 24 | """ 25 | pass 26 | -------------------------------------------------------------------------------- /humble_ws/src/navigation/isaac_ros_navigation_goal/isaac_ros_navigation_goal/goal_generators/goal_reader.py: -------------------------------------------------------------------------------- 1 | from .goal_generator import GoalGenerator 2 | 3 | 4 | class GoalReader(GoalGenerator): 5 | def __init__(self, file_path): 6 | self.__file_path = file_path 7 | self.__generator = self.__get_goal() 8 | 9 | def generate_goal(self, max_num_of_trials=1000): 10 | try: 11 | return next(self.__generator) 12 | except StopIteration: 13 | return 14 | 15 | def __get_goal(self): 16 | for row in open(self.__file_path, "r"): 17 | yield list(map(float, row.strip().split(" "))) 18 | -------------------------------------------------------------------------------- /humble_ws/src/navigation/isaac_ros_navigation_goal/isaac_ros_navigation_goal/goal_generators/random_goal_generator.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from .goal_generator import GoalGenerator 3 | 4 | 5 | class RandomGoalGenerator(GoalGenerator): 6 | """ 7 | Random goal generator. 8 | 9 | parameters 10 | ---------- 11 | grid_map: GridMap Object 12 | distance: distance in meters to check vicinity for obstacles. 13 | """ 14 | 15 | def __init__(self, grid_map, distance): 16 | self.__grid_map = grid_map 17 | self.__distance = distance 18 | 19 | def generate_goal(self, max_num_of_trials=1000): 20 | """ 21 | Generate the goal. 22 | 23 | Parameters 24 | ---------- 25 | max_num_of_trials: maximum number of pose generations when generated pose keep is not a valid pose. 26 | 27 | Returns 28 | ------- 29 | [List][Pose]: Pose in format [pose.x,pose.y,orientaion.x,orientaion.y,orientaion.z,orientaion.w] 30 | """ 31 | range_ = self.__grid_map.get_range() 32 | trial_count = 0 33 | while trial_count < max_num_of_trials: 34 | x = np.random.uniform(range_[0][0], range_[0][1]) 35 | y = np.random.uniform(range_[1][0], range_[1][1]) 36 | orient_x = np.random.uniform(0, 1) 37 | orient_y = np.random.uniform(0, 1) 38 | orient_z = np.random.uniform(0, 1) 39 | orient_w = np.random.uniform(0, 1) 40 | if self.__grid_map.is_valid_pose([x, y], self.__distance): 41 | goal = [x, y, orient_x, orient_y, orient_z, orient_w] 42 | return goal 43 | trial_count += 1 44 | -------------------------------------------------------------------------------- /humble_ws/src/navigation/isaac_ros_navigation_goal/launch/isaac_ros_navigation_goal.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. 2 | # 3 | # NVIDIA CORPORATION and its licensors retain all intellectual property 4 | # and proprietary rights in and to this software, related documentation 5 | # and any modifications thereto. Any use, reproduction, disclosure or 6 | # distribution of this software and related documentation without an express 7 | # license agreement from NVIDIA CORPORATION is strictly prohibited. 8 | 9 | import os 10 | from ament_index_python.packages import get_package_share_directory 11 | from launch import LaunchDescription 12 | from launch.substitutions import LaunchConfiguration 13 | from launch_ros.actions import Node 14 | 15 | 16 | def generate_launch_description(): 17 | 18 | map_yaml_file = LaunchConfiguration( 19 | "map_yaml_path", 20 | default=os.path.join( 21 | get_package_share_directory("isaac_ros_navigation_goal"), "assets", "carter_warehouse_navigation.yaml" 22 | ), 23 | ) 24 | 25 | goal_text_file = LaunchConfiguration( 26 | "goal_text_file_path", 27 | default=os.path.join(get_package_share_directory("isaac_ros_navigation_goal"), "assets", "goals.txt"), 28 | ) 29 | 30 | navigation_goal_node = Node( 31 | name="set_navigation_goal", 32 | package="isaac_ros_navigation_goal", 33 | executable="SetNavigationGoal", 34 | parameters=[ 35 | { 36 | "map_yaml_path": map_yaml_file, 37 | "iteration_count": 3, 38 | "goal_generator_type": "RandomGoalGenerator", 39 | "action_server_name": "navigate_to_pose", 40 | "obstacle_search_distance_in_meters": 0.2, 41 | "goal_text_file_path": goal_text_file, 42 | "initial_pose": [-6.4, -1.04, 0.0, 0.0, 0.0, 0.99, 0.02], 43 | } 44 | ], 45 | output="screen", 46 | ) 47 | 48 | return LaunchDescription([navigation_goal_node]) 49 | -------------------------------------------------------------------------------- /humble_ws/src/navigation/isaac_ros_navigation_goal/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | isaac_ros_navigation_goal 5 | 0.1.0 6 | Package to set goals for navigation stack. 7 | 8 | Isaac Sim 9 | 10 | Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. 11 | NVIDIA CORPORATION and its licensors retain all intellectual property 12 | and proprietary rights in and to this software, related documentation 13 | and any modifications thereto. Any use, reproduction, disclosure or 14 | distribution of this software and related documentation without an express 15 | license agreement from NVIDIA CORPORATION is strictly prohibited. 16 | 17 | https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html 18 | https://forums.developer.nvidia.com/c/omniverse/simulation/69 19 | https://developer.nvidia.com/isaac-ros-gems/ 20 | 21 | rclpy 22 | std_msgs 23 | sensor_msgs 24 | geometry_msgs 25 | nav2_msgs 26 | ament_flake8 27 | ament_pep257 28 | python3-pytest 29 | 30 | 31 | ament_python 32 | 33 | 34 | -------------------------------------------------------------------------------- /humble_ws/src/navigation/isaac_ros_navigation_goal/resource/isaac_ros_navigation_goal: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isaac-sim/IsaacSim-ros_workspaces/435460aaeaa426e349fe885c60664b2bab5505bf/humble_ws/src/navigation/isaac_ros_navigation_goal/resource/isaac_ros_navigation_goal -------------------------------------------------------------------------------- /humble_ws/src/navigation/isaac_ros_navigation_goal/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/isaac_ros_navigation_goal 3 | [install] 4 | install_scripts=$base/lib/isaac_ros_navigation_goal 5 | -------------------------------------------------------------------------------- /humble_ws/src/navigation/isaac_ros_navigation_goal/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | from glob import glob 3 | import os 4 | 5 | package_name = "isaac_ros_navigation_goal" 6 | 7 | setup( 8 | name=package_name, 9 | version="0.0.1", 10 | packages=[package_name, package_name + "/goal_generators"], 11 | data_files=[ 12 | ("share/ament_index/resource_index/packages", ["resource/" + package_name]), 13 | ("share/" + package_name, ["package.xml"]), 14 | (os.path.join("share", package_name, "launch"), glob("launch/*.launch.py")), 15 | ("share/" + package_name + "/assets", glob("assets/*")), 16 | ], 17 | install_requires=["setuptools"], 18 | zip_safe=True, 19 | maintainer="isaac sim", 20 | maintainer_email="isaac-sim@todo.todo", 21 | description="Package to set goals for navigation stack.", 22 | license="NVIDIA Isaac ROS Software License", 23 | tests_require=["pytest"], 24 | entry_points={"console_scripts": ["SetNavigationGoal = isaac_ros_navigation_goal.set_goal:main"]}, 25 | ) 26 | -------------------------------------------------------------------------------- /humble_ws/src/navigation/isaac_ros_navigation_goal/test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, "Found %d code style errors / warnings:\n" % len(errors) + "\n".join(errors) 24 | -------------------------------------------------------------------------------- /humble_ws/src/navigation/isaac_ros_navigation_goal/test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=[".", "test"]) 23 | assert rc == 0, "Found code style errors / warnings" 24 | -------------------------------------------------------------------------------- /humble_ws/src/navigation/iw_hub_navigation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(iw_hub_navigation) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | 6 | install(DIRECTORY 7 | launch 8 | params 9 | maps 10 | rviz2 11 | DESTINATION share/${PROJECT_NAME}) 12 | 13 | ament_package() 14 | -------------------------------------------------------------------------------- /humble_ws/src/navigation/iw_hub_navigation/launch/iw_hub_navigation.launch.py: -------------------------------------------------------------------------------- 1 | ## Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. 2 | ## NVIDIA CORPORATION and its licensors retain all intellectual property 3 | ## and proprietary rights in and to this software, related documentation 4 | ## and any modifications thereto. Any use, reproduction, disclosure or 5 | ## distribution of this software and related documentation without an express 6 | ## license agreement from NVIDIA CORPORATION is strictly prohibited. 7 | 8 | import os 9 | from ament_index_python.packages import get_package_share_directory 10 | from launch import LaunchDescription 11 | from launch.actions import DeclareLaunchArgument 12 | from launch.actions import IncludeLaunchDescription 13 | from launch.launch_description_sources import PythonLaunchDescriptionSource 14 | from launch.substitutions import LaunchConfiguration 15 | from launch_ros.actions import Node 16 | 17 | 18 | def generate_launch_description(): 19 | 20 | use_sim_time = LaunchConfiguration("use_sim_time", default="True") 21 | 22 | map_dir = LaunchConfiguration( 23 | "map", 24 | default=os.path.join( 25 | get_package_share_directory("iw_hub_navigation"), "maps", "iw_hub_warehouse_navigation.yaml" 26 | ), 27 | ) 28 | 29 | param_dir = LaunchConfiguration( 30 | "params_file", 31 | default=os.path.join( 32 | get_package_share_directory("iw_hub_navigation"), "params", "iw_hub_navigation_params.yaml" 33 | ), 34 | ) 35 | 36 | nav2_bringup_launch_dir = os.path.join(get_package_share_directory("nav2_bringup"), "launch") 37 | 38 | rviz_config_dir = os.path.join(get_package_share_directory("iw_hub_navigation"), "rviz2", "iw_hub_navigation.rviz") 39 | 40 | return LaunchDescription( 41 | [ 42 | DeclareLaunchArgument("map", default_value=map_dir, description="Full path to map file to load"), 43 | DeclareLaunchArgument( 44 | "params_file", default_value=param_dir, description="Full path to param file to load" 45 | ), 46 | DeclareLaunchArgument( 47 | "use_sim_time", default_value="true", description="Use simulation (Omniverse Isaac Sim) clock if true" 48 | ), 49 | IncludeLaunchDescription( 50 | PythonLaunchDescriptionSource(os.path.join(nav2_bringup_launch_dir, "rviz_launch.py")), 51 | launch_arguments={"namespace": "", "use_namespace": "False", "rviz_config": rviz_config_dir}.items(), 52 | ), 53 | IncludeLaunchDescription( 54 | PythonLaunchDescriptionSource([nav2_bringup_launch_dir, "/bringup_launch.py"]), 55 | launch_arguments={"map": map_dir, "use_sim_time": use_sim_time, "params_file": param_dir}.items(), 56 | ), 57 | ] 58 | ) 59 | -------------------------------------------------------------------------------- /humble_ws/src/navigation/iw_hub_navigation/maps/iw_hub_warehouse_navigation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isaac-sim/IsaacSim-ros_workspaces/435460aaeaa426e349fe885c60664b2bab5505bf/humble_ws/src/navigation/iw_hub_navigation/maps/iw_hub_warehouse_navigation.png -------------------------------------------------------------------------------- /humble_ws/src/navigation/iw_hub_navigation/maps/iw_hub_warehouse_navigation.yaml: -------------------------------------------------------------------------------- 1 | image: iw_hub_warehouse_navigation.png 2 | resolution: 0.05 3 | origin: [-11.975, -19.27500114440918, 0.0000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 -------------------------------------------------------------------------------- /humble_ws/src/navigation/iw_hub_navigation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | iw_hub_navigation 5 | 0.1.0 6 | 7 | The iw_hub_navigation package 8 | 9 | 10 | Isaac Sim 11 | 12 | Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. 13 | NVIDIA CORPORATION and its licensors retain all intellectual property 14 | and proprietary rights in and to this software, related documentation 15 | and any modifications thereto. Any use, reproduction, disclosure or 16 | distribution of this software and related documentation without an express 17 | license agreement from NVIDIA CORPORATION is strictly prohibited. 18 | 19 | https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html 20 | https://forums.developer.nvidia.com/c/omniverse/simulation/69 21 | 22 | ament_cmake 23 | 24 | launch 25 | launch_ros 26 | 27 | joint_state_publisher 28 | 29 | rviz2 30 | 31 | navigation2 32 | nav2_amcl 33 | nav2_bringup 34 | nav2_bt_navigator 35 | nav2_costmap_2d 36 | nav2_core 37 | nav2_dwb_controller 38 | nav2_lifecycle_manager 39 | nav2_map_server 40 | nav2_behaviors 41 | nav2_planner 42 | nav2_msgs 43 | nav2_navfn_planner 44 | nav2_rviz_plugins 45 | nav2_behavior_tree 46 | nav2_util 47 | nav2_voxel_grid 48 | nav2_controller 49 | nav2_waypoint_follower 50 | pointcloud_to_laserscan 51 | 52 | 53 | 54 | ament_cmake 55 | 56 | 57 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/config/command_stream_interpolator.yaml: -------------------------------------------------------------------------------- 1 | params: 2 | interpolation_delay: .1 3 | use_smoothing_interpolator: true 4 | blending_duration: 2. 5 | backend_timeout: .1 # 6 backend cycles at dt = 1/60 6 | ros_topics: 7 | joint_state: /robot/joint_state # Only used by main(). 8 | rmpflow_commands: 9 | command: /cortex/arm/command 10 | ack: /cortex/arm/command/ack 11 | suppress: /cortex/arm/command/suppress 12 | interpolated: /cortex/arm/command/interpolated 13 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/msg/CortexCommandAck.msg: -------------------------------------------------------------------------------- 1 | # Command time stamp of the latest Cortex command received. 2 | time cortex_command_time 3 | 4 | # ID of the latest command received. 5 | int64 cortex_command_id 6 | 7 | # If there is a slight accumulative clock rate difference between the Cortex 8 | # commander and the low-level controller, the time offset gives how much 9 | # further ahead the controller's clock is from the Cortex commander's clock (note 10 | # it can be negative). So synchronizing the clocks would entail 11 | # 12 | # = + time_offset 13 | duration time_offset 14 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/msg/JointPosVelAccCommand.msg: -------------------------------------------------------------------------------- 1 | # Basic joint position, velocity, and feed-forward acceleration command. 2 | # Typically, q and qd are used for PID control, and qdd is used by inverse 3 | # dynamics to compute a feed forward term. 4 | # 5 | # These message contain enough information to reconstruct the specific integral 6 | # curve on the receiving side using quintic polynomial interpolation 7 | # (rectifying the messages against jitter). See CommandStreamInterpolator. 8 | 9 | # Contains the wall-clock time stamp (unless otherwise specified explicitly 10 | # during construction of the Cortex commander). 11 | std_msgs/Header header 12 | 13 | # id's increment by one each, and period gives the amount of time between the 14 | # the previous message (with message ID id-1) and this 15 | int64 id 16 | duration period 17 | 18 | # This time stamp is the exact controller time in the sense that 19 | # 20 | # msg[id+1].t - msg[id].t = msg[id+1].period 21 | # 22 | # The header gives the wall-clock time at publication (unless otherwise 23 | # specified during initialization of the Cortex commander) so we can observe any 24 | # jitter in plotters such as rqt_plot which read the header time stamps to 25 | # rectify the incoming messages. 26 | time t 27 | 28 | string[] names 29 | float64[] q 30 | float64[] qd 31 | float64[] qdd 32 | 33 | #SymmetricMatrix32 metric 34 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | cortex_control 3 | 1.0.0 4 | NVIDIA Isaac Cortex Control ROS package. 5 | 6 | Isaac Sim 7 | 8 | Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. 9 | NVIDIA CORPORATION and its licensors retain all intellectual property 10 | and proprietary rights in and to this software, related documentation 11 | and any modifications thereto. Any use, reproduction, disclosure or 12 | distribution of this software and related documentation without an express 13 | license agreement from NVIDIA CORPORATION is strictly prohibited. 14 | 15 | https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html 16 | https://forums.developer.nvidia.com/c/omniverse/simulation/69 17 | 18 | catkin 19 | 20 | message_generation 21 | roscpp 22 | rospy 23 | roslib 24 | rosbag 25 | std_msgs 26 | robot_state_publisher 27 | yaml-cpp 28 | libgflags-dev 29 | 30 | message_runtime 31 | roscpp 32 | rospy 33 | roslib 34 | rosbag 35 | std_msgs 36 | robot_state_publisher 37 | yaml-cpp 38 | libgflags-dev 39 | 40 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/src/cortex/control/builders.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (c) 2021-2022, NVIDIA CORPORATION. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION and its licensors retain all intellectual property 5 | * and proprietary rights in and to this software, related documentation 6 | * and any modifications thereto. Any use, reproduction, disclosure or 7 | * distribution of this software and related documentation without an express 8 | * license agreement from NVIDIA CORPORATION is strictly prohibited. 9 | */ 10 | 11 | #include "cortex/control/builders.h" 12 | #include "cortex/util/yaml.h" 13 | 14 | #include 15 | 16 | namespace cortex { 17 | namespace control { 18 | 19 | std::shared_ptr LoadCommandStreamInterpolatorFromYaml( 20 | const YAML::Node& command_stream_interpolator_config, bool verbose) { 21 | // Extract params from yaml config. 22 | auto params = util::GetFieldOrDie(command_stream_interpolator_config, "params"); 23 | auto interpolation_delay = util::GetOrDie(params, "interpolation_delay"); 24 | auto use_smoothing_interpolator = util::GetOrDie(params, "use_smoothing_interpolator"); 25 | auto blending_duration = util::GetOrDie(params, "blending_duration"); 26 | auto backend_timeout = util::GetOrDie(params, "backend_timeout"); 27 | 28 | // Extract ROS topics from yaml config. 29 | auto ros_topics = util::GetFieldOrDie(command_stream_interpolator_config, "ros_topics"); 30 | auto command_topics = util::GetFieldOrDie(ros_topics, "rmpflow_commands"); 31 | auto rmpflow_command_topic = util::GetOrDie(command_topics, "command"); 32 | auto rmpflow_command_ack_topic = util::GetOrDie(command_topics, "ack"); 33 | auto rmpflow_command_suppress_topic = util::GetOrDie(command_topics, "suppress"); 34 | auto rmpflow_command_interpolated_topic = util::GetOrDie(command_topics, 35 | "interpolated"); 36 | 37 | if (verbose) { 38 | std::cout << "RMPflow backend config:" << std::endl; 39 | std::cout << " params:" << std::endl; 40 | std::cout << " interpolation delay: " << interpolation_delay << std::endl; 41 | std::cout << " use smoothing interpolator: " << use_smoothing_interpolator << std::endl; 42 | std::cout << " blending duration: " << blending_duration << std::endl; 43 | std::cout << " backend timeout: " << backend_timeout << std::endl; 44 | std::cout << " ros_topics:" << std::endl; 45 | std::cout << " rmpflow_commands:" << std::endl; 46 | std::cout << " command: " << rmpflow_command_topic << std::endl; 47 | std::cout << " ack: " << rmpflow_command_ack_topic << std::endl; 48 | std::cout << " suppress: " << rmpflow_command_suppress_topic << std::endl; 49 | std::cout << " interpolated: " << rmpflow_command_interpolated_topic << std::endl; 50 | } 51 | 52 | auto stream_interpolator = std::make_shared(); 53 | stream_interpolator->Init(ros::Duration(interpolation_delay), 54 | use_smoothing_interpolator, 55 | rmpflow_command_topic, 56 | rmpflow_command_ack_topic, 57 | rmpflow_command_suppress_topic, 58 | rmpflow_command_interpolated_topic, 59 | ros::Duration(blending_duration), 60 | backend_timeout); 61 | return stream_interpolator; 62 | } 63 | 64 | } // namespace control 65 | } // namespace cortex 66 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/src/cortex/control/builders.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (c) 2021-2022, NVIDIA CORPORATION. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION and its licensors retain all intellectual property 5 | * and proprietary rights in and to this software, related documentation 6 | * and any modifications thereto. Any use, reproduction, disclosure or 7 | * distribution of this software and related documentation without an express 8 | * license agreement from NVIDIA CORPORATION is strictly prohibited. 9 | */ 10 | #pragma once 11 | 12 | #include 13 | 14 | #include 15 | 16 | #include "cortex/control/command_stream_interpolator.h" 17 | 18 | namespace cortex { 19 | namespace control { 20 | 21 | //! Makes and initializes a command stream interpolator from the specified YAML config. One still needs 22 | //! to call Start() on the returned object to start the streaming interpolation. 23 | std::shared_ptr LoadCommandStreamInterpolatorFromYaml( 24 | const YAML::Node& command_stream_interpolator_config, bool verbose = false); 25 | 26 | } // namespace control 27 | } // namespace cortex 28 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/src/cortex/control/joint_pos_vel_acc_command_publisher.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2019-2022, NVIDIA CORPORATION. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION and its licensors retain all intellectual property 5 | * and proprietary rights in and to this software, related documentation 6 | * and any modifications thereto. Any use, reproduction, disclosure or 7 | * distribution of this software and related documentation without an express 8 | * license agreement from NVIDIA CORPORATION is strictly prohibited. 9 | */ 10 | 11 | #include "cortex/control/joint_pos_vel_acc_command_publisher.h" 12 | 13 | #include 14 | 15 | #include 16 | 17 | namespace cortex { 18 | namespace control { 19 | 20 | JointPosVelAccCommandPublisher::JointPosVelAccCommandPublisher( 21 | const std::string& topic, bool stamp_header_with_controller_time) 22 | : stamp_header_with_controller_time_(stamp_header_with_controller_time), 23 | is_first_(true), 24 | next_id_(0) { 25 | topic_ = topic; 26 | ros::NodeHandle nh; 27 | joint_command_publisher_ = nh.advertise(topic_, 10); 28 | } 29 | 30 | JointPosVelAccCommandPublisher::~JointPosVelAccCommandPublisher() {} 31 | 32 | void JointPosVelAccCommandPublisher::Publish(uint64_t id, 33 | const ros::Time& t, 34 | const std::vector& joint_names, 35 | const Eigen::VectorXd& q, 36 | const Eigen::VectorXd& qd, 37 | const Eigen::VectorXd& qdd) { 38 | cortex_control::JointPosVelAccCommand joint_command; 39 | if (stamp_header_with_controller_time_) { 40 | joint_command.header.stamp = t; 41 | } else { 42 | if (is_first_) { 43 | controller_time_offset_ = ros::Time::now() - t; 44 | } 45 | // We want to report the current time, but with the steadiness of the 46 | // controller time. 47 | joint_command.header.stamp = t + controller_time_offset_; 48 | } 49 | joint_command.id = id; 50 | 51 | if (is_first_) { 52 | // Usually this first message is missed by the interpolator (or it's 53 | // dropped because of syncing protocols), but even if it's used, the 54 | // interpolator won't use the period field because that's only used for 55 | // knowing the period between the previous point (there isn't one) and this 56 | // one. 57 | joint_command.period = ros::Duration(0.); 58 | is_first_ = false; 59 | } else { 60 | joint_command.period = (t - prev_t_); 61 | } 62 | 63 | joint_command.t = t; 64 | joint_command.names = joint_names; 65 | 66 | joint_command.q = std::vector(q.data(), q.data() + q.size()); 67 | joint_command.qd = std::vector(qd.data(), qd.data() + qd.size()); 68 | joint_command.qdd = std::vector(qdd.data(), qdd.data() + qdd.size()); 69 | 70 | joint_command_publisher_.publish(joint_command); 71 | 72 | // Updating the next_id_ member here means we can always set an ID once with 73 | // a call explicitly to this Publish(...) method and then use the ID-less 74 | // Publish(...) method to continue publishing sequential IDs from there. 75 | next_id_ = id + 1; 76 | prev_t_ = t; 77 | } 78 | 79 | void JointPosVelAccCommandPublisher::Publish(const ros::Time& t, 80 | const std::vector& joint_names, 81 | const Eigen::VectorXd& q, 82 | const Eigen::VectorXd& qd, 83 | const Eigen::VectorXd& qdd) { 84 | // Note that this call automatically increments next_id. 85 | Publish(next_id_, t, joint_names, q, qd, qdd); 86 | } 87 | 88 | } // namespace control 89 | } // namespace cortex 90 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/src/cortex/control/joint_pos_vel_acc_command_publisher.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2019-2022, NVIDIA CORPORATION. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION and its licensors retain all intellectual property 5 | * and proprietary rights in and to this software, related documentation 6 | * and any modifications thereto. Any use, reproduction, disclosure or 7 | * distribution of this software and related documentation without an express 8 | * license agreement from NVIDIA CORPORATION is strictly prohibited. 9 | */ 10 | 11 | #pragma once 12 | 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | namespace cortex { 20 | namespace control { 21 | 22 | /*!\brief Abstract class representing the base of a oint position, velocity, 23 | * and acceleration command publisher. The topic name of the publisher is 24 | * defined as ns + "joint_command". 25 | */ 26 | class JointPosVelAccCommandPublisher { 27 | public: 28 | /*!\brief Creates a JointPosVelAccCommandPublisher under the given name 29 | * space *ns*. The topic name of the publisher is defined as ns + 30 | * "joint_command". 31 | * 32 | * There are two time stamps in each JointPosVelAccCommand message, one in 33 | * the header and another as an explicit field t. The explicit field is 34 | * always set to be the controller time (with each message exactly a period 35 | * duration between), but by default (if stamp_header_with_controller_time is 36 | * false) the header contains the wall clock time so we can see the jitter in 37 | * the calculation using tools like rqt_plot. If 38 | * stamp_header_with_controller_time is true, that header stamp is also set 39 | * to the controller time so that becomes observable in plotters. 40 | */ 41 | JointPosVelAccCommandPublisher(const std::string& ns, 42 | bool stamp_header_with_controller_time = false); 43 | 44 | /*!\brief Default virtual destructor 45 | */ 46 | ~JointPosVelAccCommandPublisher(); 47 | 48 | /*!\brief Publishes the position, velocity, and acceleration command. Each 49 | * call to this method sets the id counter to the provided value, so 50 | * subsequent calls to the id-less API will increment from this id. 51 | * 52 | * \param time The time stamp of this command. 53 | * \param id the sequence id of this command. 54 | * \param joint_names Joint names vector. This vector must have the same 55 | * order as q qd, and qdd, i.e. the i-th name must correspond to the i-th q, 56 | * qd, qdd values. 57 | * \param q Joint position values 58 | * \param qd Joint velocity values 59 | * \param qdd Joint acceleration values 60 | */ 61 | virtual void Publish(uint64_t id, 62 | const ros::Time& t, 63 | const std::vector& joint_names, 64 | const Eigen::VectorXd& q, 65 | const Eigen::VectorXd& qd, 66 | const Eigen::VectorXd& qdd); 67 | 68 | /*!\brief This version automatically creates the sequence id, starting from 69 | * zero and incrementing once for each call. 70 | */ 71 | void Publish(const ros::Time& t, 72 | const std::vector& joint_names, 73 | const Eigen::VectorXd& q, 74 | const Eigen::VectorXd& qd, 75 | const Eigen::VectorXd& qdd); 76 | 77 | const std::string& topic() const { return topic_; } 78 | 79 | protected: 80 | bool stamp_header_with_controller_time_; 81 | ros::Publisher joint_command_publisher_; 82 | ros::Duration controller_time_offset_; 83 | 84 | bool is_first_; 85 | ros::Time prev_t_; 86 | uint64_t next_id_; 87 | 88 | std::string topic_; 89 | }; 90 | 91 | } // namespace control 92 | } // namespace cortex 93 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/src/cortex/control/rmpflow_commanded_joints_listener.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2021-2022, NVIDIA CORPORATION. All rights reserved. 2 | * 3 | * NVIDIA CORPORATION and its licensors retain all intellectual property and proprietary rights in 4 | * and to this software, related documentation and any modifications thereto. Any use, 5 | * reproduction, disclosure or distribution of this software and related documentation without an 6 | * express license agreement from NVIDIA CORPORATION is strictly prohibited. 7 | */ 8 | 9 | #include "cortex/control/rmpflow_commanded_joints_listener.h" 10 | 11 | namespace cortex { 12 | namespace control { 13 | 14 | RmpflowCommandedJointsListener::RmpflowCommandedJointsListener( 15 | const std::string& rmpflow_commands_topic, const std::string& joint_state_topic) 16 | : rmpflow_commands_listener_(rmpflow_commands_topic, 1), 17 | joint_state_listener_(std::make_shared()) { 18 | joint_state_listener_->Init(joint_state_topic); 19 | 20 | rmpflow_commands_listener_.RegisterCallback([&](const auto& msg) { 21 | std::lock_guard guard(mutex_); 22 | joint_state_listener_->SetRequiredJoints(msg.names); 23 | }); 24 | } 25 | 26 | bool RmpflowCommandedJointsListener::IsAvailable() const { 27 | std::lock_guard guard(mutex_); 28 | return is_set_ && joint_state_listener_->is_available(); 29 | } 30 | 31 | void RmpflowCommandedJointsListener::WaitUntilAvailable(double poll_rate) const { 32 | ros::Rate rate(poll_rate); 33 | 34 | while (ros::ok() && !IsAvailable()) { 35 | rate.sleep(); 36 | } 37 | } 38 | 39 | const std::shared_ptr 40 | RmpflowCommandedJointsListener::joint_state_listener() const { 41 | return joint_state_listener_; 42 | } 43 | } // namespace control 44 | } // namespace cortex 45 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/src/cortex/control/rmpflow_commanded_joints_listener.h: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2021-2022, NVIDIA CORPORATION. All rights reserved. 2 | * 3 | * NVIDIA CORPORATION and its licensors retain all intellectual property and proprietary rights in 4 | * and to this software, related documentation and any modifications thereto. Any use, 5 | * reproduction, disclosure or distribution of this software and related documentation without an 6 | * express license agreement from NVIDIA CORPORATION is strictly prohibited. 7 | */ 8 | 9 | #include 10 | 11 | #include 12 | 13 | #include "cortex/util/joint_state_listener.h" 14 | #include "cortex/util/ros_message_listener.h" 15 | 16 | namespace cortex { 17 | namespace control { 18 | 19 | // A wrapper around the joint state listener ensuring that we listen to the same joints we're 20 | // controlling with the RMPflow commander's commands. 21 | // 22 | // Listens to the RMPflow commander's commands as well as the joint state topic. Once we receive 23 | // the first command, we register the joint names with the joint state listener as required joints. 24 | // The IsAvailable() method (or WaitUntilAvailable()) can then be used to check whether the joint 25 | // state listener is ready and has measured values for each of those named joints. 26 | class RmpflowCommandedJointsListener { 27 | public: 28 | RmpflowCommandedJointsListener(const std::string& rmpflow_commands_topic, 29 | const std::string& joint_state_topic); 30 | 31 | bool IsAvailable() const; 32 | void WaitUntilAvailable(double poll_rate) const; 33 | 34 | const std::shared_ptr joint_state_listener() const; 35 | 36 | protected: 37 | mutable std::mutex mutex_; 38 | 39 | util::RosMessageListener rmpflow_commands_listener_; 40 | bool is_set_; 41 | std::shared_ptr joint_state_listener_; 42 | }; 43 | 44 | } // namespace control 45 | } // namespace cortex 46 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/src/cortex/math/interpolation/cubic_position_interpolator.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2019-2022, NVIDIA CORPORATION. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION and its licensors retain all intellectual property 5 | * and proprietary rights in and to this software, related documentation 6 | * and any modifications thereto. Any use, reproduction, disclosure or 7 | * distribution of this software and related documentation without an express 8 | * license agreement from NVIDIA CORPORATION is strictly prohibited. 9 | */ 10 | 11 | #include "cortex/math/interpolation/cubic_position_interpolator.h" 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | namespace cortex { 20 | namespace math { 21 | 22 | // Returns true iff t \in [0,1]. 23 | inline bool InZeroOne(double t) { return 0 <= t && t <= 1; } 24 | 25 | // clang-format off 26 | #define CUBIC_POSITION_INTERP_MATRIX \ 27 | 0, 0, 0, 1, \ 28 | 0, 0, 1, 0, \ 29 | 0, 2, 0, 0, \ 30 | 1, 1, 1, 1 31 | // clang-format on 32 | 33 | CubicPositionInterpolator1d::CubicPositionInterpolator1d(const PosVelAcc1d& p0, 34 | const PosVelAcc1d& p1, 35 | bool validate_interpolation_evals) 36 | : validate_interpolation_evals_(validate_interpolation_evals), 37 | A_((Eigen::MatrixXd(4, 4) << CUBIC_POSITION_INTERP_MATRIX).finished()), 38 | b_((Eigen::VectorXd(4) << p0.x, p0.xd, p0.xdd, p1.x).finished()), 39 | coeffs_(A_.colPivHouseholderQr().solve(b_)) {} 40 | 41 | bool CubicPositionInterpolator1d::Eval(double t, PosVelAcc1d& ret, std::string* error_str) const { 42 | if (validate_interpolation_evals_ && !InZeroOne(t)) { 43 | std::stringstream ss; 44 | ss << "t not in [0,1] (t = " << t << "). "; 45 | if (error_str) { 46 | *error_str += ss.str(); 47 | } 48 | return false; 49 | } 50 | auto a3 = coeffs_[0]; 51 | auto a2 = coeffs_[1]; 52 | auto a1 = coeffs_[2]; 53 | auto a0 = coeffs_[3]; 54 | 55 | std::vector t_powers(4, 1); 56 | for (size_t i = 1; i < t_powers.size(); ++i) { 57 | t_powers[i] = t * t_powers[i - 1]; 58 | } 59 | 60 | auto x = a3 * t_powers[3] + a2 * t_powers[2] + a1 * t_powers[1] + a0; 61 | auto xd = 3. * a3 * t_powers[2] + 2. * a2 * t_powers[1] + a1; 62 | auto xdd = 6. * a3 * t_powers[1] + 2. * a2; 63 | 64 | ret = PosVelAcc1d(x, xd, xdd); 65 | return true; 66 | } 67 | 68 | } // namespace math 69 | } // namespace cortex 70 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/src/cortex/math/interpolation/cubic_position_interpolator.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2019-2022, NVIDIA CORPORATION. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION and its licensors retain all intellectual property 5 | * and proprietary rights in and to this software, related documentation 6 | * and any modifications thereto. Any use, reproduction, disclosure or 7 | * distribution of this software and related documentation without an express 8 | * license agreement from NVIDIA CORPORATION is strictly prohibited. 9 | */ 10 | 11 | #pragma once 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | 21 | #include "cortex/math/interpolation/pos_vel_acc.h" 22 | #include "cortex/math/interpolation/time_scaled_interpolator.h" 23 | #include "cortex/math/interpolation/trajectories.h" 24 | 25 | namespace cortex { 26 | namespace math { 27 | 28 | // One-dimensional cubic interpolating polynomial. Interpolates between (x0, 29 | // xd0) and (x1, xd1). 30 | class CubicPositionInterpolator1d { 31 | public: 32 | typedef double VectorXx; 33 | 34 | CubicPositionInterpolator1d() {} 35 | 36 | // Creates a cubic spline that interpolates between p0 and p1 at t = 0 and 37 | // 1, respectively. 38 | CubicPositionInterpolator1d(const PosVelAcc1d& p0, 39 | const PosVelAcc1d& p1, 40 | bool validate_interpolation_evals = false); 41 | 42 | // Evaluate the polynomial at t. If validate_interpolating_evals is set to 43 | // true, enforces that the evaluations are only interpolating, i.e. t is in 44 | // [0, 1]; fails if not. The interpolated value is returned in the ret return 45 | // parameter. On failure, returns false and sets the error string if it's 46 | // provided. 47 | bool Eval(double t, PosVelAcc1d& ret, std::string* error_str = nullptr) const; 48 | 49 | // This verion asserts on error. 50 | PosVelAcc1d Eval(double t) const { 51 | PosVelAcc1d ret; 52 | std::string error_str; 53 | ROS_ASSERT_MSG(Eval(t, ret, &error_str), "%s", error_str.c_str()); 54 | return ret; 55 | } 56 | 57 | double operator()(double t) const { 58 | auto p = Eval(t); 59 | return p.x; 60 | } 61 | 62 | // Accessor. 63 | const Eigen::VectorXd& coeffs() const { return coeffs_; } 64 | 65 | protected: 66 | bool validate_interpolation_evals_; 67 | const Eigen::MatrixXd A_; 68 | const Eigen::VectorXd b_; 69 | const Eigen::VectorXd coeffs_; 70 | }; 71 | 72 | template 73 | MultiDimInterp CubicInterpolator( 74 | const PosVelAcc& p0, 75 | const PosVelAcc& p1, 76 | bool validate_interpolation_evals = false) { 77 | return MultiDimInterp(p0, p1, validate_interpolation_evals); 78 | } 79 | 80 | typedef MultiDimInterp CubicPositionInterpolatorXd; 81 | 82 | } // namespace math 83 | } // namespace cortex 84 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/src/cortex/math/interpolation/incremental_interpolator.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2019-2022, NVIDIA CORPORATION. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION and its licensors retain all intellectual property 5 | * and proprietary rights in and to this software, related documentation 6 | * and any modifications thereto. Any use, reproduction, disclosure or 7 | * distribution of this software and related documentation without an express 8 | * license agreement from NVIDIA CORPORATION is strictly prohibited. 9 | */ 10 | 11 | #include "cortex/math/interpolation/incremental_interpolator.h" 12 | 13 | #include 14 | #include 15 | 16 | #include "cortex/math/interpolation/quintic_interpolator.h" 17 | 18 | namespace cortex { 19 | namespace math { 20 | 21 | IncrementalInterpolator::IncrementalInterpolator(bool prune_history, 22 | bool validate_interpolation_evals) 23 | : is_first_(true), 24 | validate_interpolation_evals_(validate_interpolation_evals), 25 | prune_history_(prune_history) {} 26 | 27 | bool IncrementalInterpolator::AddPt(double t, const PosVelAccXd& p, std::string* error_str) { 28 | if (is_first_) { 29 | prev_add_t_ = t; 30 | prev_add_p_ = p; 31 | is_first_ = false; 32 | return true; 33 | } 34 | 35 | if (t <= prev_add_t_) { 36 | if (error_str) { 37 | std::stringstream ss; 38 | ss << "Add times nonmonotonic -- t = " << t << " vs prev t = " << prev_add_t_; 39 | *error_str += ss.str(); 40 | } 41 | return false; 42 | } 43 | 44 | segment_interpolators_.push_back( 45 | TimeScaledInterpolatorXd(prev_add_t_, prev_add_p_, t, p, validate_interpolation_evals_)); 46 | prev_add_t_ = t; 47 | prev_add_p_ = p; 48 | 49 | return true; 50 | } 51 | 52 | bool IncrementalInterpolator::Eval(double t, PosVelAccXd& ret, std::string* error_str) const { 53 | if (segment_interpolators_.size() == 0) { 54 | if (error_str) { 55 | *error_str += "No interpolators found."; 56 | } 57 | return false; 58 | } 59 | 60 | auto earliest_time = segment_interpolators_.front().t0(); 61 | auto latest_time = segment_interpolators_.back().t1(); 62 | if (validate_interpolation_evals_ && t < earliest_time) { 63 | if (error_str) { 64 | std::stringstream ss; 65 | ss << "Nonmonotonic evals -- t = " << t 66 | << ", earliest time segment starts with t0 = " << earliest_time; 67 | *error_str += ss.str(); 68 | } 69 | return false; 70 | } 71 | if (validate_interpolation_evals_ && t > latest_time) { 72 | if (error_str) { 73 | std::stringstream ss; 74 | ss << "Future eval (overflow) -- t = " << t 75 | << ", latest time segment ends with t1 = " << latest_time; 76 | *error_str += ss.str(); 77 | } 78 | return false; 79 | } 80 | 81 | // Find the first segment whose upper time bound is greater than the curren 82 | // time. Since the segments are contiguous and monotonically increasing, we're 83 | // guaranteed that t \in [t0, t1] of this segment. 84 | TimeScaledInterpolatorXd* active_interpolator = nullptr; 85 | for (auto it = segment_interpolators_.begin(); it != segment_interpolators_.end();) { 86 | if (t <= it->t1()) { 87 | active_interpolator = &(*it); 88 | break; 89 | } else { 90 | if (prune_history_) { 91 | it = segment_interpolators_.erase(it); 92 | } else { 93 | ++it; 94 | } 95 | } 96 | } 97 | if (!active_interpolator && !validate_interpolation_evals_) { 98 | active_interpolator = &segment_interpolators_.back(); 99 | } 100 | if (active_interpolator) { 101 | return active_interpolator->Eval(t, ret, error_str); 102 | } else { 103 | if (error_str) { 104 | std::stringstream ss; 105 | ss << "Eval time in the future -- t = " << t << " vs latest segment time = " << latest_time; 106 | *error_str += ss.str(); 107 | } 108 | return false; 109 | } 110 | } 111 | 112 | bool IncrementalInterpolator::IsReady(double t) const { 113 | return (segment_interpolators_.size() > 0) && (t >= segment_interpolators_.front().t0()); 114 | } 115 | 116 | } // namespace math 117 | } // namespace cortex 118 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/src/cortex/math/interpolation/incremental_interpolator.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2019-2022, NVIDIA CORPORATION. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION and its licensors retain all intellectual property 5 | * and proprietary rights in and to this software, related documentation 6 | * and any modifications thereto. Any use, reproduction, disclosure or 7 | * distribution of this software and related documentation without an express 8 | * license agreement from NVIDIA CORPORATION is strictly prohibited. 9 | */ 10 | 11 | #pragma once 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | 20 | #include "cortex/math/interpolation/interpolator.h" 21 | #include "cortex/math/interpolation/pos_vel_acc.h" 22 | #include "cortex/math/interpolation/quintic_interpolator.h" 23 | 24 | // Note: an incremental interpolator is one that leverages monotonicity 25 | // assumptions on the evaluation times to continually grow the interpolator head 26 | // while removing stale segments from the tail. 27 | 28 | namespace cortex { 29 | namespace math { 30 | 31 | // Enables the interpolation of a sequence of (x, xd, xdd) way-points using 32 | // quintic polynomials for each region between points. Evaluations and adding 33 | // of new way points can be interleaved, although evaluations are expected to 34 | // be with monotonically increasing time. There's a notion of a "delay_buffer" 35 | // which enables points to be received and added with wall-clock time 36 | // simultaneous with wall-clock evaluations by evaluating at a fixed time 37 | // interval in the past. The delay buffer is the number of intervals in the past 38 | // to set that fixed time offset to. 39 | // 40 | // When interpolating between (x, xd, xdd) way points at a non unity dt 41 | // (i.e. each way point is dt seconds apart), we need to scale the xd and 42 | // xdd by dt and dt^2, respectively, when adding them and undo that scaling 43 | // when evaluating. Intuition: if dt is small, it's moving fast from one 44 | // point to the next. If we then interpolate pretending that it takes a 45 | // full second to get from one to the next, it's moving and accelerating 46 | // much much slower, so we need to scale by dt and dt^2. 47 | // 48 | // This can be more rigorously derived by looking how time dilation scalars 49 | // propagate through the derivative expressions. 50 | class IncrementalInterpolator : public Interpolator { 51 | public: 52 | explicit IncrementalInterpolator(bool prune_history = true, 53 | bool validate_interpolation_evals = true); 54 | 55 | // Add a new waypoint, the time should be the current cycle time. Evals will 56 | // be offset into the past by delay_buffer number of intervals to that 57 | // incoming points can be added with the same time stamp as active 58 | // evaluations. 59 | bool AddPt(double t, const PosVelAccXd& p, 60 | std::string* error_str = nullptr) override; 61 | 62 | // Evaluates the interpolator at the given time. It uses a delay buffer to 63 | // offset the evaluations into the past so that points can be added at the 64 | // same time as evaluations and evaluations can be made after the latest 65 | // point safely as long as they're within the delay buffer (see description 66 | // above). 67 | // 68 | // This delay buffer functionality can also be implemented manually simply by 69 | // setting the delay_buffer to zero no construction and manually offsetting 70 | // evaluation points into the past. 71 | // 72 | // It's assumed the eval points are monotonically increasing. Fails if not. 73 | // the evaluation point is returned as ret. Returns true if successful and 74 | // false otherwise. 75 | bool Eval(double t, PosVelAccXd& ret, 76 | std::string* error_str = nullptr) const override; 77 | using Interpolator::Eval; 78 | 79 | int num_intervals() const { return segment_interpolators_.size(); } 80 | 81 | bool IsReady(double t) const; 82 | 83 | protected: 84 | mutable std::list segment_interpolators_; 85 | 86 | bool is_first_; 87 | double prev_add_t_; 88 | PosVelAccXd prev_add_p_; 89 | 90 | bool validate_interpolation_evals_; 91 | bool prune_history_; 92 | }; 93 | 94 | typedef IncrementalInterpolator SequentialQuinticInterpolator; 95 | 96 | } // namespace math 97 | } // namespace cortex 98 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/src/cortex/math/interpolation/interpolator.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2019-2022, NVIDIA CORPORATION. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION and its licensors retain all intellectual property 5 | * and proprietary rights in and to this software, related documentation 6 | * and any modifications thereto. Any use, reproduction, disclosure or 7 | * distribution of this software and related documentation without an express 8 | * license agreement from NVIDIA CORPORATION is strictly prohibited. 9 | */ 10 | 11 | // Pure virtual base class interface for an interpolator. 12 | 13 | #pragma once 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | 22 | #include "cortex/math/interpolation/pos_vel_acc.h" 23 | #include "cortex/math/interpolation/time_scaled_interpolator.h" 24 | 25 | namespace cortex { 26 | namespace math { 27 | 28 | // Represents a generic interpolator interface giving an API of the form: 29 | // 30 | // 1. Add p = (q, qd, qdd) point at time t: 31 | // 32 | // interp.AddPt(t, p); 33 | // 34 | // 2. Evaluate at a given time t: 35 | // 36 | // auto p = interp.Eval(t); 37 | // auto p = interp(t); 38 | // 39 | // Deriving classes need to implement the pure virtual functions 40 | // 41 | // AddPt() and Eval() 42 | // 43 | // Deriving classes might add additional restrictions, such as monotonicity of add 44 | // times t (IncrementalInterpolator). 45 | template 46 | class Interpolator { 47 | public: 48 | typedef vec_t VectorXx; 49 | 50 | virtual bool AddPt(double t, const PosVelAcc& p, std::string* error_str = nullptr) = 0; 51 | 52 | virtual bool Eval(double t, PosVelAcc& ret, std::string* error_str) const = 0; 53 | 54 | // Asserting version. 55 | PosVelAccXd Eval(double t) const { 56 | std::string error_str; 57 | PosVelAccXd p; 58 | ROS_ASSERT_MSG(Eval(t, p, &error_str), "%s", error_str.c_str()); 59 | return p; 60 | } 61 | 62 | Eigen::VectorXd operator()(double t) const { return Eval(t).x; } 63 | }; 64 | 65 | } // namespace math 66 | } // namespace cortex 67 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/src/cortex/math/interpolation/pos_vel_acc.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2019-2022, NVIDIA CORPORATION. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION and its licensors retain all intellectual property 5 | * and proprietary rights in and to this software, related documentation 6 | * and any modifications thereto. Any use, reproduction, disclosure or 7 | * distribution of this software and related documentation without an express 8 | * license agreement from NVIDIA CORPORATION is strictly prohibited. 9 | */ 10 | 11 | #pragma once 12 | 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | namespace cortex { 20 | namespace math { 21 | 22 | struct PosVelAcc1d; 23 | 24 | // Represents a triple of simultaneous position, velocity, and acceleration. 25 | template 26 | struct PosVelAcc { 27 | vec_t x; 28 | vec_t xd; 29 | vec_t xdd; 30 | 31 | int dim() const { return x.size(); } 32 | PosVelAcc Scale(double dt) const { return PosVelAcc(x, dt * xd, (dt * dt) * xdd); } 33 | PosVelAcc Unscale(double dt) const { 34 | return PosVelAcc(x, xd / dt, xdd / (dt * dt)); 35 | } 36 | 37 | PosVelAcc() {} 38 | 39 | // Initialize to all zeros with a particular dimensionality. 40 | explicit PosVelAcc(int d) { 41 | x = vec_t::Zero(d); 42 | xd = vec_t::Zero(d); 43 | xdd = vec_t::Zero(d); 44 | } 45 | 46 | // Initialize to specific (x, xd, xdd). Each vector much be the same 47 | // dimension, otherwise assert. 48 | PosVelAcc(const vec_t& x, const vec_t& xd, const vec_t& xdd); 49 | 50 | // Join a collection of one-dimensional PosVelAcc1d's into a single object of 51 | // this type. Aggregates the individual dimensions into vectors, x, xd, xdd. 52 | static PosVelAcc Join(const std::vector& dims); 53 | }; 54 | 55 | // One dimensional variant of pos, vel, acc. 56 | struct PosVelAcc1d { 57 | double x; 58 | double xd; 59 | double xdd; 60 | 61 | PosVelAcc1d() {} 62 | PosVelAcc1d(double x, double xd, double xdd) : x(x), xd(xd), xdd(xdd) {} 63 | 64 | // Slice a multi-dimensional pos, vel, acc into a one-dimensional variant 65 | // containing only the specified dimension. 66 | template 67 | static PosVelAcc1d Slice(const PosVelAcc& p, int dim) { 68 | return PosVelAcc1d(p.x[dim], p.xd[dim], p.xdd[dim]); 69 | } 70 | }; 71 | 72 | //============================================================================== 73 | // Template implementations 74 | //============================================================================== 75 | 76 | template 77 | PosVelAcc::PosVelAcc(const vec_t& x, const vec_t& xd, const vec_t& xdd) 78 | : x(x), xd(xd), xdd(xdd) { 79 | ROS_ASSERT(x.size() == xd.size()); 80 | ROS_ASSERT(x.size() == xdd.size()); 81 | } 82 | 83 | template 84 | PosVelAcc PosVelAcc::Join(const std::vector& dims) { 85 | PosVelAcc p(dims.size()); 86 | for (size_t i = 0; i < dims.size(); ++i) { 87 | p.x[i] = dims[i].x; 88 | p.xd[i] = dims[i].xd; 89 | p.xdd[i] = dims[i].xdd; 90 | } 91 | return p; 92 | } 93 | 94 | // Add specialization for VectorXd for convenience. 95 | typedef PosVelAcc PosVelAccXd; 96 | 97 | } // namespace math 98 | } // namespace cortex 99 | 100 | 101 | inline std::ostream& operator<<(std::ostream& os, const cortex::math::PosVelAcc1d& p) { 102 | os << " x = " << p.x << ", xd = " << p.xd << ", xdd = " << p.xdd; 103 | return os; 104 | } 105 | 106 | template 107 | std::ostream& operator<<(std::ostream& os, const cortex::math::PosVelAcc& p) { 108 | os << "x = " << p.x.transpose() << "\n"; 109 | os << "xd = " << p.xd.transpose() << "\n"; 110 | os << "xdd = " << p.xdd.transpose() << "\n"; 111 | return os; 112 | } 113 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/src/cortex/math/interpolation/quartic_interpolator.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2019-2022, NVIDIA CORPORATION. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION and its licensors retain all intellectual property 5 | * and proprietary rights in and to this software, related documentation 6 | * and any modifications thereto. Any use, reproduction, disclosure or 7 | * distribution of this software and related documentation without an express 8 | * license agreement from NVIDIA CORPORATION is strictly prohibited. 9 | */ 10 | 11 | #include "cortex/math/interpolation/quartic_interpolator.h" 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | namespace cortex { 20 | namespace math { 21 | 22 | // Returns true iff t \in [0,1]. 23 | inline bool InZeroOne(double t) { return 0 <= t && t <= 1; } 24 | 25 | // clang-format off 26 | #define QUARTIC_INTERP_MATRIX \ 27 | 0, 0, 0, 0, 1, \ 28 | 0, 0, 0, 1, 0, \ 29 | 0, 0, 2, 0, 0, \ 30 | 1, 1, 1, 1, 1, \ 31 | 4, 3, 2, 1, 0 32 | // clang-format on 33 | 34 | QuarticInterpolator1d::QuarticInterpolator1d(const PosVelAcc1d& p0, 35 | const PosVelAcc1d& p1, 36 | bool validate_interpolation_evals) 37 | : validate_interpolation_evals_(validate_interpolation_evals), 38 | A_((Eigen::MatrixXd(5, 5) << QUARTIC_INTERP_MATRIX).finished()), 39 | b_((Eigen::VectorXd(5) << p0.x, p0.xd, p0.xdd, p1.x, p1.xd).finished()), 40 | coeffs_(A_.colPivHouseholderQr().solve(b_)) {} 41 | 42 | bool QuarticInterpolator1d::Eval(double t, PosVelAcc1d& ret, std::string* error_str) const { 43 | if (validate_interpolation_evals_ && !InZeroOne(t)) { 44 | std::stringstream ss; 45 | ss << "t not in [0,1] (t = " << t << "). "; 46 | if (error_str) { 47 | *error_str += ss.str(); 48 | } 49 | return false; 50 | } 51 | auto a4 = coeffs_[0]; 52 | auto a3 = coeffs_[1]; 53 | auto a2 = coeffs_[2]; 54 | auto a1 = coeffs_[3]; 55 | auto a0 = coeffs_[4]; 56 | 57 | std::vector t_powers(5, 1); 58 | for (size_t i = 1; i < t_powers.size(); ++i) { 59 | t_powers[i] = t * t_powers[i - 1]; 60 | } 61 | 62 | auto x = a4 * t_powers[4] + a3 * t_powers[3] + a2 * t_powers[2] + a1 * t_powers[1] + a0; 63 | auto xd = 4. * a4 * t_powers[3] + 3. * a3 * t_powers[2] + 2. * a2 * t_powers[1] + a1; 64 | auto xdd = 12. * a4 * t_powers[2] + 6. * a3 * t_powers[1] + 2. * a2; 65 | 66 | ret = PosVelAcc1d(x, xd, xdd); 67 | return true; 68 | } 69 | 70 | } // namespace math 71 | } // namespace cortex 72 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/src/cortex/math/interpolation/quartic_interpolator.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2019-2022, NVIDIA CORPORATION. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION and its licensors retain all intellectual property 5 | * and proprietary rights in and to this software, related documentation 6 | * and any modifications thereto. Any use, reproduction, disclosure or 7 | * distribution of this software and related documentation without an express 8 | * license agreement from NVIDIA CORPORATION is strictly prohibited. 9 | */ 10 | 11 | #pragma once 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | 21 | #include "cortex/math/interpolation/pos_vel_acc.h" 22 | #include "cortex/math/interpolation/time_scaled_interpolator.h" 23 | #include "cortex/math/interpolation/trajectories.h" 24 | 25 | namespace cortex { 26 | namespace math { 27 | 28 | // One-dimensional quartic interpolating polynomial. Interpolates between 29 | // (x0, xd0, xdd0) and (x1, xd1). 30 | class QuarticInterpolator1d { 31 | public: 32 | typedef double VectorXx; 33 | 34 | QuarticInterpolator1d() {} 35 | 36 | // Creates a quintic spline that interpolates between p0 and p1 at t = 0 and 37 | // 1, respectively. 38 | QuarticInterpolator1d(const PosVelAcc1d& p0, 39 | const PosVelAcc1d& p1, 40 | bool validate_interpolation_evals = false); 41 | 42 | // Evaluate the polynomial at t. If validate_interpolating_evals is set to 43 | // true, enforces that the evaluations are only interpolating, i.e. t is in 44 | // [0, 1]; fails if not. The interpolated value is returned in the ret return 45 | // parameter. On failure, returns false and sets the error string if it's 46 | // provided. 47 | bool Eval(double t, PosVelAcc1d& ret, std::string* error_str = nullptr) const; 48 | 49 | // This verion asserts on error. 50 | PosVelAcc1d Eval(double t) const { 51 | PosVelAcc1d ret; 52 | std::string error_str; 53 | ROS_ASSERT_MSG(Eval(t, ret, &error_str), "%s", error_str.c_str()); 54 | return ret; 55 | } 56 | 57 | double operator()(double t) const { 58 | auto p = Eval(t); 59 | return p.x; 60 | } 61 | 62 | // Accessor. 63 | const Eigen::VectorXd& coeffs() const { return coeffs_; } 64 | 65 | protected: 66 | bool validate_interpolation_evals_; 67 | const Eigen::MatrixXd A_; 68 | const Eigen::VectorXd b_; 69 | const Eigen::VectorXd coeffs_; 70 | }; 71 | 72 | template 73 | MultiDimInterp QuarticInterpolator( 74 | const PosVelAcc& p0, 75 | const PosVelAcc& p1, 76 | bool validate_interpolation_evals = false) { 77 | return MultiDimInterp(p0, p1, validate_interpolation_evals); 78 | } 79 | 80 | typedef MultiDimInterp QuarticInterpolatorXd; 81 | // typedef TimeScaledInterpolator 82 | // TimeScaledInterpolatorXd; 83 | 84 | } // namespace math 85 | } // namespace cortex 86 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/src/cortex/math/interpolation/quintic_interpolator.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2019-2022, NVIDIA CORPORATION. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION and its licensors retain all intellectual property 5 | * and proprietary rights in and to this software, related documentation 6 | * and any modifications thereto. Any use, reproduction, disclosure or 7 | * distribution of this software and related documentation without an express 8 | * license agreement from NVIDIA CORPORATION is strictly prohibited. 9 | */ 10 | 11 | #include "cortex/math/interpolation/quintic_interpolator.h" 12 | 13 | #include 14 | #include 15 | 16 | #include 17 | 18 | namespace cortex { 19 | namespace math { 20 | 21 | // Returns true iff t \in [0,1]. 22 | inline bool InZeroOne(double t) { return 0 <= t && t <= 1; } 23 | 24 | inline Eigen::Matrix CubicInterpolationMatrix() { 25 | return (Eigen::MatrixXd(6, 6) << 0, 0, 0, 0, 0, 1, 26 | 0, 0, 0, 0, 1, 0, 27 | 0, 0, 0, 2, 0, 0, 28 | 1, 1, 1, 1, 1, 1, 29 | 5, 4, 3, 2, 1, 0, 30 | 20, 12, 6, 2, 0, 0).finished(); 31 | } 32 | 33 | QuinticInterpolator1d::QuinticInterpolator1d(const PosVelAcc1d& p0, 34 | const PosVelAcc1d& p1, 35 | bool validate_interpolation_evals) 36 | : validate_interpolation_evals_(validate_interpolation_evals), 37 | A_(CubicInterpolationMatrix()), 38 | b_((Eigen::VectorXd(6) << p0.x, p0.xd, p0.xdd, p1.x, p1.xd, p1.xdd).finished()), 39 | coeffs_(A_.colPivHouseholderQr().solve(b_)) {} 40 | 41 | bool QuinticInterpolator1d::Eval(double t, PosVelAcc1d& ret, std::string* error_str) const { 42 | if (validate_interpolation_evals_ && !InZeroOne(t)) { 43 | std::stringstream ss; 44 | ss << "t not in [0,1] (t = " << t << "). "; 45 | if (error_str) { 46 | *error_str += ss.str(); 47 | } 48 | return false; 49 | } 50 | auto a5 = coeffs_[0]; 51 | auto a4 = coeffs_[1]; 52 | auto a3 = coeffs_[2]; 53 | auto a2 = coeffs_[3]; 54 | auto a1 = coeffs_[4]; 55 | auto a0 = coeffs_[5]; 56 | 57 | std::vector t_powers(6, 1); 58 | for (size_t i = 1; i < t_powers.size(); ++i) { 59 | t_powers[i] = t * t_powers[i - 1]; 60 | } 61 | 62 | auto x = a5 * t_powers[5] + a4 * t_powers[4] + a3 * t_powers[3] + a2 * t_powers[2] + 63 | a1 * t_powers[1] + a0; 64 | auto xd = 5. * a5 * t_powers[4] + 4. * a4 * t_powers[3] + 3. * a3 * t_powers[2] + 65 | 2. * a2 * t_powers[1] + a1; 66 | auto xdd = 20. * a5 * t_powers[3] + 12. * a4 * t_powers[2] + 6. * a3 * t_powers[1] + 2. * a2; 67 | 68 | ret = PosVelAcc1d(x, xd, xdd); 69 | return true; 70 | } 71 | 72 | } // namespace math 73 | } // namespace cortex 74 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/src/cortex/math/interpolation/quintic_interpolator.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2019-2022, NVIDIA CORPORATION. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION and its licensors retain all intellectual property 5 | * and proprietary rights in and to this software, related documentation 6 | * and any modifications thereto. Any use, reproduction, disclosure or 7 | * distribution of this software and related documentation without an express 8 | * license agreement from NVIDIA CORPORATION is strictly prohibited. 9 | */ 10 | 11 | #pragma once 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | 21 | #include "cortex/math/interpolation/pos_vel_acc.h" 22 | #include "cortex/math/interpolation/time_scaled_interpolator.h" 23 | #include "cortex/math/interpolation/trajectories.h" 24 | 25 | namespace cortex { 26 | namespace math { 27 | 28 | // One-dimensional quintic interpolating polynomial. 29 | class QuinticInterpolator1d { 30 | public: 31 | typedef double VectorXx; 32 | 33 | QuinticInterpolator1d() {} 34 | 35 | // Creates a quintic spline that interpolates between p0 and p1 at t = 0 and 36 | // 1, respectively. 37 | QuinticInterpolator1d(const PosVelAcc1d& p0, 38 | const PosVelAcc1d& p1, 39 | bool validate_interpolation_evals = false); 40 | 41 | // Evaluate the polynomial at t. If validate_interpolating_evals is set to 42 | // true, enforces that the evaluations are only interpolating, i.e. t is in 43 | // [0, 1]; fails if not. The interpolated value is returned in the ret return 44 | // parameter. On failure, returns false and sets the error string if it's 45 | // provided. 46 | bool Eval(double t, PosVelAcc1d& ret, std::string* error_str = nullptr) const; 47 | 48 | // This verion asserts on error. 49 | PosVelAcc1d Eval(double t) const { 50 | PosVelAcc1d ret; 51 | std::string error_str; 52 | ROS_ASSERT_MSG(Eval(t, ret, &error_str), "%s", error_str.c_str()); 53 | return ret; 54 | } 55 | 56 | double operator()(double t) const { 57 | auto p = Eval(t); 58 | return p.x; 59 | } 60 | 61 | // Accessor. 62 | const Eigen::VectorXd& coeffs() const { return coeffs_; } 63 | 64 | protected: 65 | bool validate_interpolation_evals_; 66 | const Eigen::MatrixXd A_; 67 | const Eigen::VectorXd b_; 68 | const Eigen::VectorXd coeffs_; 69 | }; 70 | 71 | template 72 | MultiDimInterp QuinticInterpolator( 73 | const PosVelAcc& p0, 74 | const PosVelAcc& p1, 75 | bool validate_interpolation_evals = false) { 76 | return MultiDimInterp(p0, p1, validate_interpolation_evals); 77 | } 78 | 79 | typedef MultiDimInterp QuinticInterpolatorXd; 80 | typedef TimeScaledInterpolator TimeScaledInterpolatorXd; 81 | 82 | } // namespace math 83 | } // namespace cortex 84 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/src/cortex/math/interpolation/smoothing_incremental_interpolator.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2019-2022, NVIDIA CORPORATION. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION and its licensors retain all intellectual property 5 | * and proprietary rights in and to this software, related documentation 6 | * and any modifications thereto. Any use, reproduction, disclosure or 7 | * distribution of this software and related documentation without an express 8 | * license agreement from NVIDIA CORPORATION is strictly prohibited. 9 | */ 10 | 11 | #include "cortex/math/interpolation/smoothing_incremental_interpolator.h" 12 | 13 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/src/cortex/math/interpolation/time_scaled_interpolator.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2019-2022, NVIDIA CORPORATION. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION and its licensors retain all intellectual property 5 | * and proprietary rights in and to this software, related documentation 6 | * and any modifications thereto. Any use, reproduction, disclosure or 7 | * distribution of this software and related documentation without an express 8 | * license agreement from NVIDIA CORPORATION is strictly prohibited. 9 | */ 10 | 11 | #pragma once 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | 21 | #include "cortex/math/interpolation/pos_vel_acc.h" 22 | #include "cortex/math/interpolation/trajectories.h" 23 | 24 | namespace cortex { 25 | namespace math { 26 | 27 | // Represents a quintic interpolator interpolating between two end points 28 | // at specific times. If validate_interpolation_evals is true, valid evals 29 | // are only those within the time range of the two end points. 30 | template 31 | class TimeScaledInterpolator { 32 | public: 33 | typedef typename TimeScaledTraj::VectorXx VectorXx; 34 | 35 | TimeScaledInterpolator() {} 36 | TimeScaledInterpolator(double t0, 37 | const PosVelAcc& p0, 38 | double t1, 39 | const PosVelAcc& p1, 40 | bool validate_interpolation_evals = false) 41 | : t0_(t0), 42 | p0_(p0), 43 | t1_(t1), 44 | p1_(p1), 45 | time_range_(t1 - t0), 46 | scaled_traj_( 47 | traj_t(p0.Scale(time_range_), p1.Scale(time_range_), validate_interpolation_evals), 48 | time_range_), 49 | validate_interpolation_evals_(validate_interpolation_evals) {} 50 | 51 | bool Eval(double t, PosVelAcc& ret, std::string* error_str = nullptr) const { 52 | if (validate_interpolation_evals_ && !(t0_ <= t && t <= t1_)) { 53 | if (error_str) { 54 | std::stringstream ss; 55 | ss << "t = " << t << " outside valid range [" << t0_ << ", " << t1_ << "]"; 56 | *error_str += ss.str(); 57 | } 58 | return false; 59 | } 60 | return scaled_traj_.Eval((t - t0_) / time_range_, ret, error_str); 61 | } 62 | PosVelAcc Eval(double t) const { 63 | std::string error_str; 64 | PosVelAcc ret; 65 | ROS_ASSERT_MSG(scaled_traj_.Eval((t - t0_) / time_range_, ret, &error_str), "%s", 66 | error_str.c_str()); 67 | return ret; 68 | } 69 | 70 | // Performs a time shifted eval since the underlying trajectory starts at t0_ 71 | VectorXx operator()(double t) const { return Eval(t).x; } 72 | 73 | double t0() const { return t0_; } 74 | const PosVelAcc& p0() const { return p0_; } 75 | double t1() const { return t1_; } 76 | const PosVelAcc& p1() const { return p1_; } 77 | 78 | protected: 79 | double t0_; 80 | PosVelAcc p0_; 81 | double t1_; 82 | PosVelAcc p1_; 83 | 84 | double time_range_; 85 | TimeScaledTraj scaled_traj_; 86 | bool validate_interpolation_evals_; 87 | }; 88 | 89 | } // namespace math 90 | } // namespace cortex 91 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/src/cortex/math/state.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2019-2022, NVIDIA CORPORATION. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION and its licensors retain all intellectual property 5 | * and proprietary rights in and to this software, related documentation 6 | * and any modifications thereto. Any use, reproduction, disclosure or 7 | * distribution of this software and related documentation without an express 8 | * license agreement from NVIDIA CORPORATION is strictly prohibited. 9 | */ 10 | 11 | #include "cortex/math/state.h" 12 | 13 | #include 14 | #include 15 | 16 | namespace cortex { 17 | namespace math { 18 | 19 | State::State(int n) : state(Eigen::VectorXd::Zero(2 * n)) {} 20 | State::State(const Eigen::VectorXd &x, const Eigen::VectorXd &xd) : State(x.size()) { 21 | ROS_ASSERT(x.size() == xd.size()); 22 | pos() = x; 23 | vel() = xd; 24 | } 25 | 26 | } // namespace math 27 | } // namespace cortex 28 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/src/cortex/math/state.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2019-2022, NVIDIA CORPORATION. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION and its licensors retain all intellectual property 5 | * and proprietary rights in and to this software, related documentation 6 | * and any modifications thereto. Any use, reproduction, disclosure or 7 | * distribution of this software and related documentation without an express 8 | * license agreement from NVIDIA CORPORATION is strictly prohibited. 9 | */ 10 | 11 | #pragma once 12 | 13 | #include 14 | 15 | namespace cortex { 16 | namespace math { 17 | 18 | // Represents a vector s = (x, xd) \in \R^{2d} where d is the space dim. 19 | class State { 20 | public: 21 | State() = delete; 22 | explicit State(int n); // Initialize to the zero state (0,0)\in\R^n X \R^n. 23 | State(const Eigen::VectorXd &x, const Eigen::VectorXd &xd); 24 | 25 | Eigen::Ref pos() { return state.head(dim()); } 26 | Eigen::Ref vel() { return state.tail(dim()); } 27 | Eigen::Ref vector() { return state; } 28 | Eigen::Ref pos() const { return state.head(dim()); } 29 | Eigen::Ref vel() const { return state.tail(dim()); } 30 | Eigen::Ref vector() const { return state; } 31 | 32 | int dim() const { return state.size() / 2; } 33 | 34 | // Returns one integration step forward. 35 | // 36 | // Equations: 37 | // x_next = x + dt xd 38 | // xd_next = xd + dt xdd 39 | State Step(double dt, const Eigen::VectorXd &xdd) { 40 | return State(pos() + dt * vel(), vel() + dt * xdd); 41 | } 42 | 43 | private: 44 | Eigen::VectorXd state; 45 | }; 46 | 47 | } // namespace math 48 | } // namespace cortex 49 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/src/cortex/util/joint_state_publisher.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (c) 2019-2022, NVIDIA CORPORATION. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION and its licensors retain all intellectual property 5 | * and proprietary rights in and to this software, related documentation 6 | * and any modifications thereto. Any use, reproduction, disclosure or 7 | * distribution of this software and related documentation without an express 8 | * license agreement from NVIDIA CORPORATION is strictly prohibited. 9 | */ 10 | 11 | #include "joint_state_publisher.h" 12 | 13 | namespace cortex { 14 | namespace util { 15 | 16 | } // namespace util 17 | } // namespace cortex 18 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/src/cortex/util/joint_state_publisher.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2019-2022, NVIDIA CORPORATION. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION and its licensors retain all intellectual property 5 | * and proprietary rights in and to this software, related documentation 6 | * and any modifications thereto. Any use, reproduction, disclosure or 7 | * distribution of this software and related documentation without an express 8 | * license agreement from NVIDIA CORPORATION is strictly prohibited. 9 | */ 10 | 11 | //! @file 12 | //! @brief A simple and general joint state listener to collect the latest information 13 | //! about the robot's state. 14 | 15 | #pragma once 16 | 17 | #include 18 | 19 | #include 20 | #include 21 | 22 | #include "cortex/math/state.h" 23 | 24 | namespace cortex { 25 | namespace util { 26 | 27 | class JointStatePublisher { 28 | public: 29 | JointStatePublisher(const std::vector& joint_names, 30 | const std::string& topic, 31 | int queue_size) 32 | : joint_names_(joint_names), seq_(0) { 33 | ros::NodeHandle node_handle; 34 | pub_ = node_handle.advertise(topic, queue_size); 35 | } 36 | 37 | void Publish(const math::State& state) { 38 | sensor_msgs::JointState msg; 39 | 40 | msg.header.seq = seq_++; 41 | msg.header.stamp = ros::Time::now(); 42 | msg.name = joint_names_; 43 | msg.position = std::vector(state.pos().data(), state.pos().data() + state.pos().size()); 44 | msg.velocity = std::vector(state.vel().data(), state.vel().data() + state.vel().size()); 45 | 46 | pub_.publish(msg); 47 | } 48 | 49 | protected: 50 | ros::Publisher pub_; 51 | std::vector joint_names_; 52 | int32_t seq_; 53 | }; 54 | 55 | } // namespace util 56 | } // namespace cortex 57 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/src/cortex/util/ros_message_listener.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2021-2022, NVIDIA CORPORATION. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION and its licensors retain all intellectual property 5 | * and proprietary rights in and to this software, related documentation 6 | * and any modifications thereto. Any use, reproduction, disclosure or 7 | * distribution of this software and related documentation without an express 8 | * license agreement from NVIDIA CORPORATION is strictly prohibited. 9 | */ 10 | #pragma once 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | namespace cortex { 20 | namespace util { 21 | 22 | // Generic message listener that saves off the latest message and makes it available atomically. 23 | // 24 | // Includes flag accessor is_available() saying whether the first message has been received. 25 | // Thereafter, it always reports the last received message through GetLatestMessage(). There is no 26 | // timeout mechanism on these messages, so once is_available() returns true for the first time, it 27 | // will be true for every call after that. 28 | template 29 | class RosMessageListener { 30 | public: 31 | RosMessageListener(const std::string& topic, int queue_size) { 32 | is_available_ = false; 33 | ros::NodeHandle node_handle; 34 | sub_ = node_handle.subscribe(topic, queue_size, &RosMessageListener::Callback, this); 35 | } 36 | 37 | void Callback(const msg_t& msg) { 38 | std::lock_guard guard(mutex_); 39 | msg_ = msg; 40 | is_available_ = true; 41 | 42 | for (auto& f : callbacks_) { 43 | f(msg_); 44 | } 45 | } 46 | 47 | bool is_available() const { return is_available_; } 48 | 49 | msg_t GetLatestMessage() const { 50 | std::lock_guard guard(mutex_); 51 | return msg_; 52 | } 53 | 54 | void RegisterCallback(const std::function& f) { callbacks_.push_back(f); } 55 | 56 | protected: 57 | mutable std::mutex mutex_; 58 | ros::Subscriber sub_; 59 | std::atomic_bool is_available_; 60 | 61 | msg_t msg_; 62 | 63 | std::vector> callbacks_; 64 | }; 65 | 66 | } // namespace util 67 | } // namespace cortex 68 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/src/cortex/util/ros_util.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2019-2022, NVIDIA CORPORATION. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION and its licensors retain all intellectual property 5 | * and proprietary rights in and to this software, related documentation 6 | * and any modifications thereto. Any use, reproduction, disclosure or 7 | * distribution of this software and related documentation without an express 8 | * license agreement from NVIDIA CORPORATION is strictly prohibited. 9 | */ 10 | 11 | #include "cortex/util/ros_util.h" 12 | 13 | #include 14 | 15 | #include 16 | #include 17 | 18 | #include "cortex/util/string.h" 19 | 20 | namespace cortex { 21 | namespace util { 22 | 23 | void WaitForConnections(const ros::Publisher& pub, double stable_time, double rate_hz) { 24 | std::cout << "Waiting for connections" << std::flush; 25 | 26 | auto rate = ros::Rate(rate_hz); 27 | 28 | auto last_change_time = ros::Time::now(); 29 | auto num_con = pub.getNumSubscribers(); 30 | 31 | while (ros::ok()) { 32 | std::cout << '.' << std::flush; 33 | auto curr_time = ros::Time::now(); 34 | 35 | auto latest_num_con = pub.getNumSubscribers(); 36 | auto elapse_sec = (curr_time - last_change_time).toSec(); 37 | if (latest_num_con != num_con) { 38 | num_con = latest_num_con; 39 | std::cout << num_con << std::flush; 40 | last_change_time = curr_time; 41 | } else if (latest_num_con > 0 && latest_num_con == num_con && elapse_sec >= stable_time) { 42 | std::cout << "" << std::endl; 43 | break; 44 | } 45 | rate.sleep(); 46 | } 47 | } 48 | 49 | std::string ExpandRosPkgRelPathRaw(const std::string& pkg_relative_path) { 50 | // Parse out the json config file. 51 | char delim = '/'; 52 | std::vector tokens = Split(pkg_relative_path, delim); 53 | if (tokens.size() == 0) { 54 | return ""; 55 | } else if (tokens.size() < 2) { 56 | return tokens.front(); 57 | } 58 | auto pkg_name = tokens.front(); 59 | auto rel_path = Join(tokens, delim, 1); // Join all but first. 60 | 61 | auto package_path = ros::package::getPath(pkg_name); 62 | auto full_path = package_path + delim + rel_path; 63 | return full_path; 64 | } 65 | 66 | std::string ExpandRosPkgRelPath(const std::string& pkg_relative_path) { 67 | std::string expected_prefix = "package://"; 68 | if (pkg_relative_path.find(expected_prefix) == 0) { 69 | return ExpandRosPkgRelPathRaw(pkg_relative_path.substr(expected_prefix.size())); 70 | } else { 71 | // The string doesn't start with the expected prefix, but we're still 72 | // supporting that for the time being. WARNING -- this functionality 73 | // is DEPRECATED; we'll require the package:// prefix soon. 74 | ROS_WARN_STREAM( 75 | "Package expansion without the 'package://' prefix is DEPRECATED: " << pkg_relative_path); 76 | return ExpandRosPkgRelPathRaw(pkg_relative_path); 77 | } 78 | } 79 | 80 | } // namespace util 81 | } // namespace cortex 82 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/src/cortex/util/ros_util.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2019-2022, NVIDIA CORPORATION. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION and its licensors retain all intellectual property 5 | * and proprietary rights in and to this software, related documentation 6 | * and any modifications thereto. Any use, reproduction, disclosure or 7 | * distribution of this software and related documentation without an express 8 | * license agreement from NVIDIA CORPORATION is strictly prohibited. 9 | */ 10 | 11 | #pragma once 12 | 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | namespace cortex { 22 | namespace util { 23 | 24 | //------------------------------------------------------------------------------ 25 | // Parameter helpers 26 | //------------------------------------------------------------------------------ 27 | 28 | /*!\briefGeneric more convenient ROS parameter retrieval method that explicitly 29 | * returns the parameter value. 30 | * 31 | * Call as: 32 | * 33 | * auto value = GetParam("/robot/step_size", .5); 34 | * auto str_value = GetParam("/robot/controller_name", "lqr_controller"); 35 | * 36 | * Infers the type by the type of the default value passed in. 37 | * 38 | * TODO: Figure out a way to get this to work with passing in const char* 39 | * string literals. 40 | */ 41 | template 42 | value_t GetParam(const std::string& param_name, const value_t& default_value) { 43 | value_t param_value; 44 | ros::param::param(param_name, param_value, default_value); 45 | return param_value; 46 | } 47 | 48 | /*!\brief Call as: auto value = GetParam("/robot/step_size"); Need to 49 | * specific supply the template argument for the parameter type. 50 | */ 51 | template 52 | value_t GetParam(const std::string& param_name) { 53 | value_t param_value; 54 | ros::param::get(param_name, param_value); 55 | return param_value; 56 | } 57 | 58 | /*!\brief Get all parameters under a particular namespace. 59 | */ 60 | std::vector GetNsParams(const std::string& ns); 61 | 62 | /*!\brief Returns all of the names and corresponding tags under the given 63 | * namespace. 64 | * 65 | * Returns a vector of pairs with the first element being a name and the second 66 | * being a vector of strings for the tags: 67 | * 68 | * /ns/first/1 69 | * /ns/first/2 70 | * /ns/second 71 | * /ns/third/1 72 | * /ns/third/2 73 | * /ns/third/3 74 | * 75 | * Corresponding return structure: 76 | * 77 | * { "first", {"1", "2"}, 78 | * "second", {}, 79 | * "third", {"1", "2", "3"} } 80 | * 81 | */ 82 | void GetNsElements(const std::string& ns, std::map>& elements); 83 | 84 | 85 | //------------------------------------------------------------------------------ 86 | // Subscription helpers 87 | //------------------------------------------------------------------------------ 88 | 89 | /*!\brief Wait until connections to the publisher stabilize. 90 | * 91 | * Checks at a rate of rate_hz, and requires that the number of subscribers 92 | * doesn't change for stable_time seconds before considering the connection to 93 | * be stable and returning. 94 | */ 95 | void WaitForConnections(const ros::Publisher& pub, double stable_time = .2, double rate_hz = 30.); 96 | 97 | 98 | //------------------------------------------------------------------------------ 99 | // Package helpers 100 | //------------------------------------------------------------------------------ 101 | 102 | /*!\brief Converts a ROS package relative path into a full path. 103 | * 104 | * The ROS package relative path should take the form: 105 | * package:/// 106 | * 107 | * Returns / 108 | * 109 | * For legacy reasons, currently the package:// prefix can be left off, but 110 | * that functionality is nonstandard with ROS and now deprecated. In the near 111 | * future, we'll require these strings to be prefixed with package://. 112 | */ 113 | std::string ExpandRosPkgRelPath(const std::string& pkg_relative_path); 114 | 115 | } // namespace util 116 | } // namespace cortex 117 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/src/cortex/util/set_state_listener.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016-2022, NVIDIA CORPORATION. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION and its licensors retain all intellectual property 5 | * and proprietary rights in and to this software, related documentation 6 | * and any modifications thereto. Any use, reproduction, disclosure or 7 | * distribution of this software and related documentation without an express 8 | * license agreement from NVIDIA CORPORATION is strictly prohibited. 9 | */ 10 | 11 | #pragma once 12 | 13 | #include 14 | 15 | #include "cortex/util/state_listener.h" 16 | 17 | namespace cortex { 18 | namespace util { 19 | 20 | /** 21 | * \brief This is a very simple state listener that just reports its set state. 22 | */ 23 | class SetStateListener : public StateListener { 24 | public: 25 | SetStateListener() : is_set_(false) {} 26 | StampedState State() const override { return state_; } 27 | bool IsReady() const override { return is_set_; } 28 | void set_stamped_state(const StampedState &state) { state_ = state; } 29 | 30 | protected: 31 | bool is_set_; 32 | StampedState state_; 33 | }; 34 | 35 | } // namespace util 36 | } // namespace cortex 37 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/src/cortex/util/stamped_state.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016-2022, NVIDIA CORPORATION. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION and its licensors retain all intellectual property 5 | * and proprietary rights in and to this software, related documentation 6 | * and any modifications thereto. Any use, reproduction, disclosure or 7 | * distribution of this software and related documentation without an express 8 | * license agreement from NVIDIA CORPORATION is strictly prohibited. 9 | */ 10 | 11 | #include 12 | 13 | #include "cortex/util/state_listener.h" 14 | 15 | namespace cortex { 16 | namespace util { 17 | 18 | bool StampedState::HasU() const { return u.size() > 0; } 19 | 20 | StampedState::StampedState(uint32_t num_dim) 21 | : time(0.), 22 | q(Eigen::VectorXd::Zero(num_dim)), 23 | qd(Eigen::VectorXd::Zero(num_dim)), 24 | u(Eigen::VectorXd::Zero(num_dim)) {} 25 | 26 | StampedState::StampedState(double time, const Eigen::VectorXd &q, const Eigen::VectorXd &qd) 27 | : time(time), q(q), qd(qd) {} 28 | 29 | } // namespace util 30 | } // namespace cortex 31 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/src/cortex/util/stamped_state.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016-2022, NVIDIA CORPORATION. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION and its licensors retain all intellectual property 5 | * and proprietary rights in and to this software, related documentation 6 | * and any modifications thereto. Any use, reproduction, disclosure or 7 | * distribution of this software and related documentation without an express 8 | * license agreement from NVIDIA CORPORATION is strictly prohibited. 9 | */ 10 | 11 | #pragma once 12 | 13 | #include 14 | 15 | #include 16 | 17 | namespace cortex { 18 | namespace util { 19 | 20 | struct StampedState { 21 | double time; 22 | Eigen::VectorXd q; 23 | Eigen::VectorXd qd; 24 | Eigen::VectorXd u; 25 | 26 | int dim() const { return q.size(); } 27 | 28 | StampedState() = default; 29 | StampedState(uint32_t num_dim); 30 | StampedState(double time, const Eigen::VectorXd &q, const Eigen::VectorXd &qd); 31 | virtual ~StampedState() = default; 32 | 33 | bool HasU() const; 34 | }; 35 | 36 | } // namespace util 37 | } // namespace cortex 38 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/src/cortex/util/state_listener.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016-2022, NVIDIA CORPORATION. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION and its licensors retain all intellectual property 5 | * and proprietary rights in and to this software, related documentation 6 | * and any modifications thereto. Any use, reproduction, disclosure or 7 | * distribution of this software and related documentation without an express 8 | * license agreement from NVIDIA CORPORATION is strictly prohibited. 9 | */ 10 | 11 | #include "cortex/util/state_listener.h" 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | namespace cortex { 20 | namespace util { 21 | 22 | // std::atomic_bool StateListener::interruped_(false); 23 | 24 | StateListener::StateListener() { 25 | // std::signal(SIGINT, &StateListener::signal_handler); 26 | } 27 | 28 | void StateListener::WaitForReady(double poll_hz) const { 29 | // This is an alternative and ros free implementation of the thread SIGINT 30 | // signal handling 31 | 32 | // auto sleep_duration = std::chrono::duration(1. / poll_hz); 33 | // while (!interruped_.load() && !IsReady()) { 34 | // std::this_thread::sleep_for(sleep_duration); 35 | // } 36 | 37 | ros::Rate rate(poll_hz); 38 | while (ros::ok() && !IsReady()) { 39 | rate.sleep(); 40 | } 41 | } 42 | 43 | // void StateListener::signal_handler(int signal) { interruped_.store(true); } 44 | 45 | } // namespace util 46 | } // namespace cortex 47 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/src/cortex/util/state_listener.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016-2022, NVIDIA CORPORATION. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION and its licensors retain all intellectual property 5 | * and proprietary rights in and to this software, related documentation 6 | * and any modifications thereto. Any use, reproduction, disclosure or 7 | * distribution of this software and related documentation without an express 8 | * license agreement from NVIDIA CORPORATION is strictly prohibited. 9 | */ 10 | 11 | #pragma once 12 | 13 | #include 14 | 15 | #include 16 | 17 | #include "cortex/util/stamped_state.h" 18 | 19 | namespace cortex { 20 | namespace util { 21 | 22 | /** 23 | * \brief Abstract state listener. 24 | */ 25 | class StateListener { 26 | public: 27 | /** 28 | * \brief Creates a StateListener. 29 | */ 30 | StateListener(); 31 | 32 | /** 33 | * \brief Default virtual destructor. 34 | */ 35 | virtual ~StateListener() = default; 36 | 37 | /** 38 | * \brief Returns the latest state. 39 | */ 40 | virtual StampedState State() const = 0; 41 | 42 | /** 43 | * \brief Returns true if the state is available. 44 | */ 45 | virtual bool IsReady() const = 0; 46 | 47 | /** 48 | * \brief Blocking call to wait until the state is available. 49 | */ 50 | virtual void WaitForReady(double poll_hz = 100) const; 51 | 52 | private: 53 | // This is an alternative and ros free implementation of the thread SIGINT 54 | // signal handling 55 | // static void signal_handler(int signal); 56 | // static std::atomic_bool interruped_; 57 | }; 58 | 59 | } // namespace util 60 | } // namespace cortex 61 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/src/cortex/util/string.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2019-2022, NVIDIA CORPORATION. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION and its licensors retain all intellectual property 5 | * and proprietary rights in and to this software, related documentation 6 | * and any modifications thereto. Any use, reproduction, disclosure or 7 | * distribution of this software and related documentation without an express 8 | * license agreement from NVIDIA CORPORATION is strictly prohibited. 9 | */ 10 | 11 | #include "cortex/util/string.h" 12 | 13 | #include 14 | 15 | namespace cortex { 16 | namespace util { 17 | 18 | std::vector Split(const std::string& str, char delimiter){ 19 | std::vector tokens; 20 | std::string token; 21 | std::istringstream token_stream(str); 22 | while (std::getline(token_stream, token, delimiter)) { 23 | if (token.size() > 0) { 24 | tokens.push_back(token); 25 | } 26 | } 27 | return tokens; 28 | } 29 | 30 | std::string Join(const std::vector& tokens, char delimiter, size_t pos) { 31 | std::stringstream ss; 32 | for (auto i = pos; i < tokens.size(); ++i) { 33 | if (i > pos) ss << delimiter; 34 | ss << tokens[i]; 35 | } 36 | return ss.str(); 37 | } 38 | 39 | } // namespace util 40 | } // namespace cortex 41 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/src/cortex/util/string.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2019-2022, NVIDIA CORPORATION. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION and its licensors retain all intellectual property 5 | * and proprietary rights in and to this software, related documentation 6 | * and any modifications thereto. Any use, reproduction, disclosure or 7 | * distribution of this software and related documentation without an express 8 | * license agreement from NVIDIA CORPORATION is strictly prohibited. 9 | */ 10 | 11 | #pragma once 12 | 13 | #include 14 | #include 15 | 16 | namespace cortex { 17 | namespace util { 18 | 19 | //! Split the specified string `str` into a set of strings delimited by the `delimiter` character. 20 | //! If the delimiter is not found, the entire string is returned as a single token. The returned 21 | //! vector always contains, in union, the set of all characters in the string that aren't 22 | //! delimiters. 23 | std::vector Split(const std::string& str, char delimiter); 24 | 25 | //! Join the tokens together separated by the specified `delimiter` character. Start with token 26 | //! `pos`. By default, `pos` is zero, so all tokens are included. 27 | std::string Join(const std::vector& tokens, char delimiter, size_t pos = 0); 28 | 29 | } // namespace util 30 | } // namespace cortex 31 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/src/cortex/util/yaml.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2019-2022, NVIDIA CORPORATION. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION and its licensors retain all intellectual property 5 | * and proprietary rights in and to this software, related documentation 6 | * and any modifications thereto. Any use, reproduction, disclosure or 7 | * distribution of this software and related documentation without an express 8 | * license agreement from NVIDIA CORPORATION is strictly prohibited. 9 | */ 10 | 11 | #pragma once 12 | 13 | #include 14 | #include 15 | 16 | namespace cortex { 17 | namespace util { 18 | 19 | //! Extract the named YAML field or assert if the field doesn't exist. 20 | YAML::Node GetFieldOrDie(const YAML::Node& node, const std::string& name) { 21 | auto field = node[name]; 22 | ROS_ASSERT_MSG(field, "YAML field not found: %s", name.c_str()); 23 | return field; 24 | } 25 | 26 | //! Extract a field of the specified type from the YAML node or assert if the field doesn't exist. 27 | template 28 | T GetOrDie(const YAML::Node& node, const std::string& name) { 29 | auto field = node[name]; 30 | ROS_ASSERT_MSG(field, "Could not extract YAML field: %s", name.c_str()); 31 | return field.as(); 32 | } 33 | 34 | } // namespace util 35 | } // namespace cortex 36 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control/srv/MsgService.srv: -------------------------------------------------------------------------------- 1 | # commands sent in json format 2 | string msg 3 | --- 4 | uint8 SUCCESS=0 5 | uint8 FAILURE=1 6 | uint8 result 7 | string error_str 8 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control_franka/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(cortex_control_franka) 3 | 4 | set(CMAKE_BUILD_TYPE Release) 5 | 6 | set(CMAKE_CXX_STANDARD 17) 7 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 8 | 9 | set(PKG_DEPENDS_LIST 10 | controller_interface 11 | cortex_control 12 | dynamic_reconfigure 13 | franka_hw 14 | hardware_interface 15 | pluginlib 16 | realtime_tools 17 | roscpp 18 | std_msgs 19 | ) 20 | 21 | find_package( 22 | catkin 23 | REQUIRED 24 | COMPONENTS 25 | ${PKG_DEPENDS_LIST} 26 | ) 27 | 28 | generate_dynamic_reconfigure_options( 29 | cfg/compliance_param.cfg 30 | cfg/desired_mass_param.cfg 31 | ) 32 | 33 | 34 | catkin_package( 35 | INCLUDE_DIRS include 36 | LIBRARIES cortex_control_franka 37 | CATKIN_DEPENDS 38 | ${PKG_DEPENDS_LIST} 39 | ) 40 | 41 | include_directories( 42 | include 43 | ${catkin_INCLUDE_DIRS} 44 | ) 45 | 46 | 47 | add_library( 48 | ${PROJECT_NAME} 49 | src/interpolated_command_stream_controller.cpp 50 | ) 51 | target_link_libraries( 52 | ${PROJECT_NAME} 53 | ${catkin_LIBRARIES} 54 | ) 55 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control_franka/cfg/compliance_param.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "franka_controllers" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("translational_stiffness", double_t, 0, "Cartesian translational stiffness", 200, 0, 400) 9 | gen.add("rotational_stiffness", double_t, 0, "Cartesian rotational stiffness", 10, 0, 30) 10 | gen.add("nullspace_stiffness", double_t, 0, "Stiffness of the joint space nullspace controller (the desired configuration is the one at startup)", 0, 0, 100) 11 | 12 | exit(gen.generate(PACKAGE, "dynamic_compliance", "compliance_param")) 13 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control_franka/cfg/desired_mass_param.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "franka_controllers" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("desired_mass", double_t, 0, "desired mass for rendered force due to gravity applied in the z axis", 0.0, 0.0, 2.0) 9 | gen.add("k_p", double_t, 0, "force P gain", 0.0, 0.0, 2.0) 10 | gen.add("k_i", double_t, 0, "force I gain", 0.0, 0.0, 2.0) 11 | 12 | exit(gen.generate(PACKAGE, "dynamic_mass", "desired_mass_param")) 13 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control_franka/config/command_stream_interpolator.yaml: -------------------------------------------------------------------------------- 1 | params: 2 | interpolation_delay: .2 3 | use_smoothing_interpolator: true 4 | blending_duration: 2. 5 | backend_timeout: .5 6 | ros_topics: 7 | joint_state: /robot/joint_state 8 | rmpflow_commands: 9 | command: /cortex/arm/command 10 | ack: /cortex/arm/command/ack 11 | suppress: /cortex/arm/command/suppress 12 | interpolated: /cortex/arm/command/interpolated 13 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control_franka/config/controller.yaml: -------------------------------------------------------------------------------- 1 | joint_position_controller: 2 | type: cortex_control_franka/InterpolatedCommandStreamController 3 | joint_names: 4 | - panda_joint1 5 | - panda_joint2 6 | - panda_joint3 7 | - panda_joint4 8 | - panda_joint5 9 | - panda_joint6 10 | - panda_joint7 11 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control_franka/include/cortex/control/franka/interpolated_command_stream_controller.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019-2022, NVIDIA CORPORATION. All rights reserved. 2 | // 3 | // NVIDIA CORPORATION and its licensors retain all intellectual property 4 | // and proprietary rights in and to this software, related documentation 5 | // and any modifications thereto. Any use, reproduction, disclosure or 6 | // distribution of this software and related documentation without an express 7 | // license agreement from NVIDIA CORPORATION is strictly prohibited. 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | namespace cortex { 24 | namespace control { 25 | namespace franka { 26 | 27 | /** 28 | * \brief Joint position controller using cortex rmp control commands. 29 | * 30 | * This controller forwards with interpolation the received cortex 31 | * control commands to the robot's joint interface. 32 | */ 33 | class InterpolatedCommandStreamController 34 | : public controller_interface::MultiInterfaceController< 35 | hardware_interface::PositionJointInterface> { 36 | public: 37 | /** 38 | * \brief Initializes the controller. 39 | * 40 | * \param robot_hardware handle to the robot's hardware abstraction 41 | * \param node_handle node handle instance 42 | */ 43 | bool init(hardware_interface::RobotHW *robot_hardware, ros::NodeHandle &node_handle) override; 44 | 45 | /** 46 | * \brief Initialization of the controller upon activation. 47 | * 48 | * \param time time at which the controller was activated 49 | */ 50 | void starting(ros::Time const &time) override; 51 | 52 | /** 53 | * \brief Control update loop execution. 54 | * 55 | * \param time current time 56 | * \param period time elapsed since last call 57 | */ 58 | void update(ros::Time const &time, ros::Duration const &period) override; 59 | 60 | private: 61 | /** 62 | * \brief Retrieves the current position from the joint handles. 63 | */ 64 | Eigen::VectorXd current_position() const; 65 | 66 | /** 67 | * \brief Sends the robot's current pose to the robot. 68 | */ 69 | void send_current_position(); 70 | 71 | /** 72 | * \brief Sends the defined position to the robot's joints. 73 | * 74 | * \param q joint position to be sent to the robot 75 | */ 76 | void send_position_command(const Eigen::VectorXd &q); 77 | 78 | private: 79 | std::shared_ptr command_stream_interpolator_; 80 | 81 | bool initialize_blending_; 82 | ros::Time controller_time_; 83 | ros::Time start_time_; 84 | hardware_interface::PositionJointInterface *joint_interface_; 85 | std::vector joint_handles_; 86 | 87 | ros::Duration print_period_; 88 | ros::Time next_print_time_; 89 | }; 90 | 91 | } // namespace franka 92 | } // namespace control 93 | } // namespace cortex 94 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control_franka/launch/franka_control_cortex.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | [franka_state_controller/joint_states, franka_gripper/joint_states] 26 | [franka_state_controller/joint_states] 27 | 28 | 29 | 30 | 31 | 32 | [franka_state_controller/joint_states_desired, franka_gripper/joint_states] 33 | [franka_state_controller/joint_states_desired] 34 | 35 | 36 | 37 | 38 | 39 | 40 | [franka_state_controller/joint_states, franka_gripper/joint_states] 41 | [franka_state_controller/joint_states] 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control_franka/launch/joint_position_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control_franka/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | cortex_control_franka 4 | 0.1.0 5 | Cortex Control ROS extensions with Franka ROS dependencies. 6 | 7 | Isaac Sim 8 | 9 | Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. 10 | NVIDIA CORPORATION and its licensors retain all intellectual property 11 | and proprietary rights in and to this software, related documentation 12 | and any modifications thereto. Any use, reproduction, disclosure or 13 | distribution of this software and related documentation without an express 14 | license agreement from NVIDIA CORPORATION is strictly prohibited. 15 | 16 | https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html 17 | https://forums.developer.nvidia.com/c/omniverse/simulation/69 18 | 19 | catkin 20 | 21 | controller_interface 22 | dynamic_reconfigure 23 | franka_hw 24 | hardware_interface 25 | cortex_control 26 | pluginlib 27 | realtime_tools 28 | roscpp 29 | std_msgs 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control_franka/plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Receives a stream of commands from a reactive motion generator, interpolates them, and 5 | streams realtime position commands to Franka Panda's joint-space position controller. 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /noetic_ws/src/cortex_control_franka/src/python/set_high_collision_thresholds.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2021-2022, NVIDIA CORPORATION. All rights reserved. 4 | # 5 | # NVIDIA CORPORATION and its licensors retain all intellectual property 6 | # and proprietary rights in and to this software, related documentation 7 | # and any modifications thereto. Any use, reproduction, disclosure or 8 | # distribution of this software and related documentation without an express 9 | # license agreement from NVIDIA CORPORATION is strictly prohibited. 10 | # Simple action client interface to the gripper action server. 11 | 12 | import rospy 13 | from franka_control.srv import SetJointImpedance 14 | from franka_control.srv import SetJointImpedanceRequest 15 | from franka_control.srv import SetForceTorqueCollisionBehavior 16 | from franka_control.srv import SetForceTorqueCollisionBehaviorRequest 17 | 18 | rospy.init_node("set_control_parameters") 19 | force_torque_srv = "/franka_control/set_force_torque_collision_behavior" 20 | 21 | lower_torque_thresholds_nominal = [1000.0, 1000.0, 1000.0, 1000.0, 1000.0, 1000.0, 1000.0] 22 | upper_torque_thresholds_nominal = [1000.0, 1000.0, 1000.0, 1000.0, 1000.0, 1000.0, 1000.0] 23 | lower_force_thresholds_nominal = [1000.0, 1000.0, 1000.0, 1000.0, 1000.0, 1000.0] 24 | upper_force_thresholds_nominal = [1000.0, 1000.0, 1000.0, 1000.0, 1000.0, 1000.0] 25 | 26 | ft_req = SetForceTorqueCollisionBehaviorRequest() 27 | 28 | ft_req.lower_torque_thresholds_nominal = lower_torque_thresholds_nominal 29 | ft_req.upper_torque_thresholds_nominal = upper_torque_thresholds_nominal 30 | ft_req.lower_force_thresholds_nominal = lower_force_thresholds_nominal 31 | ft_req.upper_force_thresholds_nominal = upper_force_thresholds_nominal 32 | 33 | print(ft_req) 34 | 35 | rospy.loginfo("Waiting for services...") 36 | rospy.wait_for_service(force_torque_srv) 37 | rospy.loginfo("Services ready.") 38 | 39 | ft_srv = rospy.ServiceProxy(force_torque_srv, SetForceTorqueCollisionBehavior) 40 | 41 | resp = ft_srv(ft_req) 42 | failed = False 43 | if not resp.success: 44 | rospy.logerr("Could not set force torque collision behavior!") 45 | failed = True 46 | else: 47 | rospy.loginfo("Set force torque collision behavior!") 48 | 49 | if failed: 50 | raise RuntimeError("Failed to set control parameters") 51 | -------------------------------------------------------------------------------- /noetic_ws/src/isaac_moveit/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(isaac_moveit) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | rospy 6 | sensor_msgs 7 | panda_moveit_config 8 | ) 9 | 10 | catkin_package() 11 | 12 | include_directories( 13 | ${catkin_INCLUDE_DIRS} 14 | ) 15 | 16 | catkin_install_python(PROGRAMS 17 | scripts/panda_combined_joints_publisher.py 18 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 19 | ) 20 | -------------------------------------------------------------------------------- /noetic_ws/src/isaac_moveit/launch/franka_isaac_execution.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /noetic_ws/src/isaac_moveit/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | isaac_moveit 4 | 0.1.0 5 | The moveit samples package 6 | 7 | Isaac Sim 8 | 9 | Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. 10 | NVIDIA CORPORATION and its licensors retain all intellectual property 11 | and proprietary rights in and to this software, related documentation 12 | and any modifications thereto. Any use, reproduction, disclosure or 13 | distribution of this software and related documentation without an express 14 | license agreement from NVIDIA CORPORATION is strictly prohibited. 15 | 16 | https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html 17 | https://forums.developer.nvidia.com/c/omniverse/simulation/69 18 | 19 | catkin 20 | rospy 21 | sensor_msgs 22 | rospy 23 | sensor_msgs 24 | rospy 25 | sensor_msgs 26 | panda_moveit_config 27 | franka_description 28 | pilz_industrial_motion 29 | moveit_planners_chomp 30 | rviz_visual_tools 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /noetic_ws/src/isaac_moveit/scripts/panda_combined_joints_publisher.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2021-2022, NVIDIA CORPORATION. All rights reserved. 4 | # 5 | # NVIDIA CORPORATION and its licensors retain all intellectual property 6 | # and proprietary rights in and to this software, related documentation 7 | # and any modifications thereto. Any use, reproduction, disclosure or 8 | # distribution of this software and related documentation without an express 9 | # license agreement from NVIDIA CORPORATION is strictly prohibited. 10 | 11 | 12 | import rospy 13 | from sensor_msgs.msg import JointState 14 | 15 | joints_dict = {} 16 | 17 | 18 | def joint_states_callback(message): 19 | 20 | joint_commands = JointState() 21 | 22 | joint_commands.header = message.header 23 | 24 | for i, name in enumerate(message.name): 25 | 26 | # Storing arm joint names and positions 27 | joints_dict[name] = message.position[i] 28 | 29 | if name == "panda_finger_joint1": 30 | 31 | # Adding additional panda_finger_joint2 state info (extra joint used in isaac sim) 32 | # panda_finger_joint2 mirrors panda_finger_joint1 33 | joints_dict["panda_finger_joint2"] = message.position[i] 34 | 35 | joint_commands.name = joints_dict.keys() 36 | joint_commands.position = joints_dict.values() 37 | 38 | # Publishing combined message containing all arm and finger joints 39 | pub.publish(joint_commands) 40 | 41 | return 42 | 43 | 44 | if __name__ == "__main__": 45 | rospy.init_node("panda_combined_joints_publisher") 46 | pub = rospy.Publisher("/joint_command", JointState, queue_size=1) 47 | rospy.Subscriber("/joint_command_desired", JointState, joint_states_callback, queue_size=1) 48 | rospy.spin() 49 | -------------------------------------------------------------------------------- /noetic_ws/src/isaac_ros_messages/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(isaac_ros_messages) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | geometry_msgs 6 | roscpp 7 | rospy 8 | std_msgs 9 | message_generation 10 | ) 11 | 12 | add_service_files( 13 | FILES 14 | IsaacPose.srv 15 | ) 16 | 17 | generate_messages( 18 | DEPENDENCIES 19 | geometry_msgs 20 | std_msgs 21 | ) 22 | 23 | catkin_package( 24 | CATKIN_DEPENDS geometry_msgs roscpp rospy std_msgs message_runtime 25 | ) 26 | 27 | include_directories( 28 | ${catkin_INCLUDE_DIRS} 29 | ) 30 | -------------------------------------------------------------------------------- /noetic_ws/src/isaac_ros_messages/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | isaac_ros_messages 4 | 0.1.0 5 | The isaac_ros_messages package 6 | Isaac Sim 7 | 8 | Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. 9 | NVIDIA CORPORATION and its licensors retain all intellectual property 10 | and proprietary rights in and to this software, related documentation 11 | and any modifications thereto. Any use, reproduction, disclosure or 12 | distribution of this software and related documentation without an express 13 | license agreement from NVIDIA CORPORATION is strictly prohibited. 14 | 15 | https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html 16 | https://forums.developer.nvidia.com/c/omniverse/simulation/69 17 | 18 | catkin 19 | geometry_msgs 20 | roscpp 21 | rospy 22 | std_msgs 23 | geometry_msgs 24 | roscpp 25 | rospy 26 | std_msgs 27 | geometry_msgs 28 | roscpp 29 | rospy 30 | std_msgs 31 | message_generation 32 | message_runtime 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /noetic_ws/src/isaac_ros_messages/srv/IsaacPose.srv: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | string[] names 3 | geometry_msgs/Pose[] poses 4 | geometry_msgs/Twist[] velocities 5 | geometry_msgs/Vector3[] scales 6 | --- 7 | -------------------------------------------------------------------------------- /noetic_ws/src/isaac_tutorials/launch/apriltag_continuous_detection.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | [{'id': 0, 'size': 0.5, 'name': tag_0},{'id': 1, 'size': 0.5, 'name': tag_1},{'id': 2, 'size': 0.5, 'name': tag_2},] 10 | [] 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /noetic_ws/src/isaac_tutorials/msg/ContactSensor.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float64 time 3 | float64 value 4 | bool in_contact 5 | -------------------------------------------------------------------------------- /noetic_ws/src/isaac_tutorials/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | isaac_tutorials 4 | 0.1.0 5 | The isaac_tutorials package 6 | 7 | Isaac Sim 8 | 9 | Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. 10 | NVIDIA CORPORATION and its licensors retain all intellectual property 11 | and proprietary rights in and to this software, related documentation 12 | and any modifications thereto. Any use, reproduction, disclosure or 13 | distribution of this software and related documentation without an express 14 | license agreement from NVIDIA CORPORATION is strictly prohibited. 15 | 16 | https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html 17 | https://forums.developer.nvidia.com/c/omniverse/simulation/69 18 | 19 | message_generation 20 | message_runtime 21 | ackermann_msgs 22 | catkin 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /noetic_ws/src/isaac_tutorials/rviz/apriltag_config.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Camera1 10 | Splitter Ratio: 0.6198830604553223 11 | Tree Height: 525 12 | - Class: rviz/Tool Properties 13 | Expanded: 14 | - /2D Pose Estimate1 15 | - /2D Nav Goal1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.5886790156364441 19 | - Class: rviz/Time 20 | Experimental: false 21 | Name: Time 22 | SyncMode: 0 23 | SyncSource: Camera 24 | Preferences: 25 | PromptSaveOnExit: true 26 | Toolbars: 27 | toolButtonStyle: 2 28 | Visualization Manager: 29 | Class: "" 30 | Displays: 31 | - Class: rviz/Camera 32 | Enabled: true 33 | Image Rendering: background and overlay 34 | Image Topic: /tag_detections_image 35 | Name: Camera 36 | Overlay Alpha: 0.5 37 | Queue Size: 2 38 | Transport Hint: raw 39 | Unreliable: false 40 | Value: true 41 | Visibility: true 42 | Zoom Factor: 1 43 | Enabled: true 44 | Global Options: 45 | Background Color: 48; 48; 48 46 | Default Light: true 47 | Fixed Frame: sim_camera 48 | Frame Rate: 30 49 | Name: root 50 | Tools: 51 | - Class: rviz/Interact 52 | Hide Inactive Objects: true 53 | - Class: rviz/MoveCamera 54 | - Class: rviz/Select 55 | - Class: rviz/FocusCamera 56 | - Class: rviz/Measure 57 | - Class: rviz/SetInitialPose 58 | Theta std deviation: 0.2617993950843811 59 | Topic: /initialpose 60 | X std deviation: 0.5 61 | Y std deviation: 0.5 62 | - Class: rviz/SetGoal 63 | Topic: /move_base_simple/goal 64 | - Class: rviz/PublishPoint 65 | Single click: true 66 | Topic: /clicked_point 67 | Value: true 68 | Views: 69 | Current: 70 | Class: rviz/Orbit 71 | Distance: 9.993002891540527 72 | Enable Stereo Rendering: 73 | Stereo Eye Separation: 0.05999999865889549 74 | Stereo Focal Distance: 1 75 | Swap Stereo Eyes: false 76 | Value: false 77 | Focal Point: 78 | X: -0.032822735607624054 79 | Y: -0.10360819101333618 80 | Z: 0.054841917008161545 81 | Focal Shape Fixed Size: true 82 | Focal Shape Size: 0.05000000074505806 83 | Invert Z Axis: false 84 | Name: Current View 85 | Near Clip Distance: 0.009999999776482582 86 | Pitch: 0.8803979158401489 87 | Target Frame: 88 | Value: Orbit (rviz) 89 | Yaw: 0.3453984558582306 90 | Saved: ~ 91 | Window Geometry: 92 | Camera: 93 | collapsed: false 94 | Displays: 95 | collapsed: false 96 | Height: 846 97 | Hide Left Dock: true 98 | Hide Right Dock: false 99 | QMainWindow State: 000000ff00000000fd00000004000000000000015600000201fc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000000000000000fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000001e1fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000010c000001e10000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b2000000000000000000000002000004f300000298fc0100000003fb000000100044006900730070006c0061007900730100000000000001580000015600fffffffb0000000c00430061006d006500720061010000015e000003950000006900fffffffb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004f300000039fc0100000002fb0000000800540069006d00650100000000000004f3000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004f30000001700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 100 | Time: 101 | collapsed: false 102 | Tool Properties: 103 | collapsed: false 104 | Width: 1267 105 | X: 214 106 | Y: 474 107 | -------------------------------------------------------------------------------- /noetic_ws/src/isaac_tutorials/scripts/ros_ackermann_publisher.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2021-2024, NVIDIA CORPORATION. All rights reserved. 4 | # 5 | # NVIDIA CORPORATION and its licensors retain all intellectual property 6 | # and proprietary rights in and to this software, related documentation 7 | # and any modifications thereto. Any use, reproduction, disclosure or 8 | # distribution of this software and related documentation without an express 9 | # license agreement from NVIDIA CORPORATION is strictly prohibited. 10 | 11 | import rospy 12 | from ackermann_msgs.msg import AckermannDriveStamped 13 | import numpy as np 14 | 15 | rospy.init_node("test_rosbridge", anonymous=True) 16 | 17 | pub = rospy.Publisher("/ackermann_cmd", AckermannDriveStamped, queue_size=10) 18 | 19 | msg = AckermannDriveStamped() 20 | 21 | i = 0 22 | 23 | rate = rospy.Rate(20) 24 | while not rospy.is_shutdown(): 25 | 26 | # Only command forklift using acceleration and steering angle 27 | degrees1 = np.arange(0, 60) 28 | degrees2 = np.arange(-60, 0) 29 | degrees = np.concatenate((degrees1, degrees1[::-1], degrees2[::-1], degrees2)) 30 | msg.header.frame_id = "forklift" 31 | msg.header.stamp = rospy.Time.now() 32 | msg.drive.steering_angle = 0.0174533 * (degrees[i % degrees.size]) 33 | msg.drive.acceleration = float(degrees[i % degrees.size]) 34 | 35 | pub.publish(msg) 36 | 37 | i += 1 38 | rate.sleep() 39 | -------------------------------------------------------------------------------- /noetic_ws/src/isaac_tutorials/scripts/ros_publisher.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2021-2022, NVIDIA CORPORATION. All rights reserved. 4 | # 5 | # NVIDIA CORPORATION and its licensors retain all intellectual property 6 | # and proprietary rights in and to this software, related documentation 7 | # and any modifications thereto. Any use, reproduction, disclosure or 8 | # distribution of this software and related documentation without an express 9 | # license agreement from NVIDIA CORPORATION is strictly prohibited. 10 | 11 | import rospy 12 | from sensor_msgs.msg import JointState 13 | import numpy as np 14 | import time 15 | 16 | rospy.init_node("test_rosbridge", anonymous=True) 17 | 18 | pub = rospy.Publisher("/joint_command", JointState, queue_size=10) 19 | joint_state = JointState() 20 | 21 | 22 | joint_state.name = [ 23 | "panda_joint1", 24 | "panda_joint2", 25 | "panda_joint3", 26 | "panda_joint4", 27 | "panda_joint5", 28 | "panda_joint6", 29 | "panda_joint7", 30 | "panda_finger_joint1", 31 | "panda_finger_joint2", 32 | ] 33 | 34 | num_joints = len(joint_state.name) 35 | 36 | 37 | # make sure kit's editor is playing for receiving messages ## 38 | 39 | joint_state.position = np.array([0.0] * num_joints) 40 | default_joints = [0.0, -1.16, -0.0, -2.3, -0.0, 1.6, 1.1, 0.4, 0.4] 41 | 42 | # limiting the movements to a smaller range (this is not the range of the robot, just the range of the movement 43 | max_joints = np.array(default_joints) + 0.5 44 | min_joints = np.array(default_joints) - 0.5 45 | 46 | # position control the robot to wiggle around each joint 47 | time_start = time.time() 48 | rate = rospy.Rate(20) 49 | while not rospy.is_shutdown(): 50 | joint_state.position = np.sin(time.time() - time_start) * (max_joints - min_joints) * 0.5 + default_joints 51 | pub.publish(joint_state) 52 | rate.sleep() 53 | -------------------------------------------------------------------------------- /noetic_ws/src/isaac_tutorials/scripts/ros_service_client.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2021-2022, NVIDIA CORPORATION. All rights reserved. 4 | # 5 | # NVIDIA CORPORATION and its licensors retain all intellectual property 6 | # and proprietary rights in and to this software, related documentation 7 | # and any modifications thereto. Any use, reproduction, disclosure or 8 | # distribution of this software and related documentation without an express 9 | # license agreement from NVIDIA CORPORATION is strictly prohibited. 10 | 11 | 12 | import rospy 13 | import numpy as np 14 | from isaac_ros_messages.srv import IsaacPose 15 | from isaac_ros_messages.srv import IsaacPoseRequest 16 | from geometry_msgs.msg import Pose 17 | 18 | 19 | def teleport_client(msg): 20 | rospy.wait_for_service("teleport") 21 | try: 22 | teleport = rospy.ServiceProxy("teleport", IsaacPose) 23 | teleport(msg) 24 | return 25 | except rospy.ServiceException as e: 26 | print("Service call failed: %s" % e) 27 | 28 | 29 | # compose teleport messages 30 | cube_pose = Pose() 31 | cube_pose.position.x = np.random.uniform(-2, 2) 32 | cube_pose.position.y = 0 33 | cube_pose.position.z = 0 34 | cube_pose.orientation.w = 1 35 | cube_pose.orientation.x = 0 36 | cube_pose.orientation.y = 0 37 | cube_pose.orientation.z = 0 38 | 39 | cone_pose = Pose() 40 | cone_pose.position.x = 0 41 | cone_pose.position.y = np.random.uniform(-2, 2) 42 | cone_pose.position.z = 0 43 | cone_pose.orientation.w = 1 44 | cone_pose.orientation.x = 0 45 | cone_pose.orientation.y = 0 46 | cone_pose.orientation.z = 0 47 | 48 | teleport_msg = IsaacPoseRequest() 49 | teleport_msg.names = ["/World/Cube", "/World/Cone"] 50 | teleport_msg.poses = [cube_pose, cone_pose] 51 | 52 | teleport_client(teleport_msg) 53 | -------------------------------------------------------------------------------- /noetic_ws/src/navigation/carter_2dnav/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(carter_2dnav) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | move_base 6 | ) 7 | 8 | catkin_package() 9 | 10 | include_directories( 11 | ${catkin_INCLUDE_DIRS} 12 | ) 13 | 14 | -------------------------------------------------------------------------------- /noetic_ws/src/navigation/carter_2dnav/launch/amcl_robot_individual.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /noetic_ws/src/navigation/carter_2dnav/launch/carter_navigation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | target_frame: carter_lidar # Leave disabled to output scan in pointcloud frame 32 | transform_tolerance: 0.01 33 | min_height: -0.1 34 | max_height: 2 35 | 36 | angle_min: -1.5708 # -M_PI/2 37 | angle_max: 1.5708 # M_PI/2 38 | angle_increment: 0.0087 # M_PI/360.0 39 | scan_time: 0.3333 40 | range_min: 0.15 41 | range_max: 100.0 42 | use_inf: true 43 | inf_epsilon: 1.0 44 | 45 | # Concurrency level, affects number of pointclouds queued for processing and number of threads used 46 | # 0 : Detect number of cores 47 | # 1 : Single threaded 48 | # 2->inf : Parallelism level 49 | concurrency_level: 1 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /noetic_ws/src/navigation/carter_2dnav/launch/carter_slam_gmapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | target_frame: carter_lidar # Leave disabled to output scan in pointcloud frame 10 | transform_tolerance: 0.01 11 | min_height: -0.1 12 | max_height: 2 13 | 14 | angle_min: -1.5708 # -M_PI/2 15 | angle_max: 1.5708 # M_PI/2 16 | angle_increment: 0.0087 # M_PI/360.0 17 | scan_time: 0.3333 18 | range_min: 0.15 19 | range_max: 100.0 20 | use_inf: true 21 | inf_epsilon: 1.0 22 | 23 | # Concurrency level, affects number of pointclouds queued for processing and number of threads used 24 | # 0 : Detect number of cores 25 | # 1 : Single threaded 26 | # 2->inf : Parallelism level 27 | concurrency_level: 1 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /noetic_ws/src/navigation/carter_2dnav/launch/multiple_robot_carter_navigation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | -------------------------------------------------------------------------------- /noetic_ws/src/navigation/carter_2dnav/map/carter_hospital_navigation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isaac-sim/IsaacSim-ros_workspaces/435460aaeaa426e349fe885c60664b2bab5505bf/noetic_ws/src/navigation/carter_2dnav/map/carter_hospital_navigation.png -------------------------------------------------------------------------------- /noetic_ws/src/navigation/carter_2dnav/map/carter_hospital_navigation.yaml: -------------------------------------------------------------------------------- 1 | image: carter_hospital_navigation.png 2 | resolution: 0.05 3 | origin: [-49.625, -4.675, 0.0000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /noetic_ws/src/navigation/carter_2dnav/map/carter_office_navigation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isaac-sim/IsaacSim-ros_workspaces/435460aaeaa426e349fe885c60664b2bab5505bf/noetic_ws/src/navigation/carter_2dnav/map/carter_office_navigation.png -------------------------------------------------------------------------------- /noetic_ws/src/navigation/carter_2dnav/map/carter_office_navigation.yaml: -------------------------------------------------------------------------------- 1 | image: carter_office_navigation.png 2 | resolution: 0.05 3 | origin: [-29.975, -39.975, 0.0000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /noetic_ws/src/navigation/carter_2dnav/map/carter_warehouse_navigation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isaac-sim/IsaacSim-ros_workspaces/435460aaeaa426e349fe885c60664b2bab5505bf/noetic_ws/src/navigation/carter_2dnav/map/carter_warehouse_navigation.png -------------------------------------------------------------------------------- /noetic_ws/src/navigation/carter_2dnav/map/carter_warehouse_navigation.yaml: -------------------------------------------------------------------------------- 1 | image: carter_warehouse_navigation.png 2 | resolution: 0.05 3 | origin: [-11.975, -17.975, 0.0000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /noetic_ws/src/navigation/carter_2dnav/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | carter_2dnav 4 | 0.2.0 5 | The carter_2dnav package 6 | 7 | Isaac Sim 8 | 9 | Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. 10 | NVIDIA CORPORATION and its licensors retain all intellectual property 11 | and proprietary rights in and to this software, related documentation 12 | and any modifications thereto. Any use, reproduction, disclosure or 13 | distribution of this software and related documentation without an express 14 | license agreement from NVIDIA CORPORATION is strictly prohibited. 15 | 16 | https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html 17 | https://forums.developer.nvidia.com/c/omniverse/simulation/69 18 | 19 | catkin 20 | navigation 21 | pointcloud_to_laserscan 22 | 23 | navigation 24 | pointcloud_to_laserscan 25 | 26 | navigation 27 | pointcloud_to_laserscan 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /noetic_ws/src/navigation/carter_2dnav/params/base_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | TrajectoryPlannerROS: 2 | holonomic_robot: false 3 | max_vel_x: 1.2 4 | min_vel_x: 0.1 5 | max_vel_y: 0.0 6 | min_vel_y: 0.0 7 | max_vel_theta: 0.8 8 | min_vel_theta: -0.8 9 | min_in_place_vel_theta: 0.3 10 | acc_lim_theta: 3.2 11 | acc_lim_x: 2.5 12 | acc_lim_y: 0.0 13 | xy_goal_tolerance: 0.25 14 | yaw_goal_tolerance: 0.05 15 | occdist_scale: 0.7 16 | escape_vel: -0.1 17 | meter_scoring: true 18 | path_distance_bias: 0.8 19 | -------------------------------------------------------------------------------- /noetic_ws/src/navigation/carter_2dnav/params/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | obstacle_range: 25 2 | raytrace_range: 3 3 | robot_radius: 0.36 4 | cost_scaling_factor: 3.0 5 | observation_sources: laser_scan_sensor 6 | laser_scan_sensor: {sensor_frame: carter_lidar, data_type: LaserScan, topic: scan, marking: true, clearing: true} 7 | -------------------------------------------------------------------------------- /noetic_ws/src/navigation/carter_2dnav/params/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: map 3 | robot_base_frame: base_link 4 | update_frequency: 1.0 5 | publish_frequency: 0.5 6 | static_map: true 7 | transform_tolerance: 1.25 8 | inflation_radius: 0.85 9 | -------------------------------------------------------------------------------- /noetic_ws/src/navigation/carter_2dnav/params/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: odom 3 | robot_base_frame: base_link 4 | update_frequency: 5.0 5 | publish_frequency: 2.0 6 | static_map: false 7 | rolling_window: true 8 | width: 7.0 9 | height: 7.0 10 | resolution: 0.1 11 | transform_tolerance: 1.25 12 | inflation_radius: 0.32 13 | 14 | -------------------------------------------------------------------------------- /noetic_ws/src/navigation/carter_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(carter_description) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | geometry_msgs 6 | roscpp 7 | rviz 8 | tf 9 | urdf 10 | xacro 11 | ) 12 | 13 | catkin_package() 14 | 15 | include_directories( 16 | ${catkin_INCLUDE_DIRS} 17 | ) 18 | 19 | -------------------------------------------------------------------------------- /noetic_ws/src/navigation/carter_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | carter_description 4 | 0.1.0 5 | The carter_description package 6 | 7 | Isaac Sim 8 | 9 | Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. 10 | NVIDIA CORPORATION and its licensors retain all intellectual property 11 | and proprietary rights in and to this software, related documentation 12 | and any modifications thereto. Any use, reproduction, disclosure or 13 | distribution of this software and related documentation without an express 14 | license agreement from NVIDIA CORPORATION is strictly prohibited. 15 | 16 | https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html 17 | https://forums.developer.nvidia.com/c/omniverse/simulation/69 18 | 19 | catkin 20 | geometry_msgs 21 | roscpp 22 | rviz 23 | tf 24 | urdf 25 | xacro 26 | geometry_msgs 27 | roscpp 28 | rviz 29 | tf 30 | urdf 31 | xacro 32 | geometry_msgs 33 | roscpp 34 | rviz 35 | tf 36 | urdf 37 | xacro 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /noetic_ws/src/navigation/isaac_ros_navigation_goal/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(isaac_ros_navigation_goal) 3 | 4 | 5 | find_package(catkin REQUIRED COMPONENTS 6 | actionlib 7 | geometry_msgs 8 | move_base_msgs 9 | rospy 10 | sensor_msgs 11 | std_msgs 12 | ) 13 | 14 | 15 | catkin_python_setup() 16 | 17 | ################################### 18 | ## catkin specific configuration ## 19 | ################################### 20 | ## The catkin_package macro generates cmake config files for your package 21 | ## Declare things to be passed to dependent projects 22 | ## INCLUDE_DIRS: uncomment this if your package contains header files 23 | ## LIBRARIES: libraries you create in this project that dependent projects also need 24 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 25 | ## DEPENDS: system dependencies of this project that dependent projects also need 26 | catkin_package( 27 | # INCLUDE_DIRS include 28 | # LIBRARIES isaac_ros_navigation_goal 29 | # CATKIN_DEPENDS actionlib geometry_msgs move_base_msgs rospy sensor_msgs std_msgs 30 | # DEPENDS system_lib 31 | ) 32 | 33 | ########### 34 | ## Build ## 35 | ########### 36 | 37 | ## Specify additional locations of header files 38 | ## Your package locations should be listed before other locations 39 | include_directories( 40 | # include 41 | ${catkin_INCLUDE_DIRS} 42 | ) 43 | 44 | ############# 45 | ## Install ## 46 | ############# 47 | 48 | # all install targets should use catkin DESTINATION variables 49 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 50 | 51 | ## Mark executable scripts (Python etc.) for installation 52 | ## in contrast to setup.py, you can choose the destination 53 | catkin_install_python(PROGRAMS isaac_ros_navigation_goal/set_goal.py 54 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 55 | -------------------------------------------------------------------------------- /noetic_ws/src/navigation/isaac_ros_navigation_goal/assets/carter_warehouse_navigation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isaac-sim/IsaacSim-ros_workspaces/435460aaeaa426e349fe885c60664b2bab5505bf/noetic_ws/src/navigation/isaac_ros_navigation_goal/assets/carter_warehouse_navigation.png -------------------------------------------------------------------------------- /noetic_ws/src/navigation/isaac_ros_navigation_goal/assets/carter_warehouse_navigation.yaml: -------------------------------------------------------------------------------- 1 | image: carter_warehouse_navigation.png 2 | resolution: 0.05 3 | origin: [-11.975, -17.975, 0.0000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /noetic_ws/src/navigation/isaac_ros_navigation_goal/assets/goals.txt: -------------------------------------------------------------------------------- 1 | 1 2 0 0 1 0 2 | 2 3 0 0 1 1 3 | 3.4 4.5 0 0 0.5 0.5 -------------------------------------------------------------------------------- /noetic_ws/src/navigation/isaac_ros_navigation_goal/isaac_ros_navigation_goal/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isaac-sim/IsaacSim-ros_workspaces/435460aaeaa426e349fe885c60664b2bab5505bf/noetic_ws/src/navigation/isaac_ros_navigation_goal/isaac_ros_navigation_goal/__init__.py -------------------------------------------------------------------------------- /noetic_ws/src/navigation/isaac_ros_navigation_goal/isaac_ros_navigation_goal/goal_generators/__init__.py: -------------------------------------------------------------------------------- 1 | from .random_goal_generator import RandomGoalGenerator 2 | from .goal_reader import GoalReader 3 | -------------------------------------------------------------------------------- /noetic_ws/src/navigation/isaac_ros_navigation_goal/isaac_ros_navigation_goal/goal_generators/goal_generator.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | from abc import ABCMeta, abstractmethod 3 | 4 | 5 | class GoalGenerator: 6 | __metaclass__ = ABCMeta 7 | """ 8 | Parent class for the Goal generators 9 | """ 10 | 11 | def __init__(self): 12 | pass 13 | 14 | @abstractmethod 15 | def generate_goal(self, max_num_of_trials=2000): 16 | """ 17 | Generate the goal. 18 | 19 | Parameters 20 | ---------- 21 | max_num_of_trials: maximum number of pose generations when generated pose keep is not a valid pose. 22 | 23 | Returns 24 | ------- 25 | [List][Pose]: Pose in format [pose.x,pose.y,orientaion.x,orientaion.y,orientaion.z,orientaion.w] 26 | """ 27 | pass 28 | -------------------------------------------------------------------------------- /noetic_ws/src/navigation/isaac_ros_navigation_goal/isaac_ros_navigation_goal/goal_generators/goal_reader.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | from .goal_generator import GoalGenerator 3 | 4 | 5 | class GoalReader(GoalGenerator): 6 | def __init__(self, file_path): 7 | self.__file_path = file_path 8 | self.__generator = self.__get_goal() 9 | 10 | def generate_goal(self, max_num_of_trials=1000): 11 | try: 12 | return next(self.__generator) 13 | except StopIteration: 14 | return 15 | 16 | def __get_goal(self): 17 | for row in open(self.__file_path, "r"): 18 | yield list(map(float, row.strip().split(" "))) 19 | -------------------------------------------------------------------------------- /noetic_ws/src/navigation/isaac_ros_navigation_goal/isaac_ros_navigation_goal/goal_generators/random_goal_generator.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | import numpy as np 3 | from .goal_generator import GoalGenerator 4 | 5 | 6 | class RandomGoalGenerator(GoalGenerator): 7 | """ 8 | Random goal generator. 9 | 10 | parameters 11 | ---------- 12 | grid_map: GridMap Object 13 | distance: distance in meters to check vicinity for obstacles. 14 | """ 15 | 16 | def __init__(self, grid_map, distance): 17 | self.__grid_map = grid_map 18 | self.__distance = distance 19 | 20 | def generate_goal(self, max_num_of_trials=1000): 21 | """ 22 | Generate the goal. 23 | 24 | Parameters 25 | ---------- 26 | max_num_of_trials: maximum number of pose generations when generated pose keep is not a valid pose. 27 | 28 | Returns 29 | ------- 30 | [List][Pose]: Pose in format [pose.x,pose.y,orientaion.x,orientaion.y,orientaion.z,orientaion.w] 31 | """ 32 | range_ = self.__grid_map.get_range() 33 | trial_count = 0 34 | while trial_count < max_num_of_trials: 35 | x = np.random.uniform(range_[0][0], range_[0][1]) 36 | y = np.random.uniform(range_[1][0], range_[1][1]) 37 | orient_x = 0 # not needed because robot is in x,y plane 38 | orient_y = 0 # not needed because robot is in x,y plane 39 | orient_z = np.random.uniform(0, 1) 40 | orient_w = np.random.uniform(0, 1) 41 | if self.__grid_map.is_valid_pose([x, y], self.__distance): 42 | goal = [x, y, orient_x, orient_y, orient_z, orient_w] 43 | return goal 44 | trial_count += 1 45 | -------------------------------------------------------------------------------- /noetic_ws/src/navigation/isaac_ros_navigation_goal/launch/isaac_ros_navigation_goal.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | [-6.0, -1.0, 0, 0, 0, 1.0, 0] 9 | 10 | -------------------------------------------------------------------------------- /noetic_ws/src/navigation/isaac_ros_navigation_goal/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | isaac_ros_navigation_goal 4 | 0.1.0 5 | Package to set goals for navigation stack. 6 | 7 | Isaac Sim 8 | 9 | Copyright (c) 2024, NVIDIA CORPORATION. All rights reserved. 10 | NVIDIA CORPORATION and its licensors retain all intellectual property 11 | and proprietary rights in and to this software, related documentation 12 | and any modifications thereto. Any use, reproduction, disclosure or 13 | distribution of this software and related documentation without an express 14 | license agreement from NVIDIA CORPORATION is strictly prohibited. 15 | 16 | 17 | https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html 18 | https://forums.developer.nvidia.com/c/omniverse/simulation/69 19 | https://developer.nvidia.com/isaac-ros-gems/ 20 | 21 | catkin 22 | actionlib 23 | geometry_msgs 24 | move_base_msgs 25 | rospy 26 | sensor_msgs 27 | std_msgs 28 | actionlib 29 | geometry_msgs 30 | move_base_msgs 31 | rospy 32 | sensor_msgs 33 | std_msgs 34 | actionlib 35 | geometry_msgs 36 | move_base_msgs 37 | rospy 38 | sensor_msgs 39 | std_msgs 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /noetic_ws/src/navigation/isaac_ros_navigation_goal/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | from catkin_pkg.python_setup import generate_distutils_setup 3 | 4 | d = generate_distutils_setup( 5 | packages=["goal_generators", "obstacle_map"], package_dir={"": "isaac_ros_navigation_goal"} 6 | ) 7 | setup(**d) 8 | -------------------------------------------------------------------------------- /ubuntu_20_humble_minimal.dockerfile: -------------------------------------------------------------------------------- 1 | ARG BASE_IMAGE=ubuntu:20.04 2 | FROM ${BASE_IMAGE} 3 | 4 | ENV ROS_DISTRO=humble 5 | ENV ROS_ROOT=humble_ws 6 | ENV ROS_PYTHON_VERSION=3 7 | 8 | ENV DEBIAN_FRONTEND=noninteractive 9 | 10 | WORKDIR /workspace 11 | 12 | RUN apt-get update && \ 13 | apt-get install -y --no-install-recommends \ 14 | git \ 15 | cmake \ 16 | build-essential \ 17 | curl \ 18 | wget \ 19 | gnupg2 \ 20 | lsb-release 21 | 22 | 23 | # Upgrade installed packages 24 | RUN apt update && apt upgrade -y && apt clean 25 | 26 | # Install Python3.10 27 | RUN apt update && \ 28 | apt install --no-install-recommends -y build-essential software-properties-common && \ 29 | add-apt-repository -y ppa:deadsnakes/ppa && \ 30 | apt install --no-install-recommends -y python3.10 python3.10-dev python3.10-distutils 31 | 32 | # Setting up locale stuff 33 | RUN apt update && apt install locales 34 | 35 | RUN locale-gen en_US en_US.UTF-8 && \ 36 | update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 && \ 37 | export LANG=en_US.UTF-8 38 | 39 | # Set default Python3 to Python3.10 40 | RUN update-alternatives --install /usr/bin/python3 python3 /usr/bin/python3.8 1 41 | RUN update-alternatives --install /usr/bin/python3 python3 /usr/bin/python3.10 2 42 | 43 | # Pip install stuff 44 | RUN curl -s https://bootstrap.pypa.io/get-pip.py -o get-pip.py && \ 45 | python3.10 get-pip.py --force-reinstall && \ 46 | rm get-pip.py 47 | 48 | RUN wget --no-check-certificate https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc && apt-key add ros.asc 49 | RUN sh -c 'echo "deb [arch=$(dpkg --print-architecture)] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" > /etc/apt/sources.list.d/ros2-latest.list' 50 | 51 | RUN pip3 install setuptools==70.0.0 52 | 53 | RUN apt update && apt install -y \ 54 | python3-pip \ 55 | python3-pytest-cov \ 56 | python3-rosinstall-generator \ 57 | ros-dev-tools \ 58 | libbullet-dev \ 59 | libasio-dev \ 60 | libtinyxml2-dev \ 61 | libcunit1-dev \ 62 | libacl1-dev 63 | 64 | RUN python3 -m pip install -U \ 65 | argcomplete \ 66 | flake8-blind-except \ 67 | flake8-builtins \ 68 | flake8-class-newline \ 69 | flake8-comprehensions \ 70 | flake8-deprecated \ 71 | flake8-docstrings \ 72 | flake8-import-order \ 73 | flake8-quotes \ 74 | pytest-repeat \ 75 | pytest-rerunfailures \ 76 | pytest \ 77 | lark 78 | 79 | RUN python3.10 -m pip uninstall numpy -y 80 | RUN python3.10 -m pip install --upgrade pip 81 | RUN python3.10 -m pip install numpy 82 | 83 | 84 | RUN mkdir -p ${ROS_ROOT}/src && \ 85 | cd ${ROS_ROOT} && \ 86 | rosinstall_generator --deps --rosdistro ${ROS_DISTRO} rosidl_runtime_c rcutils rcl rmw tf2 tf2_msgs geometry_msgs nav_msgs std_msgs rosgraph_msgs sensor_msgs vision_msgs rclpy ros2topic ros2pkg ros2doctor ros2run ros2node ros_environment ackermann_msgs example_interfaces > ros2.${ROS_DISTRO}.${ROS_PKG}.rosinstall && \ 87 | cat ros2.${ROS_DISTRO}.${ROS_PKG}.rosinstall && \ 88 | vcs import src < ros2.${ROS_DISTRO}.${ROS_PKG}.rosinstall 89 | 90 | 91 | RUN rosdep init && rosdep update 92 | 93 | # Build ROS workspace set python3 flag for some reason still default remains python3.8 when colcon building 94 | RUN cd ${ROS_ROOT} && colcon build --cmake-args ' -DPython3_EXECUTABLE=/usr/bin/python3.10' --merge-install 95 | 96 | # Need these to maintain compatibility on non 20.04 systems 97 | RUN cp /usr/lib/x86_64-linux-gnu/libtinyxml2.so* /workspace/humble_ws/install/lib/ 98 | RUN cp /usr/lib/x86_64-linux-gnu/libssl.so* /workspace/humble_ws/install/lib/ 99 | RUN cp /usr/lib/x86_64-linux-gnu/libcrypto.so* /workspace/humble_ws/install/lib/ 100 | 101 | # Next, build the additional workspace 102 | RUN cd ${WORKDIR} && mkdir build_ws 103 | COPY humble_ws build_ws 104 | 105 | RUN /bin/bash -c "source ${ROS_ROOT}/install/setup.sh && cd build_ws && colcon build --cmake-args ' -DPython3_EXECUTABLE=/usr/bin/python3.10' --merge-install" --------------------------------------------------------------------------------