├── .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"
--------------------------------------------------------------------------------