Port to Humble

LICENSE.txt

@@ -0,0 +1,25 @@
+Copyright (c) 2024 TOYOTA MOTOR CORPORATION
+All rights reserved.
+Redistribution and use in source and binary forms, with or without
+modification, are permitted (subject to the limitations in the disclaimer
+below) provided that the following conditions are met:
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+* Neither the name of the copyright holder nor the names of its contributors may be used
+  to endorse or promote products derived from this software without specific
+  prior written permission.
+NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
+GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
+OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
+DAMAGE.

tmc_collision_detecting_validator/CHANGELOG.rst

@@ -0,0 +1,9 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package tmc_collision_detecting_validator
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+2.0.0 (2024-10-15)
+-------------------
+* Initial release
+* Contributors: Keisuke Takeshita, Satoru Onoda
+

tmc_collision_detecting_validator/CMakeLists.txt

@@ -0,0 +1,64 @@
+cmake_minimum_required(VERSION 3.5)
+project(tmc_collision_detecting_validator)
+
+find_package(ament_cmake REQUIRED)
+find_package(moveit_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(tf2_eigen REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tmc_manipulation_types REQUIRED)
+find_package(tmc_manipulation_types_bridge REQUIRED)
+find_package(tmc_planning_msgs REQUIRED)
+find_package(tmc_robot_collision_detector REQUIRED)
+find_package(tmc_robot_local_planner REQUIRED)
+find_package(tmc_utils REQUIRED)
+
+find_package(Eigen3 REQUIRED)
+
+add_library(${PROJECT_NAME} SHARED
+  src/${PROJECT_NAME}/collision_detecting_validator.cpp
+  src/${PROJECT_NAME}/map_collision_checker.cpp
+  src/${PROJECT_NAME}/robot_collision_checker.cpp
+  src/${PROJECT_NAME}/utils.cpp
+)
+target_include_directories(${PROJECT_NAME} PUBLIC ${EIGEN3_INCLUDE_DIRS})
+ament_target_dependencies(${PROJECT_NAME} moveit_msgs nav_msgs pluginlib rclcpp rclcpp_action tf2_eigen tf2_ros tmc_manipulation_types tmc_manipulation_types_bridge tmc_planning_msgs tmc_robot_collision_detector tmc_robot_local_planner tmc_utils)
+
+add_executable(collision_detecting_validator src/${PROJECT_NAME}/node_main.cpp)
+target_link_libraries(collision_detecting_validator ${PROJECT_NAME})
+ament_target_dependencies(collision_detecting_validator rclcpp)
+
+add_executable(validate_offline src/${PROJECT_NAME}/validate_offline.cpp)
+target_include_directories(validate_offline PUBLIC ${EIGEN3_INCLUDE_DIRS})
+target_link_libraries(validate_offline ${PROJECT_NAME})
+ament_target_dependencies(validate_offline rclcpp tmc_manipulation_types_bridge tmc_planning_msgs tmc_robot_collision_detector tmc_utils)
+
+pluginlib_export_plugin_description_file(tmc_robot_local_planner validator_plugins.xml)
+
+install(TARGETS ${PROJECT_NAME} collision_detecting_validator validate_offline
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+install(
+  DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(tmc_manipulation_tests REQUIRED)
+
+  ament_add_gtest(collision_detecting_validator_test test/collision_detecting_validator-test.cpp)
+  target_link_libraries(collision_detecting_validator_test ${PROJECT_NAME})
+  ament_target_dependencies(collision_detecting_validator_test tmc_manipulation_tests)
+endif()
+
+ament_export_include_directories()
+ament_export_libraries(${PROJECT_NAME})
+ament_export_dependencies(moveit_msgs nav_msgs rclcpp rclcpp_action  tf2_eigen tf2_ros tmc_manipulation_types tmc_manipulation_types_bridge tmc_planning_msgs tmc_robot_collision_detector tmc_robot_local_planner tmc_utils)
+
+ament_package()

tmc_collision_detecting_validator/launch/display.launch.py

@@ -0,0 +1,70 @@
+#!/usr/bin/env python3
+'''
+Copyright (c) 2024 TOYOTA MOTOR CORPORATION
+All rights reserved.
+Redistribution and use in source and binary forms, with or without
+modification, are permitted (subject to the limitations in the disclaimer
+below) provided that the following conditions are met:
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+* Neither the name of the copyright holder nor the names of its contributors may be used
+  to endorse or promote products derived from this software without specific
+  prior written permission.
+NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
+GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
+OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
+DAMAGE.
+'''
+from launch import LaunchDescription
+
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import PathJoinSubstitution
+
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+from tmc_launch_ros_utils.tmc_launch_ros_utils import load_robot_description
+
+
+def declare_arguments():
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument('description_package',
+                              default_value='hsre_description',
+                              description='Description package with robot URDF/xacro files.'))
+    declared_arguments.append(
+        DeclareLaunchArgument('description_file',
+                              default_value='hsre1_1p_whole_body.urdf.xacro',
+                              description='URDF/XACRO description file with the robot base.'))
+    return declared_arguments
+
+
+def generate_launch_description():
+    robot_description = load_robot_description()
+
+    joint_state_publisher = Node(package='joint_state_publisher',
+                                 executable='joint_state_publisher',
+                                 parameters=[{'source_list': ['debug_joint_state']}])
+    robot_state_publisher = Node(package='robot_state_publisher',
+                                 executable='robot_state_publisher',
+                                 parameters=[robot_description])
+
+    rviz_config_file = PathJoinSubstitution(
+        [FindPackageShare('tmc_collision_detecting_validator'), 'launch', 'display.rviz'])
+    rviz_node = Node(package='rviz2',
+                     executable='rviz2',
+                     arguments=['-d', rviz_config_file])
+
+    nodes = [joint_state_publisher, robot_state_publisher, rviz_node]
+    return LaunchDescription(declare_arguments() + nodes)