diff --git a/LICENSE.txt b/LICENSE.txt new file mode 100644 index 0000000..2ec1593 --- /dev/null +++ b/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. \ No newline at end of file diff --git a/tmc_collision_detecting_validator/CHANGELOG.rst b/tmc_collision_detecting_validator/CHANGELOG.rst new file mode 100644 index 0000000..6b57e9f --- /dev/null +++ b/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 + diff --git a/tmc_collision_detecting_validator/CMakeLists.txt b/tmc_collision_detecting_validator/CMakeLists.txt new file mode 100644 index 0000000..fa45b98 --- /dev/null +++ b/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() diff --git a/tmc_collision_detecting_validator/launch/display.launch.py b/tmc_collision_detecting_validator/launch/display.launch.py new file mode 100644 index 0000000..327ad4b --- /dev/null +++ b/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) diff --git a/tmc_collision_detecting_validator/launch/display.rviz b/tmc_collision_detecting_validator/launch/display.rviz new file mode 100644 index 0000000..b2c00b5 --- /dev/null +++ b/tmc_collision_detecting_validator/launch/display.rviz @@ -0,0 +1,392 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 87 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + Splitter Ratio: 0.5 + Tree Height: 655 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_accurate_imu_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + base_b_bumper_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_f_bumper_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + base_imu_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + base_l_drive_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_l_passive_wheel_x_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + base_l_passive_wheel_y_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + base_l_passive_wheel_z_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_r_drive_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_r_passive_wheel_x_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + base_r_passive_wheel_y_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + base_r_passive_wheel_z_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_range_sensor_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_rear_range_sensor_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + d435_gazebo_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + d435_gazebo_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + d435_gazebo_left_ir_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + d435_gazebo_right_ir_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + d435_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + elbow_pitch_link: + Alpha: 1 + Show Axes: false + Show Trail: false + elbow_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_virtual_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + hand_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + head_l_stereo_camera_gazebo_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_l_stereo_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_r_stereo_camera_gazebo_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_r_stereo_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_rgbd_sensor_gazebo_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_rgbd_sensor_gazebo_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_rgbd_sensor_gazebo_left_ir_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_rgbd_sensor_gazebo_right_ir_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_rgbd_sensor_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rh_p12_rn_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rh_p12_rn_l1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rh_p12_rn_l2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rh_p12_rn_r1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rh_p12_rn_r2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulder_pitch_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulder_yaw_link: + Alpha: 1 + Show Axes: false + Show Trail: false + tlibar_root_link: + Alpha: 1 + Show Axes: false + Show Trail: false + torso_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_pitch_link: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_yaw_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 1 + Name: TF + Show Arrows: false + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /debug_environment + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: odom + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 2.88112211227417 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.296915203332901 + Y: 0.4020375609397888 + Z: 0.5328660607337952 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.3253994286060333 + Target Frame: + Value: Orbit (rviz) + Yaw: 1.592233657836914 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 893 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000323fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000323000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000011000000323fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000323000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003e50000032300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1643 + X: 667 + Y: 181 diff --git a/tmc_collision_detecting_validator/launch/validate_offline.launch.py b/tmc_collision_detecting_validator/launch/validate_offline.launch.py new file mode 100644 index 0000000..81e7f35 --- /dev/null +++ b/tmc_collision_detecting_validator/launch/validate_offline.launch.py @@ -0,0 +1,74 @@ +#!/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 LaunchConfiguration + +from launch_ros.actions import Node + +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='hsre3p_whole_body.urdf.xacro', + description='URDF/XACRO description file with the robot base.')) + declared_arguments.append( + DeclareLaunchArgument('collision_config_file', + default_value='collision_pair_hsre3p.xml', + description='Robot collision config xml file.')) + declared_arguments.append( + DeclareLaunchArgument('validation_log_prefix', + description='File prefix of validation debug target.')) + return declared_arguments + + +def load_robot_collision_config(): + config = load_robot_description(description_file_arg_name='collision_config_file') + return {'robot_collision_pair': config['robot_description']} + + +def generate_launch_description(): + robot_description = load_robot_description() + robot_collision_config = load_robot_collision_config() + + validation_offline = Node(package='tmc_collision_detecting_validator', + executable='validate_offline', + parameters=[robot_description, robot_collision_config], + # arguments=['~/.ros/log/validation_20230628T173914_778619166']) + arguments=[LaunchConfiguration('validation_log_prefix')]) + + return LaunchDescription(declare_arguments() + [validation_offline]) diff --git a/tmc_collision_detecting_validator/package.xml b/tmc_collision_detecting_validator/package.xml new file mode 100644 index 0000000..d7c02f9 --- /dev/null +++ b/tmc_collision_detecting_validator/package.xml @@ -0,0 +1,39 @@ + + tmc_collision_detecting_validator + 2.0.0 + The tmc_collision_detecting_validator package + + HSR Support + + Keisuke Takeshita + Satoru Onoda + + BSD 3-clause Clear License + + ament_cmake + + eigen + + 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 + + tmc_launch_ros_utils + + ament_cmake_gtest + tmc_manipulation_tests + + + ament_cmake + + \ No newline at end of file diff --git a/tmc_collision_detecting_validator/src/tmc_collision_detecting_validator/collision_detecting_validator.cpp b/tmc_collision_detecting_validator/src/tmc_collision_detecting_validator/collision_detecting_validator.cpp new file mode 100644 index 0000000..6ea5511 --- /dev/null +++ b/tmc_collision_detecting_validator/src/tmc_collision_detecting_validator/collision_detecting_validator.cpp @@ -0,0 +1,224 @@ +/* +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. +*/ +/// @brief Collision Detecting Validator Class + +#include "collision_detecting_validator.hpp" + +#include + +#include +#include +#include + +namespace { +const char* const kOriginFrameId = "odom"; + +} // namespace + +namespace tmc_collision_detecting_validator { + +CollisionDetectingValidatorPlugin::CollisionDetectingValidatorPlugin() {} + +void CollisionDetectingValidatorPlugin::Initialize(const rclcpp::Node::SharedPtr& node) { + node_ = node; + + tf_buffer_ = std::make_unique(node_->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + validate_timeout_ = std::make_shared>(node, "validate_timeout", 0.15); + set_param_handlers_.emplace_back(node->add_on_set_parameters_callback( + std::bind(&tmc_utils::DynamicParameter::SetParameterCallback, validate_timeout_, std::placeholders::_1))); + + save_request_ = std::make_shared>(node, "save_request", true); + set_param_handlers_.emplace_back(node->add_on_set_parameters_callback( + std::bind(&tmc_utils::DynamicParameter::SetParameterCallback, save_request_, std::placeholders::_1))); + + print_debug_info_ = std::make_shared>(node, "print_debug_info", true); + set_param_handlers_.emplace_back(node->add_on_set_parameters_callback( + std::bind(&tmc_utils::DynamicParameter::SetParameterCallback, print_debug_info_, std::placeholders::_1))); + + logger_ = std::make_shared(tmc_utils::GetLogDirectory() + "/validation_"); + + robot_collision_checker_ = std::make_shared(node, print_debug_info_); + + const auto obstacle_map_topic_names = tmc_utils::GetParameter>( + node, "obstacle_map_topic_names", {}); + for (const auto& name : obstacle_map_topic_names) { + map_collision_checkers_.emplace_back(std::make_shared(node, name, print_debug_info_)); + } +} + +bool CollisionDetectingValidatorPlugin::Validate( + const std::vector& trajectories_in, + std::function interrupt, + tmc_manipulation_types::TimedRobotTrajectory& trajectory_out) { + auto end_stamp = node_->get_clock()->now() + rclcpp::Duration::from_seconds(validate_timeout_->value()); + auto interrupt_func = [this, end_stamp, interrupt]() { + return (this->node_->get_clock()->now() > end_stamp) || interrupt(); + }; + if (save_request_->value()) { + logger_->UpdateStamp(node_->get_clock()->now()); + tmc_planning_msgs::action::ValidateRobotTrajectories::Goal goal; + goal.robot_trajectories.reserve(trajectories_in.size()); + for (const auto trajectory : trajectories_in) { + moveit_msgs::msg::RobotTrajectory trajectory_msg; + tmc_manipulation_types_bridge::TimedRobotTrajectoryToRobotTrajectoryMsg(trajectory, trajectory_msg); + goal.robot_trajectories.emplace_back(trajectory_msg); + } + logger_->SaveMessage(".goal", goal); + } + + std::vector> map_collision_checkers; + for (const auto& checker : map_collision_checkers_) { + if (UpdateMapToRobotOrigin(checker)) { + map_collision_checkers.push_back(checker); + } + } + + robot_collision_checker_->DestroyAllObstacleObject(); + robot_collision_checker_->UpdateEnvironmentObstacleObject(); + if (save_request_->value()) { + robot_collision_checker_->DumpEnvironment(logger_); + } + + for (auto i = 0u; i < trajectories_in.size(); ++i) { + if (interrupt_func()) { + DumpCount(i); + return false; + } + bool is_feasible = true; + for (const auto& checker : map_collision_checkers) { + if (!checker->IsFeasible(trajectories_in[i], interrupt_func)) { + is_feasible = false; + break; + } + } + if (!is_feasible) { + continue; + } + if (robot_collision_checker_->IsFeasible(trajectories_in[i], interrupt_func)) { + trajectory_out = trajectories_in[i]; + DumpCount(i); + return true; + } + } + DumpCount(trajectories_in.size() - 1); + return false; +} + +void CollisionDetectingValidatorPlugin::DumpCount(uint32_t count) { + if (save_request_->value()) { + std_msgs::msg::UInt32 count_msg; + count_msg.data = count; + logger_->SaveMessage(".count", count_msg); + } +} + +bool CollisionDetectingValidatorPlugin::UpdateMapToRobotOrigin( + const std::shared_ptr& map_collision_checker) { + const auto map_frame = map_collision_checker->GetMapFrame(); + if (map_frame.empty()) { + return false; + } + Eigen::Affine3d map_to_odom; + try { + // Strictly speaking, I think it's better to convert it at the time of MAP + const auto transform = tf_buffer_->lookupTransform(map_frame, kOriginFrameId, tf2::TimePointZero); + map_to_odom = tf2::transformToEigen(transform); + } catch (const tf2::TransformException& e) { + RCLCPP_WARN(node_->get_logger(), "Could not transform %s to %s: %s", map_frame.c_str(), kOriginFrameId, e.what()); + return false; + } + map_collision_checker->set_map_to_origin(map_to_odom); + return true; +} + + +CollisionDetectingValidator::CollisionDetectingValidator() : CollisionDetectingValidator(rclcpp::NodeOptions()) {} + +CollisionDetectingValidator::CollisionDetectingValidator(const rclcpp::NodeOptions& options) + : Node("validator", options) {} + +void CollisionDetectingValidator::Init() { + auto node = shared_from_this(); + impl_.Initialize(node); + + server_ = rclcpp_action::create_server( + node, "~/validate", + std::bind(&CollisionDetectingValidator::GoalCallback, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&CollisionDetectingValidator::CancelCallback, this, std::placeholders::_1), + std::bind(&CollisionDetectingValidator::FeedbackSetupCallback, this, std::placeholders::_1)); +} + +rclcpp_action::GoalResponse CollisionDetectingValidator::GoalCallback( + const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal) { + if (goal->robot_trajectories.empty()) { + return rclcpp_action::GoalResponse::REJECT; + } else { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } +} + +rclcpp_action::CancelResponse CollisionDetectingValidator::CancelCallback(const ServerGoalHandlePtr goal_handle) { + return rclcpp_action::CancelResponse::ACCEPT; +} + +void CollisionDetectingValidator::FeedbackSetupCallback(ServerGoalHandlePtr goal_handle) { + const auto goal = goal_handle->get_goal(); + + std::vector trajectories_in; + trajectories_in.reserve(goal->robot_trajectories.size()); + for (const auto& trajectory_msg : goal->robot_trajectories) { + tmc_manipulation_types::TimedRobotTrajectory trajectory; + tmc_manipulation_types_bridge::RobotTrajectoryMsgToTimedRobotTrajectory(trajectory_msg, trajectory); + trajectories_in.emplace_back(trajectory); + } + + tmc_manipulation_types::TimedRobotTrajectory trajectory_out; + auto interrupt_func = [goal_handle]() { return goal_handle->is_canceling(); }; + const auto val_result = impl_.Validate(trajectories_in, interrupt_func, trajectory_out); + + auto action_result = std::make_shared(); + if (val_result) { + tmc_manipulation_types_bridge::TimedRobotTrajectoryToRobotTrajectoryMsg( + trajectory_out, action_result->robot_trajectory); + goal_handle->succeed(action_result); + } else if (goal_handle->is_canceling()) { + goal_handle->canceled(action_result); + } else { + goal_handle->abort(action_result); + } +} + +} // namespace tmc_collision_detecting_validator + +#include + +PLUGINLIB_EXPORT_CLASS(tmc_collision_detecting_validator::CollisionDetectingValidatorPlugin, + tmc_robot_local_planner::IValidator) diff --git a/tmc_collision_detecting_validator/src/tmc_collision_detecting_validator/collision_detecting_validator.hpp b/tmc_collision_detecting_validator/src/tmc_collision_detecting_validator/collision_detecting_validator.hpp new file mode 100644 index 0000000..7dadbdd --- /dev/null +++ b/tmc_collision_detecting_validator/src/tmc_collision_detecting_validator/collision_detecting_validator.hpp @@ -0,0 +1,112 @@ +/* +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. +*/ +/// @brief Collision Detecting Validator Class +#ifndef TMC_COLLISION_DETECTING_VALIDATOR_COLLISION_DETECTING_VALIDATOR_HPP_ +#define TMC_COLLISION_DETECTING_VALIDATOR_COLLISION_DETECTING_VALIDATOR_HPP_ + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "map_collision_checker.hpp" +#include "robot_collision_checker.hpp" + +namespace tmc_collision_detecting_validator { + +class CollisionDetectingValidatorPlugin : public tmc_robot_local_planner::IValidator { + public: + CollisionDetectingValidatorPlugin(); + virtual ~CollisionDetectingValidatorPlugin() = default; + + void Initialize(const rclcpp::Node::SharedPtr& node) override; + + bool Validate(const std::vector& trajectories_in, + std::function interrupt, + tmc_manipulation_types::TimedRobotTrajectory& trajectory_out) override; + + private: + rclcpp::Node::SharedPtr node_; + + // Interference check + std::shared_ptr robot_collision_checker_; + std::vector> map_collision_checkers_; + + // Interference check timeout + rclcpp::Clock::SharedPtr clock_; + tmc_utils::DynamicParameter::Ptr validate_timeout_; + std::vector set_param_handlers_; + + // Logger for Debug + tmc_utils::DynamicParameter::Ptr save_request_; + tmc_utils::DynamicParameter::Ptr print_debug_info_; + tmc_utils::MessageLogger::Ptr logger_; + + void DumpCount(uint32_t count); + + // MAP-> ODOM update function + bool UpdateMapToRobotOrigin(const std::shared_ptr& map_collision_checker); + std::unique_ptr tf_buffer_; + std::shared_ptr tf_listener_; +}; + +class CollisionDetectingValidator : public rclcpp::Node { + public: + /// @brief constructor + CollisionDetectingValidator(); + explicit CollisionDetectingValidator(const rclcpp::NodeOptions& options); + + void Init(); + + private: + using ServerGoalHandle = rclcpp_action::ServerGoalHandle; + using ServerGoalHandlePtr = std::shared_ptr; + + rclcpp_action::GoalResponse GoalCallback( + const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal); + rclcpp_action::CancelResponse CancelCallback(const ServerGoalHandlePtr goal_handle); + void FeedbackSetupCallback(ServerGoalHandlePtr goal_handle); + + // Action server + rclcpp_action::Server::SharedPtr server_; + + CollisionDetectingValidatorPlugin impl_; +}; + +} // namespace tmc_collision_detecting_validator + +# endif // TMC_COLLISION_DETECTING_VALIDATOR_COLLISION_DETECTING_VALIDATOR_HPP_ diff --git a/tmc_collision_detecting_validator/src/tmc_collision_detecting_validator/map_collision_checker.cpp b/tmc_collision_detecting_validator/src/tmc_collision_detecting_validator/map_collision_checker.cpp new file mode 100644 index 0000000..935fefe --- /dev/null +++ b/tmc_collision_detecting_validator/src/tmc_collision_detecting_validator/map_collision_checker.cpp @@ -0,0 +1,126 @@ +/* +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. +*/ +#include "map_collision_checker.hpp" + +#include +#include + +#include + +namespace { +// WORLD_JOINT: Virtual joint between robot Odometry (ODOM) and robot orbital +const char* const kBaseJointName = "world_joint"; +// It may be better to use parameters +const uint8_t kOccupancyFree = 0; + +uint32_t GetTargetJointIndex(const std::vector& joint_names, const std::string& target_joint) { + return std::distance(joint_names.begin(), std::find(joint_names.begin(), joint_names.end(), target_joint)); +} + +void OccupancyGridMsgToOccupancyGrid( + const nav_msgs::msg::OccupancyGrid& msg, + tmc_manipulation_types::OccupancyGrid& map) { + map.info.resolution = msg.info.resolution; + map.info.width = msg.info.width; + map.info.height = msg.info.height; + tf2::fromMsg(msg.info.origin, map.info.origin); + map.data.clear(); + std::copy(msg.data.begin(), msg.data.end(), std::back_inserter(map.data)); +} +} // anonymous namespace + +namespace tmc_collision_detecting_validator { + +MapCollisionChecker::MapCollisionChecker(const rclcpp::Node::SharedPtr& node, const std::string& map_topic_name) + : MapCollisionChecker(node, map_topic_name, nullptr) {} + +MapCollisionChecker::MapCollisionChecker(const rclcpp::Node::SharedPtr& node, const std::string& map_topic_name, + const tmc_utils::DynamicParameter::Ptr& verbose) + : map_frame_to_trajectory_origin_(Eigen::Affine3d::Identity()), + logger_(node->get_logger()), verbose_(verbose), map_topic_name_(map_topic_name) { + // TODO(Takeshita) QoS settings + map_sub_ = node->create_subscription( + map_topic_name, 1, std::bind(&MapCollisionChecker::MapCallback, this, std::placeholders::_1)); +} + +bool MapCollisionChecker::IsFeasible( + const tmc_manipulation_types::TimedRobotTrajectory& trajectory, + std::function interrupt) { + // If the MAP is empty, it does not collide + if (map_.data.empty()) { + return true; + } + + const auto map_origin_to_map_frame = map_.info.origin.inverse(); + const auto map_origin_to_trajectory_origin = map_origin_to_map_frame * map_frame_to_trajectory_origin_; + + // The orbit generated by RobotlocalPlanner is specification containing movement information. + uint32_t base_index = GetTargetJointIndex(trajectory.multi_dof_joint_trajectory.joint_names, kBaseJointName); + if (base_index == trajectory.multi_dof_joint_trajectory.joint_names.size()) { + return false; + } + + for (const auto& point : trajectory.multi_dof_joint_trajectory.points) { + if (interrupt()) { + return false; + } + if (base_index >= point.transforms.size()) { + return false; + } + if (IsRobotInCollision(map_origin_to_trajectory_origin * point.transforms[base_index])) { + if (verbose_ && verbose_->value()) { + RCLCPP_INFO(logger_, "Point (%f, %f) is in the obstacle area on the map %s", + point.transforms[base_index].translation().x(), point.transforms[base_index].translation().y(), + map_topic_name_.c_str()); + } + return false; + } + } + return true; +} + +void MapCollisionChecker::MapCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) { + OccupancyGridMsgToOccupancyGrid(*msg, map_); + map_frame_ = msg->header.frame_id; +} + +bool MapCollisionChecker::IsRobotInCollision(const Eigen::Affine3d& robot_position) const { + int32_t grid_x = std::floor(robot_position.translation().x() / map_.info.resolution); + int32_t grid_y = std::floor(robot_position.translation().y() / map_.info.resolution); + if (grid_x < 0 || grid_y < 0 || grid_x > map_.info.width - 1 || grid_y > map_.info.height - 1) { + return false; + } + + if (map_.data[grid_x + map_.info.width * grid_y] > kOccupancyFree) { + return true; + } else { + return false; + } +} + +} // namespace tmc_collision_detecting_validator diff --git a/tmc_collision_detecting_validator/src/tmc_collision_detecting_validator/map_collision_checker.hpp b/tmc_collision_detecting_validator/src/tmc_collision_detecting_validator/map_collision_checker.hpp new file mode 100644 index 0000000..3886889 --- /dev/null +++ b/tmc_collision_detecting_validator/src/tmc_collision_detecting_validator/map_collision_checker.hpp @@ -0,0 +1,75 @@ +/* +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. +*/ +#ifndef TMC_COLLISION_DETECTING_VALIDATOR_MAP_COLLISION_CHECKER_HPP_ +#define TMC_COLLISION_DETECTING_VALIDATOR_MAP_COLLISION_CHECKER_HPP_ + +#include +#include + +#include + +#include +#include + +#include +#include + +namespace tmc_collision_detecting_validator { + +class MapCollisionChecker { + public: + using Ptr = std::shared_ptr; + + MapCollisionChecker(const rclcpp::Node::SharedPtr& node, const std::string& map_topic_name); + MapCollisionChecker(const rclcpp::Node::SharedPtr& node, const std::string& map_topic_name, + const tmc_utils::DynamicParameter::Ptr& verbose); + + /// @brief Feasibility Check Stop immediately after checking collision + bool IsFeasible(const tmc_manipulation_types::TimedRobotTrajectory& trajectory, + std::function interrupt); + + void set_map_to_origin(const Eigen::Affine3d& map_to_origin) { map_frame_to_trajectory_origin_ = map_to_origin; } + std::string GetMapFrame() const { return map_frame_; } + + private: + Eigen::Affine3d map_frame_to_trajectory_origin_; + tmc_manipulation_types::OccupancyGrid map_; + std::string map_frame_; + + bool IsRobotInCollision(const Eigen::Affine3d& robot_position) const; + + void MapCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg); + rclcpp::Subscription::SharedPtr map_sub_; + + rclcpp::Logger logger_; + tmc_utils::DynamicParameter::Ptr verbose_; + std::string map_topic_name_; +}; + +} // namespace tmc_collision_detecting_validator +#endif // TMC_COLLISION_DETECTING_VALIDATOR_MAP_COLLISION_CHECKER_HPP_ diff --git a/tmc_collision_detecting_validator/src/tmc_collision_detecting_validator/node_main.cpp b/tmc_collision_detecting_validator/src/tmc_collision_detecting_validator/node_main.cpp new file mode 100644 index 0000000..2183ecf --- /dev/null +++ b/tmc_collision_detecting_validator/src/tmc_collision_detecting_validator/node_main.cpp @@ -0,0 +1,39 @@ +/* +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. +*/ +/// @brief main of collision detecting validator +#include +#include "collision_detecting_validator.hpp" + +int main(int argc, char* argv[]) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + node->Init(); + rclcpp::spin(node); + rclcpp::shutdown(); + return EXIT_SUCCESS; +} diff --git a/tmc_collision_detecting_validator/src/tmc_collision_detecting_validator/robot_collision_checker.cpp b/tmc_collision_detecting_validator/src/tmc_collision_detecting_validator/robot_collision_checker.cpp new file mode 100644 index 0000000..619212e --- /dev/null +++ b/tmc_collision_detecting_validator/src/tmc_collision_detecting_validator/robot_collision_checker.cpp @@ -0,0 +1,275 @@ +/* +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. +*/ +#include "robot_collision_checker.hpp" + +#include +#include +#include + +#include + +#include +#include + +namespace { +// WORLD_JOINT: Virtual joint between robot Odometry (ODOM) and robot orbital +const char* const kBaseJointName = "world_joint"; +} // anonymous namespace + +namespace tmc_collision_detecting_validator { + +AttachedObject::AttachedObject(const rclcpp::Node::SharedPtr& node, const std::string& topic_name) { + attached_object_sub_ = node->create_subscription( + topic_name, tmc_utils::BestEffortQoS(), + std::bind(&AttachedObject::AttachedObjectCallback, this, std::placeholders::_1)); +} + +void AttachedObject::UpdateCollisionDetector( + const tmc_robot_collision_detector::RobotCollisionDetector::Ptr& collision_detector) { + std::lock_guard lock(mutex_); + for (const auto& collision_object : attached_objects_) { + // If it is not included in the obstacle environment, add it + // Use the RobotCollisionDetector specification that external obstacles are included in a group called "Outer". + const auto object_names = collision_detector->GetObjectNameListByGroup("OUTER"); + if (std::find(object_names.begin(), object_names.end(), collision_object.object.id) == object_names.end()) { + // If the holding is set, the position etc. will be overwritten, so you only need to add an object anyway. + tmc_manipulation_types::OuterObjectParameters params; + tmc_manipulation_types_bridge::CollisionObjectToOuterObjectParameters(collision_object.object, params); + collision_detector->CreateOuterObject(params); + } + // Setting of gratitude + Eigen::Affine3d link_to_object; + tf2::fromMsg(collision_object.object.pose, link_to_object); + collision_detector->HoldObject(collision_object.object.id, collision_object.link_name, link_to_object); + } +} + +void AttachedObject::AttachedObjectCallback(const moveit_msgs::msg::RobotState::SharedPtr msg) { + std::unique_lock lock(mutex_, std::defer_lock); + if (lock.try_lock()) { + attached_objects_ = msg->attached_collision_objects; + } +} + + +RobotCollisionChecker::RobotCollisionChecker(const rclcpp::Node::SharedPtr& node) + : RobotCollisionChecker(node, nullptr) {} + +RobotCollisionChecker::RobotCollisionChecker(const rclcpp::Node::SharedPtr& node, + const tmc_utils::DynamicParameter::Ptr& verbose) + : fk_loader_("tmc_robot_kinematics_model", "tmc_robot_kinematics_model::IRobotKinematicsModel"), + logger_(node->get_logger()), verbose_(verbose) { + auto robot_model = tmc_utils::GetParameter(node, "robot_description_kinematics", ""); + if (robot_model.empty()) { + robot_model = tmc_utils::GetParameter(node, "robot_description", ""); + } + if (robot_model.empty()) { + RCLCPP_FATAL(node->get_logger(), "cannot get paramter robot_description and robot_description_kinematics"); + exit(EXIT_FAILURE); + } + const auto robot_collision_pair = tmc_utils::GetParameter(node, "robot_collision_pair", ""); + if (robot_collision_pair.empty()) { + RCLCPP_FATAL(node->get_logger(), "cannot get paramter robot_collision_pair"); + exit(EXIT_FAILURE); + } + const auto collision_engine = tmc_utils::GetParameter(node, "collision_engine", "ODE"); + + const auto kinematics_type = tmc_utils::GetParameter(node, "kinematics_type", ""); + if (kinematics_type.empty()) { + collision_detector_ = std::make_shared( + robot_model, robot_collision_pair, collision_engine); + } else { + RCLCPP_INFO_STREAM(node->get_logger(), "Kinematics type is " << kinematics_type); + const auto robot = fk_loader_.createSharedInstance(kinematics_type); + robot->Initialize(robot_model); + + collision_detector_ = std::make_shared( + robot, robot_model, robot_collision_pair, collision_engine); + } + + const auto attached_object_topics = tmc_utils::GetParameter( + node, "attached_object_topics", std::vector({"attached_object_publisher/attached_object"})); + for (const auto& topic_name : attached_object_topics) { + attached_objects_.emplace_back(std::make_shared(node, topic_name)); + } + + const auto trajectory_points_collision_check_order_type = tmc_utils::GetParameter( + node, "trajectory_points_collision_check_order_type", "TrajectoryPointsCollisionCheckSkippingOrder"); + if (trajectory_points_collision_check_order_type == "TrajectoryPointsCollisionCheckBothEnds") { + collision_check_order_ = std::make_shared(); + } else { + collision_check_order_ = std::make_shared(); + } + collision_check_order_->Init(node); + + environment_sub_ = node->create_subscription( + "collision_environment_server/transformed_environment", tmc_utils::BestEffortQoS(), + std::bind(&RobotCollisionChecker::EnvironmentCallback, this, std::placeholders::_1)); + + disabled_object_pairs_sub_ = node->create_subscription( + "~/collisions_disabled_object_pairs", tmc_utils::ReliableVolatileQoS(), + std::bind(&RobotCollisionChecker::DisabledObjectPairsCallback, this, std::placeholders::_1)); +} + +bool RobotCollisionChecker::IsFeasible( + const tmc_manipulation_types::TimedRobotTrajectory& trajectory, + std::function interrupt) { + // trajectory check + // If the size of Name is 0, return False + if (trajectory.joint_trajectory.joint_names.size() == 0) { + return false; + } + + uint32_t base_index = GetTargetJointIndex(trajectory.multi_dof_joint_trajectory.joint_names, kBaseJointName); + if (base_index == trajectory.multi_dof_joint_trajectory.joint_names.size()) { + return false; + } + + const auto indices = collision_check_order_->GenerateIndices(trajectory); + for (auto i : indices) { + if (interrupt()) { + return false; + } + // Return False if the size of name and point is different + if (trajectory.joint_trajectory.joint_names.size() != trajectory.joint_trajectory.points[i].positions.size()) { + return false; + } + // There is no base orbit to judge and returns False + if (base_index >= trajectory.multi_dof_joint_trajectory.points[i].transforms.size()) { + return false; + } + + // Set the orbit + tmc_manipulation_types::JointState state = + {trajectory.joint_trajectory.joint_names, + trajectory.joint_trajectory.points[i].positions, + Eigen::VectorXd(), + Eigen::VectorXd()}; + collision_detector_->SetRobotNamedAngle(state); + collision_detector_->SetRobotTransform(trajectory.multi_dof_joint_trajectory.points[i].transforms[base_index]); + + // Interference check + std::vector> contact_pair; + if (collision_detector_->CheckCollision(true, contact_pair)) { + if (verbose_ && verbose_->value()) { + std::stringstream stream; + stream << "Collision: " << contact_pair.front().first << " - " << contact_pair.front().second << std::endl; + for (auto j = 0; j < trajectory.joint_trajectory.joint_names.size(); ++j) { + stream << " " << trajectory.joint_trajectory.joint_names[j] << ": " + << trajectory.joint_trajectory.points[i].positions[j] << std::endl; + } + stream << " base_pos_x: " + << trajectory.multi_dof_joint_trajectory.points[i].transforms[base_index].translation().x() << std::endl; + stream << " base_pos_y: " + << trajectory.multi_dof_joint_trajectory.points[i].transforms[base_index].translation().y() << std::endl; + const auto euler_angles = + trajectory.multi_dof_joint_trajectory.points[i].transforms[base_index].linear().eulerAngles(0, 1, 2); + stream << " base_ori_yaw: " << euler_angles[2] << std::endl; + RCLCPP_INFO_STREAM(logger_, stream.str()); + } + return false; + } + } + return true; +} + +void RobotCollisionChecker::UpdateEnvironmentObstacleObject() { + std::lock_guard lock(mutex_); + for (const auto& collision_object : environment_msg_.collision_objects) { + // There is no coordinate conversion, and coordinates have already been converted to the topic + tmc_manipulation_types::OuterObjectParameters params; + tmc_manipulation_types_bridge::CollisionObjectToOuterObjectParameters(collision_object, params); + collision_detector_->CreateOuterObject(params); + } + for (const auto& attached_objects : attached_objects_) { + attached_objects->UpdateCollisionDetector(collision_detector_); + } + for (const auto& pair : disabled_object_pairs_msg_.pairs) { + try { + collision_detector_->DisableCollisionCheckObjectToObject(pair.names[0], pair.names[1]); + } catch(const std::domain_error& error) { + RCLCPP_WARN_STREAM(logger_, "DisableCollisionCheck failure: " << pair.names[0] << " - " << pair.names[1]); + RCLCPP_DEBUG_STREAM(logger_, error.what()); + } + } +} + +void RobotCollisionChecker::CreateObstacleObject( + const tmc_manipulation_types::OuterObjectParametersSeq& obstacle_object_seq) { + for (auto object : obstacle_object_seq) { + collision_detector_->CreateOuterObject(object); + } +} + +void RobotCollisionChecker::DestroyAllObstacleObject() { + collision_detector_->DestroyAllOuterObject(); +} + +void RobotCollisionChecker::DumpEnvironment(const tmc_utils::MessageLogger::Ptr& logger) { + if (logger) { + { + moveit_msgs::msg::PlanningSceneWorld msg; + const auto params_seq = collision_detector_->GetAllOuterObjectParameters(); + for (const auto& params : params_seq) { + moveit_msgs::msg::CollisionObject collision_object; + tmc_manipulation_types_bridge::OuterObjectParametersToCollisionObject(params, collision_object); + msg.collision_objects.emplace_back(collision_object); + } + logger->SaveMessage(".env", msg); + } + { + moveit_msgs::msg::RobotState msg; + const auto attached_object_names = collision_detector_->GetHeldObjectList(); + for (const auto& name : attached_object_names) { + auto params = collision_detector_->GetObjectParameter(name); + + moveit_msgs::msg::AttachedCollisionObject attached_object; + attached_object.link_name = params.parent_name; + + params.origin_to_base = params.parent_to_base; + tmc_manipulation_types_bridge::OuterObjectParametersToCollisionObject(params, attached_object.object); + + msg.attached_collision_objects.push_back(attached_object); + } + logger->SaveMessage(".attached", msg); + } + logger->SaveMessage(".disabled", disabled_object_pairs_msg_); + } +} + +void RobotCollisionChecker::EnvironmentCallback(const moveit_msgs::msg::PlanningSceneWorld::SharedPtr msg) { + std::unique_lock lock(mutex_, std::defer_lock); + if (lock.try_lock()) { + environment_msg_ = *msg; + } +} + +void RobotCollisionChecker::DisabledObjectPairsCallback(const tmc_planning_msgs::msg::ObjectPairsList::SharedPtr msg) { + disabled_object_pairs_msg_ = *msg; +} +} // namespace tmc_collision_detecting_validator diff --git a/tmc_collision_detecting_validator/src/tmc_collision_detecting_validator/robot_collision_checker.hpp b/tmc_collision_detecting_validator/src/tmc_collision_detecting_validator/robot_collision_checker.hpp new file mode 100644 index 0000000..d94209e --- /dev/null +++ b/tmc_collision_detecting_validator/src/tmc_collision_detecting_validator/robot_collision_checker.hpp @@ -0,0 +1,117 @@ +/* +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. +*/ +#ifndef TMC_COLLISION_DETECTING_VALIDATOR_ROBOT_COLLISION_CHECKER_HPP_ +#define TMC_COLLISION_DETECTING_VALIDATOR_ROBOT_COLLISION_CHECKER_HPP_ + +#include +#include +#include + +#include +#include +#include + +#include + +// It is necessary to include the one with Pinocchio mixed first +#include // NOLINT +#include +#include +#include +#include +#include + +#include "utils.hpp" + +using tmc_robot_collision_detector::RobotCollisionDetector; + +namespace tmc_collision_detecting_validator { + +class AttachedObject { + public: + AttachedObject(const rclcpp::Node::SharedPtr& node, const std::string& topic_name); + + /// @brief Updating the attached object information of collision_detector + /// @param collision_detector Collision detector + void UpdateCollisionDetector(const tmc_robot_collision_detector::RobotCollisionDetector::Ptr& collision_detector); + + private: + void AttachedObjectCallback(const moveit_msgs::msg::RobotState::SharedPtr msg); + rclcpp::Subscription::SharedPtr attached_object_sub_; + + std::vector attached_objects_; + std::mutex mutex_; +}; + +class RobotCollisionChecker { + public: + using Ptr = std::shared_ptr; + + explicit RobotCollisionChecker(const rclcpp::Node::SharedPtr& node); + RobotCollisionChecker(const rclcpp::Node::SharedPtr& node, const tmc_utils::DynamicParameter::Ptr& verbose); + + /// @brief Feasibility Check Stop immediately after checking collision + bool IsFeasible(const tmc_manipulation_types::TimedRobotTrajectory& trajectory, + std::function interrupt); + + /// @brief Add obstacle information from the collision environment topic + void UpdateEnvironmentObstacleObject(); + + /// @brief Set Obstacle objects to robot outside + void CreateObstacleObject(const tmc_manipulation_types::OuterObjectParametersSeq& obstacle_object_seq); + + /// @brief Clear robot outside obstacle objects + void DestroyAllObstacleObject(); + + /// @brief Dump collision objects as moveit_msgs::msg::PlanningSceneWorld + void DumpEnvironment(const tmc_utils::MessageLogger::Ptr& logger); + + private: + pluginlib::ClassLoader fk_loader_; + + tmc_robot_collision_detector::RobotCollisionDetector::Ptr collision_detector_; + + rclcpp::Logger logger_; + tmc_utils::DynamicParameter::Ptr verbose_; + + void EnvironmentCallback(const moveit_msgs::msg::PlanningSceneWorld::SharedPtr msg); + rclcpp::Subscription::SharedPtr environment_sub_; + moveit_msgs::msg::PlanningSceneWorld environment_msg_; + std::mutex mutex_; + + void DisabledObjectPairsCallback(const tmc_planning_msgs::msg::ObjectPairsList::SharedPtr msg); + rclcpp::Subscription::SharedPtr disabled_object_pairs_sub_; + tmc_planning_msgs::msg::ObjectPairsList disabled_object_pairs_msg_; + + std::vector> attached_objects_; + + std::shared_ptr collision_check_order_; +}; + +} // namespace tmc_collision_detecting_validator +#endif // TMC_COLLISION_DETECTING_VALIDATOR_ROBOT_COLLISION_CHECKER_HPP_ diff --git a/tmc_collision_detecting_validator/src/tmc_collision_detecting_validator/utils.cpp b/tmc_collision_detecting_validator/src/tmc_collision_detecting_validator/utils.cpp new file mode 100644 index 0000000..4465d5c --- /dev/null +++ b/tmc_collision_detecting_validator/src/tmc_collision_detecting_validator/utils.cpp @@ -0,0 +1,195 @@ +/* +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. +*/ +/// @file utils.cpp +/// @brief Utility functions +/// @author Yuta Watanabe + +#include "utils.hpp" + +#include +#include +#include + +#include + +namespace { + +uint32_t Devide(std::vector>& candidates_seq) { + if (candidates_seq.size() == 1 && candidates_seq[0].size() == 1) { + auto value = candidates_seq[0][0]; + candidates_seq.clear(); + return value; + } + + uint32_t max_size = 0; + std::vector>::iterator target_it; + for (std::vector>::iterator it = candidates_seq.begin(); it != candidates_seq.end(); ++it) { + if (it->size() > max_size) { + max_size = it->size(); + target_it = it; + } + } + + if (target_it->size() == 2) { + auto value = target_it->back(); + target_it->pop_back(); + return value; + } + + auto half_index = static_cast(std::floor(static_cast(target_it->size()) / 2.0)); + auto value = target_it->at(half_index); + std::vector devided; + for (uint32_t i = half_index + 1; i < target_it->size(); ++i) { + devided.push_back(target_it->at(i)); + } + target_it->resize(half_index); + candidates_seq.insert(target_it + 1, devided); + return value; +} + +void Union(std::vector>& candidates_seq) { + std::vector new_candidates; + for (const auto candidates : candidates_seq) { + if (candidates.size() != 1) { + return; + } + new_candidates.push_back(candidates[0]); + } + candidates_seq = {new_candidates}; +} + +/// @breif Return the index to explore by jumping +std::vector MakeSkippingIndices(uint32_t num) { + std::vector indices; + if (num == 0) { + } else if (num == 1) { + indices.push_back(0); + } else if (num == 2) { + indices.push_back(1); + indices.push_back(0); + } else { + indices.push_back(num - 1); + indices.push_back(0); + std::vector candidates; + for (uint32_t i = 1; i < num - 1; ++i) { + candidates.push_back(i); + } + std::vector> candidates_seq = {candidates}; + while (true) { + indices.push_back(Devide(candidates_seq)); + if (candidates_seq.empty()) break; + Union(candidates_seq); + } + } + return indices; +} + +std::vector MergeIndices(const std::vector& indices_a, const std::vector& indices_b) { + std::vector indices_merged; + uint32_t count = 0; + while ((count < indices_a.size()) && (count < indices_b.size())) { + if (count < indices_a.size()) { + indices_merged.push_back(indices_a[count]); + } + if (count < indices_b.size()) { + indices_merged.push_back(indices_b[count]); + } + ++count; + } + return indices_merged; +} + +} // namespace + +namespace tmc_collision_detecting_validator { + +uint32_t GetTargetJointIndex(const std::vector& joint_names, const std::string& target_joint) { + return std::distance(joint_names.begin(), std::find(joint_names.begin(), joint_names.end(), target_joint)); +} + +std::vector TrajectoryPointsCollisionCheckSkippingOrder::GenerateIndices( + const tmc_manipulation_types::TimedRobotTrajectory& trajectory) { + return MakeSkippingIndices(trajectory.joint_trajectory.points.size()); +} + +void TrajectoryPointsCollisionCheckBothEnds::Init(const rclcpp::Node::SharedPtr& node) { + time_from_start_ = tmc_utils::GetParameter(node, "time_from_start", 1.0); + time_from_end_ = tmc_utils::GetParameter(node, "time_from_end", 1.0); + interval_middle_ = tmc_utils::GetParameter(node, "interval_middle", 0.2); +} + +std::vector TrajectoryPointsCollisionCheckBothEnds::GenerateIndices( + const tmc_manipulation_types::TimedRobotTrajectory& trajectory) { + const auto motion_time = trajectory.joint_trajectory.points.back().time_from_start; + if (motion_time < time_from_start_ + time_from_end_) { + return MakeSkippingIndices(trajectory.joint_trajectory.points.size()); + } + + // It is better to work hard on abnormal processing, but it is a premise that a decent chopped trajectory will be entered, so I will not do my best now. + std::vector indices_from_start; + for (auto i = 0u; i < trajectory.joint_trajectory.points.size(); ++i) { + if (trajectory.joint_trajectory.points[i].time_from_start < time_from_start_) { + indices_from_start.push_back(i); + } else { + break; + } + } + + std::vector indices_from_end; + const auto time_from_end_threshold = motion_time - time_from_end_; + for (auto i = 0u; i < trajectory.joint_trajectory.points.size(); ++i) { + const auto index = trajectory.joint_trajectory.points.size() - i - 1; + if (trajectory.joint_trajectory.points[index].time_from_start > time_from_end_threshold) { + indices_from_end.push_back(index); + } else { + break; + } + } + + auto merged_indices = indices_from_start; + + auto target_stamp = trajectory.joint_trajectory.points[indices_from_start.back()].time_from_start + interval_middle_; + for (auto i = indices_from_start.back() + 1; i < indices_from_end.back(); ++i) { + if (trajectory.joint_trajectory.points[i].time_from_start > target_stamp) { + merged_indices.push_back(i); + target_stamp += interval_middle_; + } + } + merged_indices.insert(merged_indices.end(), indices_from_end.rbegin(), indices_from_end.rend()); + + const auto skipping = MakeSkippingIndices(merged_indices.size()); + + std::vector result; + for (auto x : skipping) { + result.push_back(merged_indices[x]); + } + return result; +} + + +} // namespace tmc_collision_detecting_validator diff --git a/tmc_collision_detecting_validator/src/tmc_collision_detecting_validator/utils.hpp b/tmc_collision_detecting_validator/src/tmc_collision_detecting_validator/utils.hpp new file mode 100644 index 0000000..6bc7b8e --- /dev/null +++ b/tmc_collision_detecting_validator/src/tmc_collision_detecting_validator/utils.hpp @@ -0,0 +1,86 @@ +/* +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. +*/ +#ifndef TMC_COLLISION_DETECTING_VALIDATOR_UTILS_HPP_ +#define TMC_COLLISION_DETECTING_VALIDATOR_UTILS_HPP_ + +#include +#include +#include +#include + +#include + +#include + +namespace tmc_collision_detecting_validator { + +/// @breif Returns the element number of the target joint +/// @param Joint_names search joint +/// @param Target_Joint target joint +/// @return If you do not do index, return the number of elements +uint32_t GetTargetJointIndex(const std::vector& joint_names, const std::string& target_joint); + +class ITrajectoryPointsCollisionCheckOrder { + public: + ITrajectoryPointsCollisionCheckOrder() = default; + virtual ~ITrajectoryPointsCollisionCheckOrder() = default; + + /// @breif Prepare an INIT function separately from the constructor in consideration of initialization and pluginization. + virtual void Init(const rclcpp::Node::SharedPtr& node) = 0; + + /// @breif Returns the index in the order of collisions at orbit + virtual std::vector GenerateIndices(const tmc_manipulation_types::TimedRobotTrajectory& trajectory) = 0; +}; + +// Explore to jump +class TrajectoryPointsCollisionCheckSkippingOrder : public ITrajectoryPointsCollisionCheckOrder { + public: + TrajectoryPointsCollisionCheckSkippingOrder() = default; + virtual ~TrajectoryPointsCollisionCheckSkippingOrder() = default; + + void Init(const rclcpp::Node::SharedPtr& node) override {} + std::vector GenerateIndices(const tmc_manipulation_types::TimedRobotTrajectory& trajectory) override; +}; + +// Explore only the beginning and the end +class TrajectoryPointsCollisionCheckBothEnds : public ITrajectoryPointsCollisionCheckOrder { + public: + TrajectoryPointsCollisionCheckBothEnds() = default; + virtual ~TrajectoryPointsCollisionCheckBothEnds() = default; + + void Init(const rclcpp::Node::SharedPtr& node) override; + std::vector GenerateIndices(const tmc_manipulation_types::TimedRobotTrajectory& trajectory) override; + + private: + double time_from_start_; + double time_from_end_; + double interval_middle_; +}; + +} // namespace tmc_collision_detecting_validator +#endif // TMC_COLLISION_DETECTING_VALIDATOR_UTILS_HPP_ diff --git a/tmc_collision_detecting_validator/src/tmc_collision_detecting_validator/validate_offline.cpp b/tmc_collision_detecting_validator/src/tmc_collision_detecting_validator/validate_offline.cpp new file mode 100644 index 0000000..3d20746 --- /dev/null +++ b/tmc_collision_detecting_validator/src/tmc_collision_detecting_validator/validate_offline.cpp @@ -0,0 +1,236 @@ +/* +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. +*/ +/// @brief Validation visualization script + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "utils.hpp" + +namespace { +const char* const kBaseJointName = "world_joint"; +constexpr double kPublishPeriod = 0.1; + +class RobotStatePublisher { + public: + explicit RobotStatePublisher(const rclcpp::Node::SharedPtr& node); + + void Publish(const tmc_robot_collision_detector::RobotCollisionDetector::Ptr& robot, double duration); + + private: + rclcpp::Publisher::SharedPtr joint_pub_; + std::shared_ptr broadcaster_; + + rclcpp::Clock::SharedPtr clock_; +}; + +RobotStatePublisher::RobotStatePublisher(const rclcpp::Node::SharedPtr& node) : clock_(node->get_clock()) { + joint_pub_ = node->create_publisher("debug_joint_state", rclcpp::SystemDefaultsQoS()); + broadcaster_ = std::make_shared(node); +} + +void RobotStatePublisher::Publish(const tmc_robot_collision_detector::RobotCollisionDetector::Ptr& robot, + double duration) { + const auto origin_to_base = robot->GetRobotTransform(); + + auto origin_to_base_msg = tf2::eigenToTransform(origin_to_base); + origin_to_base_msg.header.frame_id = "odom"; + origin_to_base_msg.child_frame_id = "base_footprint"; + + const auto joint_states = robot->GetRobotNamedAngle(); + sensor_msgs::msg::JointState joint_states_msg; + tmc_manipulation_types_bridge::JointStateToJointStateMsg(joint_states, joint_states_msg); + + const auto steps = static_cast(duration / kPublishPeriod); + for (uint32_t i = 0; i < steps; ++i) { + origin_to_base_msg.header.stamp = clock_->now(); + broadcaster_->sendTransform(origin_to_base_msg); + + joint_pub_->publish(joint_states_msg); + + std::this_thread::sleep_for(std::chrono::milliseconds(static_cast(kPublishPeriod * 1000))); + } +} + + +class EnvironmentMarkerPublisher { + public: + explicit EnvironmentMarkerPublisher(const rclcpp::Node::SharedPtr& node); + void Publish(const tmc_robot_collision_detector::RobotCollisionDetector::Ptr& robot); + + private: + rclcpp::Publisher::SharedPtr marker_pub_; +}; + +EnvironmentMarkerPublisher::EnvironmentMarkerPublisher(const rclcpp::Node::SharedPtr& node) { + marker_pub_ = node->create_publisher( + "debug_environment", rclcpp::SystemDefaultsQoS()); +} + +void EnvironmentMarkerPublisher::Publish(const tmc_robot_collision_detector::RobotCollisionDetector::Ptr& robot) { + const auto collision_objects = robot->GetAllOuterObjectParameters(); + visualization_msgs::msg::MarkerArray marker_array; + std_msgs::msg::ColorRGBA color; + color.g = 1.0; + color.a = 1.0; + for (const auto& collision_object : collision_objects) { + std::vector markers; + tmc_manipulation_types_bridge::OuterObjectParametersToMarkerMsg( + collision_object, marker_array.markers.size(), "odom", rclcpp::Time(0, 0), color, markers); + marker_array.markers.insert(marker_array.markers.end(), markers.begin(), markers.end()); + } + marker_pub_->publish(marker_array); +} + + +Eigen::Affine3d Extract(const trajectory_msgs::msg::MultiDOFJointTrajectoryPoint& point, uint32_t index) { + return tf2::transformToEigen(point.transforms[index]); +} + +tmc_manipulation_types::JointState Extract(const std::vector& joint_names, + const trajectory_msgs::msg::JointTrajectoryPoint& point) { + tmc_manipulation_types::JointState state; + state.name = joint_names; + state.position = Eigen::Map(&point.positions[0], point.positions.size()); + return state; +} + +} // namespace + +int main(int argc, char* argv[]) { + rclcpp::init(argc, argv); + + if (argc < 2) { + std::cerr << "usage: " << argv[0] << " validation_xxxx" << std::endl; + return EXIT_FAILURE; + } + const auto node = rclcpp::Node::make_shared("client"); + + tmc_planning_msgs::action::ValidateRobotTrajectories::Goal action_goal; + if (!tmc_utils::LoadMsg(std::string(argv[1]) + ".goal", action_goal)) { + std::cerr << "Fail to load " << argv[1] << ".goal" << std::endl; + return EXIT_FAILURE; + } + RCLCPP_INFO_STREAM(node->get_logger(), "Num of Input Trajectory: " << action_goal.robot_trajectories.size()); + + std_msgs::msg::UInt32 validation_count; + if (tmc_utils::LoadMsg(std::string(argv[1]) + ".count", validation_count)) { + // There is information that you checked up to Index N, so the number of checks is displayed by +1. + RCLCPP_INFO_STREAM(node->get_logger(), "Num of Validated Trajectory: " << validation_count.data + 1); + } + + moveit_msgs::msg::PlanningSceneWorld env; + if (!tmc_utils::LoadMsg(std::string(argv[1]) + ".env", env)) { + // There may not be any environmental information or knowledgeable information, so it will continue to process just by giving a message. + std::cerr << "Fail to load " << argv[1] << ".env" << std::endl; + } + RCLCPP_INFO_STREAM(node->get_logger(), "Num of Collision Objects: " << env.collision_objects.size()); + + moveit_msgs::msg::RobotState attached_objects; + if (!tmc_utils::LoadMsg(std::string(argv[1]) + ".attached", attached_objects)) { + std::cerr << "Fail to load " << argv[1] << ".attached" << std::endl; + } + RCLCPP_INFO_STREAM(node->get_logger(), + "Num of Attached Objects: " << attached_objects.attached_collision_objects.size()); + + tmc_planning_msgs::msg::ObjectPairsList disabled_objects; + if (!tmc_utils::LoadMsg(std::string(argv[1]) + ".disabled", disabled_objects)) { + std::cerr << "Fail to load " << argv[1] << ".disabled" << std::endl; + } + + const auto publisher = std::make_shared(node); + const auto environment_pub = std::make_shared(node); + const auto robot_description = tmc_utils::GetParameter(node, "robot_description", ""); + const auto robot_collision_pair = tmc_utils::GetParameter(node, "robot_collision_pair", ""); + + const auto collision_detector = std::make_shared( + robot_description, robot_collision_pair, "ODE"); + for (const auto& collision_object : env.collision_objects) { + tmc_manipulation_types::OuterObjectParameters params; + tmc_manipulation_types_bridge::CollisionObjectToOuterObjectParameters(collision_object, params); + collision_detector->CreateOuterObject(params); + } + for (const auto& attached_object : attached_objects.attached_collision_objects) { + Eigen::Affine3d link_to_object; + tf2::fromMsg(attached_object.object.pose, link_to_object); + collision_detector->HoldObject(attached_object.object.id, attached_object.link_name, link_to_object); + RCLCPP_INFO_STREAM(node->get_logger(), attached_object.link_name << " - " << attached_object.object.id); + } + for (const auto& pair : disabled_objects.pairs) { + RCLCPP_INFO_STREAM(node->get_logger(), "Disabled: " << pair.names[0] << " - " << pair.names[1]); + collision_detector->DisableCollisionCheckObjectToObject(pair.names[0], pair.names[1]); + } + + environment_pub->Publish(collision_detector); + + for (auto i = 0; i < action_goal.robot_trajectories.size(); ++i) { + RCLCPP_INFO_STREAM(node->get_logger(), "Trajectory " << i); + bool is_collision = false; + + const auto robot_trajectory = action_goal.robot_trajectories[i]; + const uint32_t base_index = tmc_collision_detecting_validator::GetTargetJointIndex( + robot_trajectory.multi_dof_joint_trajectory.joint_names, kBaseJointName); + for (auto j = 0; j < robot_trajectory.joint_trajectory.points.size(); ++j) { + collision_detector->SetRobotTransform( + Extract(robot_trajectory.multi_dof_joint_trajectory.points[j], base_index)); + collision_detector->SetRobotNamedAngle( + Extract(robot_trajectory.joint_trajectory.joint_names, robot_trajectory.joint_trajectory.points[j])); + environment_pub->Publish(collision_detector); + + std::vector> contact_pair; + if (collision_detector->CheckCollision(true, contact_pair)) { + RCLCPP_INFO_STREAM(node->get_logger(), " Point " << j << " Collision: " + << contact_pair.front().first << " - " << contact_pair.front().second); + is_collision = true; + + publisher->Publish(collision_detector, 3.0); + break; + } + publisher->Publish(collision_detector, 0.1); + } + + if (!is_collision) { + RCLCPP_INFO_STREAM(node->get_logger(), " Safe"); + break; + } + } + + return EXIT_SUCCESS; +} diff --git a/tmc_collision_detecting_validator/test/collision_detecting_validator-test.cpp b/tmc_collision_detecting_validator/test/collision_detecting_validator-test.cpp new file mode 100644 index 0000000..923e726 --- /dev/null +++ b/tmc_collision_detecting_validator/test/collision_detecting_validator-test.cpp @@ -0,0 +1,469 @@ +/* +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. +*/ +/// @brief CollisionDetectioningValidator test + +#include + +#include +#include +#include + +#include + +#include "../src/tmc_collision_detecting_validator/collision_detecting_validator.hpp" + +namespace { + +void SetMapData(const uint32_t& map_size, + uint32_t free_ratio, + nav_msgs::msg::OccupancyGrid& map_out) { + auto free_num = std::round(map_size * free_ratio / 100); + for (uint32_t i = 0; i < free_num; ++i) { + map_out.data.push_back(0); + } + for (uint32_t i = free_num; i < map_size; ++i) { + map_out.data.push_back(100); + } +} + +nav_msgs::msg::OccupancyGrid GenerateMap() { + // A map with the lower half occupied + nav_msgs::msg::OccupancyGrid map; + map.header.frame_id = "map"; + map.info.resolution = 0.05; + map.info.width = 200; + map.info.height = 200; + map.info.origin.position.x = 1.0; + map.info.origin.position.y = -1.0; + map.info.origin.orientation.z = 1.0; + map.info.origin.orientation.w = 0.0; + SetMapData(map.info.width * map.info.height, 50, map); + return map; +} + +moveit_msgs::msg::RobotTrajectory GenerateRobotTrajectory(double x, double y, uint32_t num_of_points) { + moveit_msgs::msg::RobotTrajectory robot_trajectory; + robot_trajectory.joint_trajectory.joint_names = {"joint1", "joint2", "joint3", "joint4", "joint5", "joint6"}; + robot_trajectory.joint_trajectory.points.resize(num_of_points); + for (uint32_t i = 0; i < num_of_points; ++i) { + robot_trajectory.joint_trajectory.points[i].positions.resize(6, 1.57); + } + geometry_msgs::msg::Transform transform; + transform.translation.x = x; + transform.translation.y = y; + transform.rotation.w = 1.0; + robot_trajectory.multi_dof_joint_trajectory.joint_names = {"world_joint"}; + robot_trajectory.multi_dof_joint_trajectory.points.resize(num_of_points); + for (uint32_t i = 0; i < num_of_points; ++i) { + robot_trajectory.multi_dof_joint_trajectory.points[i].transforms.push_back(transform); + } + return robot_trajectory; +} + +moveit_msgs::msg::RobotTrajectory GenerateOnePointRobotTrajectory(double x, double y) { + return GenerateRobotTrajectory(x, y, 1); +} + +moveit_msgs::msg::RobotTrajectory GenerateTestDefaultTrajectory() { + moveit_msgs::msg::RobotTrajectory robot_trajectory; + robot_trajectory.joint_trajectory.joint_names = {"joint1", "joint2", "joint3", "joint4", "joint5", "joint6"}; + robot_trajectory.joint_trajectory.points.resize(2); + for (uint32_t i = 0; i < robot_trajectory.joint_trajectory.joint_names.size(); ++i) { + robot_trajectory.joint_trajectory.points[0].positions.push_back(0.0); + robot_trajectory.joint_trajectory.points[1].positions.push_back(1.57); + } + robot_trajectory.joint_trajectory.points[0].positions[2] = 0.3; + geometry_msgs::msg::Transform transform; + robot_trajectory.multi_dof_joint_trajectory.joint_names = {"world_joint"}; + robot_trajectory.multi_dof_joint_trajectory.points.resize(2); + transform.translation.x = 1.0; + transform.translation.y = 0.5; + robot_trajectory.multi_dof_joint_trajectory.points[0].transforms.push_back(transform); + transform.translation.x = 0.0; + transform.translation.y = 0.5; + robot_trajectory.multi_dof_joint_trajectory.points[1].transforms.push_back(transform); + + return robot_trajectory; +} + +void CheckTrajectory(const moveit_msgs::msg::RobotTrajectory& actual, + const moveit_msgs::msg::RobotTrajectory& expected) { + ASSERT_EQ(actual.joint_trajectory.points.size(), expected.joint_trajectory.points.size()); + for (auto point_i = 0; point_i < expected.joint_trajectory.points.size(); ++point_i) { + SCOPED_TRACE(::testing::Message() << "Point " << point_i); + ASSERT_EQ(actual.joint_trajectory.points[point_i].positions.size(), + expected.joint_trajectory.points[point_i].positions.size()); + for (auto position_i = 0; position_i < expected.joint_trajectory.points[point_i].positions.size(); ++position_i) { + SCOPED_TRACE(::testing::Message() << "Position " << position_i); + EXPECT_DOUBLE_EQ(actual.joint_trajectory.points[point_i].positions[position_i], + expected.joint_trajectory.points[point_i].positions[position_i]); + } + } + + ASSERT_EQ(actual.multi_dof_joint_trajectory.points.size(), expected.multi_dof_joint_trajectory.points.size()); + for (auto point_i = 0; point_i < expected.multi_dof_joint_trajectory.points.size(); ++point_i) { + SCOPED_TRACE(::testing::Message() << "Point " << point_i); + ASSERT_EQ(actual.multi_dof_joint_trajectory.points[point_i].transforms.size(), + expected.multi_dof_joint_trajectory.points[point_i].transforms.size()); + for (auto position_i = 0; position_i < expected.multi_dof_joint_trajectory.points[point_i].transforms.size(); + ++position_i) { + SCOPED_TRACE(::testing::Message() << "Transform " << position_i); + auto actual_point = actual.multi_dof_joint_trajectory.points[point_i]; + auto expected_point = expected.multi_dof_joint_trajectory.points[point_i]; + ASSERT_EQ(actual_point.transforms.size(), 1); + EXPECT_DOUBLE_EQ(actual_point.transforms[0].translation.x, expected_point.transforms[0].translation.x); + EXPECT_DOUBLE_EQ(actual_point.transforms[0].translation.y, expected_point.transforms[0].translation.y); + EXPECT_DOUBLE_EQ(std::atan2(actual_point.transforms[0].rotation.z, actual_point.transforms[0].rotation.w), + std::atan2(expected_point.transforms[0].rotation.z, expected_point.transforms[0].rotation.w)); + } + } +} + +moveit_msgs::msg::CollisionObject GenerateOneObstacleCube() { + moveit_msgs::msg::CollisionObject object; + object.header.frame_id = "odom"; + object.id = "object1"; + // A position that collides with the orbit created in GenerateTestdefaultTrajectory + object.pose.position.x = 1.0; + object.pose.position.y = 0.5; + object.primitives.resize(1); + object.primitives[0].type = shape_msgs::msg::SolidPrimitive::BOX; + object.primitives[0].dimensions = {0.01, 0.01, 0.01}; + object.primitive_poses.resize(1); + object.operation = moveit_msgs::msg::CollisionObject::ADD; + return object; +} + +} // namespace + +namespace tmc_collision_detecting_validator { + +// Differences in behavior due to differences in parameters are performed by node testing. +TEST(CollisionDetectingValidatorPluginTest, Validate) { + pluginlib::ClassLoader loader( + "tmc_robot_local_planner", "tmc_robot_local_planner::IValidator"); + auto validator = loader.createSharedInstance("tmc_collision_detecting_validator/CollisionDetectingValidatorPlugin"); + + auto node = rclcpp::Node::make_shared("plugins"); + node->declare_parameter("robot_description", tmc_manipulation_tests::stanford_manipulator::GetUrdf()); + node->declare_parameter("robot_collision_pair", tmc_manipulation_tests::stanford_manipulator::GetCollisionConfig()); + validator->Initialize(node); + + std::vector trajectories_in; + trajectories_in.resize(1); + tmc_manipulation_types_bridge::RobotTrajectoryMsgToTimedRobotTrajectory( + GenerateTestDefaultTrajectory(), trajectories_in[0]); + + tmc_manipulation_types::TimedRobotTrajectory trajectory_out; + std::function func = []() -> bool{ return false; }; + EXPECT_TRUE(validator->Validate(trajectories_in, func, trajectory_out)); +} + + +class CollisionDetectingValidatorTest : public ::testing::Test { + protected: + void SetUp() override; + + void SpinSome() { + rclcpp::spin_some(client_node_); + rclcpp::spin_some(server_node_); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + template + void WaitForFutureComplete(const typename std::shared_future& future) { + while (future.wait_for(std::chrono::microseconds(1)) == std::future_status::timeout) { + SpinSome(); + } + } + + tmc_planning_msgs::action::ValidateRobotTrajectories::Result::SharedPtr SendGoalSync( + const std::vector& robot_trajectory, int8_t expected) { + tmc_planning_msgs::action::ValidateRobotTrajectories::Goal goal; + goal.robot_trajectories = robot_trajectory; + + auto future_goal_handle = validate_client_->async_send_goal(goal); + WaitForFutureComplete(future_goal_handle); + + auto goal_handle = future_goal_handle.get(); + EXPECT_TRUE(goal_handle.get()); + + auto future_result = validate_client_->async_get_result(goal_handle); + WaitForFutureComplete(future_result); + + auto wrapped_result = future_result.get(); + EXPECT_EQ(static_cast(wrapped_result.code), expected); + EXPECT_TRUE(wrapped_result.result); + return wrapped_result.result; + } + + void PublishAttachedObject() { + moveit_msgs::msg::RobotState robot_state; + robot_state.attached_collision_objects.resize(1); + robot_state.attached_collision_objects[0].link_name = "link7"; + robot_state.attached_collision_objects[0].object.header.frame_id = "link7"; + robot_state.attached_collision_objects[0].object.id = "object2"; + robot_state.attached_collision_objects[0].object.pose.position.x = 0.0; + robot_state.attached_collision_objects[0].object.pose.position.y = 0.0; + robot_state.attached_collision_objects[0].object.pose.position.z = 0.15; + robot_state.attached_collision_objects[0].object.primitives.resize(1); + robot_state.attached_collision_objects[0].object.primitives[0].type = shape_msgs::msg::SolidPrimitive::BOX; + robot_state.attached_collision_objects[0].object.primitives[0].dimensions = {0.05, 0.05, 0.05}; + robot_state.attached_collision_objects[0].object.primitive_poses.resize(1); + attached_object_pub_->publish(robot_state); + SpinSome(); + } + + rclcpp::Node::SharedPtr client_node_; + std::shared_ptr server_node_; + + rclcpp_action::Client::SharedPtr validate_client_; + + std::shared_ptr tf_static_broadcaster_; + rclcpp::Publisher::SharedPtr static_map_pub_; + rclcpp::Publisher::SharedPtr dynamic_map_pub_; + rclcpp::Publisher::SharedPtr collision_object_pub_; + rclcpp::Publisher::SharedPtr attached_object_pub_; + rclcpp::Publisher::SharedPtr disabled_object_pairs_pub_; + + moveit_msgs::msg::RobotTrajectory robot_trajectory_; + std::vector robot_trajectories_; +}; + +void CollisionDetectingValidatorTest::SetUp() { + rclcpp::NodeOptions options; + options.parameter_overrides() = { + rclcpp::Parameter("robot_description", tmc_manipulation_tests::stanford_manipulator::GetUrdf()), + rclcpp::Parameter("robot_collision_pair", tmc_manipulation_tests::stanford_manipulator::GetCollisionConfig()), + rclcpp::Parameter("obstacle_map_topic_names", std::vector({"static_map", "dynamic_map"}))}; + server_node_ = std::make_shared(options); + server_node_->Init(); + + client_node_ = rclcpp::Node::make_shared("client"); + tf_static_broadcaster_ = std::make_shared(client_node_); + + geometry_msgs::msg::TransformStamped map_to_odom_tf; + map_to_odom_tf.header.frame_id = "map"; + map_to_odom_tf.child_frame_id = "odom"; + map_to_odom_tf.transform.translation.x = 1.0; + tf_static_broadcaster_->sendTransform(map_to_odom_tf); + + // QOS is according to tmc_grid_map_server, tmc_map_merger + static_map_pub_ = client_node_->create_publisher( + "static_map", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + dynamic_map_pub_ = client_node_->create_publisher("dynamic_map", 10); + + collision_object_pub_ = client_node_->create_publisher( + "collision_environment_server/transformed_environment", 1); + attached_object_pub_ = client_node_->create_publisher( + "attached_object_publisher/attached_object", 1); + disabled_object_pairs_pub_ = client_node_->create_publisher( + "validator/collisions_disabled_object_pairs", 1); + + validate_client_ = rclcpp_action::create_client( + client_node_, "validator/validate"); + while (!validate_client_->action_server_is_ready()) { + SpinSome(); + } + robot_trajectory_ = GenerateTestDefaultTrajectory(); + + robot_trajectories_.clear(); + robot_trajectories_.push_back(robot_trajectory_); +} + +// The action is successful and returned without interference +TEST_F(CollisionDetectingValidatorTest, ValidatorSucceeded) { + // uint32_t goal_file_num = GetFileNames(".goal").size(); + // uint32_t env_file_num = GetFileNames(".environment").size(); + + auto result = SendGoalSync(robot_trajectories_, action_msgs::msg::GoalStatus::STATUS_SUCCEEDED); + CheckTrajectory(result->robot_trajectory, robot_trajectory_); + + // // Confirm that it will not be output to the file unless save_request is made to true + // EXPECT_EQ(GetFileNames(".goal").size(), goal_file_num); + // EXPECT_EQ(GetFileNames(".environment").size(), env_file_num); +} + +// The orbit that is not interfered with the interference orbitals that do not interfere is selected +TEST_F(CollisionDetectingValidatorTest, ValidateFeasibleTrajectory) { + // Insert the interference orbit at the top + robot_trajectories_.insert(robot_trajectories_.begin(), GenerateOnePointRobotTrajectory(0.0, 0.0)); + robot_trajectories_[0].joint_trajectory.points[0].positions = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + + auto result = SendGoalSync(robot_trajectories_, action_msgs::msg::GoalStatus::STATUS_SUCCEEDED); + CheckTrajectory(result->robot_trajectory, robot_trajectory_); +} + +// Interference check with static maps +TEST_F(CollisionDetectingValidatorTest, StaticMapCollision) { + const auto map = GenerateMap(); + static_map_pub_->publish(map); + SpinSome(); + + // Leave a place that does not collide + robot_trajectories_[0].multi_dof_joint_trajectory.points[1].transforms[0].translation.x = -1.0; + robot_trajectories_[0].multi_dof_joint_trajectory.points[1].transforms[0].translation.y = -2.0; + + // Judgment that it will not collide because it is outside the map + { + SCOPED_TRACE("Outside of map"); + robot_trajectories_[0].multi_dof_joint_trajectory.points[0].transforms[0].translation.x = 0.1; + robot_trajectories_[0].multi_dof_joint_trajectory.points[0].transforms[0].translation.y = -6.0; + SendGoalSync(robot_trajectories_, action_msgs::msg::GoalStatus::STATUS_SUCCEEDED); + } + + // It is in the map but there are no obstacles, so it does not collide + { + SCOPED_TRACE("Not in collision"); + robot_trajectories_[0].multi_dof_joint_trajectory.points[0].transforms[0].translation.x = -0.1; + robot_trajectories_[0].multi_dof_joint_trajectory.points[0].transforms[0].translation.y = -5.9; + SendGoalSync(robot_trajectories_, action_msgs::msg::GoalStatus::STATUS_SUCCEEDED); + } + + // It collides because it is an area with obstacles + { + SCOPED_TRACE("In collision"); + robot_trajectories_[0].multi_dof_joint_trajectory.points[0].transforms[0].translation.x = -0.1; + robot_trajectories_[0].multi_dof_joint_trajectory.points[0].transforms[0].translation.y = -6.1; + SendGoalSync(robot_trajectories_, action_msgs::msg::GoalStatus::STATUS_ABORTED); + } +} + +// Interference check with dynamic map +TEST_F(CollisionDetectingValidatorTest, DynamicMapCollision) { + // I just want to check if the QOS may be different, so check only when colliding + const auto map = GenerateMap(); + dynamic_map_pub_->publish(map); + SpinSome(); + + // Leave a place that does not collide + robot_trajectories_[0].multi_dof_joint_trajectory.points[1].transforms[0].translation.x = -1.0; + robot_trajectories_[0].multi_dof_joint_trajectory.points[1].transforms[0].translation.y = -2.0; + + // It collides because it is an area with obstacles + robot_trajectories_[0].multi_dof_joint_trajectory.points[0].transforms[0].translation.x = -0.1; + robot_trajectories_[0].multi_dof_joint_trajectory.points[0].transforms[0].translation.y = -6.1; + SendGoalSync(robot_trajectories_, action_msgs::msg::GoalStatus::STATUS_ABORTED); +} + +// The orbital interference with the known object +TEST_F(CollisionDetectingValidatorTest, RobotCollision) { + moveit_msgs::msg::PlanningSceneWorld environment; + environment.collision_objects = {GenerateOneObstacleCube()}; + collision_object_pub_->publish(environment); + SpinSome(); + + SendGoalSync(robot_trajectories_, action_msgs::msg::GoalStatus::STATUS_ABORTED); + + // Out the collision judgment by specifying an object + tmc_planning_msgs::msg::ObjectPairsList object_pairs_list; + object_pairs_list.pairs.resize(2); + object_pairs_list.pairs[0].names = {"link1/collision/0", "object1"}; + object_pairs_list.pairs[1].names = {"link1/collision/1", "object1"}; + disabled_object_pairs_pub_->publish(object_pairs_list); + SpinSome(); + + SendGoalSync(robot_trajectories_, action_msgs::msg::GoalStatus::STATUS_SUCCEEDED); + + // Returns the conflict judgment settings + object_pairs_list.pairs.clear(); + disabled_object_pairs_pub_->publish(object_pairs_list); + SpinSome(); + + SendGoalSync(robot_trajectories_, action_msgs::msg::GoalStatus::STATUS_ABORTED); + + // Extract the collision judgment by specifying the index of the object + object_pairs_list.pairs.resize(2); + object_pairs_list.pairs[0].names = {"link1/collision/0", "object1#0"}; + object_pairs_list.pairs[1].names = {"link1/collision/1", "object1#0"}; + disabled_object_pairs_pub_->publish(object_pairs_list); + SpinSome(); + + SendGoalSync(robot_trajectories_, action_msgs::msg::GoalStatus::STATUS_SUCCEEDED); +} + +// Involved ingredients that are not in the environment are reflected in the collision judgment +TEST_F(CollisionDetectingValidatorTest, AttachedObjectNotInEnvironment) { + moveit_msgs::msg::PlanningSceneWorld environment; + environment.collision_objects = {GenerateOneObstacleCube()}; + // It is a position/size that does not hit the arm, but it hits an ingredient + environment.collision_objects[0].pose.position.y = 0.25; + environment.collision_objects[0].pose.position.z = 1.40; + environment.collision_objects[0].primitives[0].dimensions = {0.05, 0.05, 0.05}; + collision_object_pub_->publish(environment); + SpinSome(); + + SendGoalSync(robot_trajectories_, action_msgs::msg::GoalStatus::STATUS_SUCCEEDED); + + // Added an ingredient + PublishAttachedObject(); + + SendGoalSync(robot_trajectories_, action_msgs::msg::GoalStatus::STATUS_ABORTED); +} + +// The personal knowledge in the environment is reflected in the collision judgment due to the relative position/posture for the knowledge frame. +TEST_F(CollisionDetectingValidatorTest, AttachedObjectInEnvironment) { + moveit_msgs::msg::PlanningSceneWorld environment; + environment.collision_objects = {GenerateOneObstacleCube(), GenerateOneObstacleCube()}; + // Position/size that does not hit the arm + environment.collision_objects[0].pose.position.y = 0.25; + environment.collision_objects[0].pose.position.z = 1.40; + environment.collision_objects[0].primitives[0].dimensions = {0.05, 0.05, 0.05}; + // Move the patch to a ridiculous place + environment.collision_objects[1].id = "object2"; + environment.collision_objects[1].pose.position.z = 10.0; + environment.collision_objects[1].primitives[0].dimensions = {0.05, 0.05, 0.05}; + collision_object_pub_->publish(environment); + SpinSome(); + + SendGoalSync(robot_trajectories_, action_msgs::msg::GoalStatus::STATUS_SUCCEEDED); + + // The relative position/posture for the grip frame is given priority + PublishAttachedObject(); + + SendGoalSync(robot_trajectories_, action_msgs::msg::GoalStatus::STATUS_ABORTED); +} + +// The orbit fails in the sky +TEST_F(CollisionDetectingValidatorTest, EmptyTrajectory) { + tmc_planning_msgs::action::ValidateRobotTrajectories::Goal goal; + + auto future_goal_handle = validate_client_->async_send_goal(goal); + WaitForFutureComplete(future_goal_handle); + + auto goal_handle = future_goal_handle.get(); + EXPECT_FALSE(goal_handle.get()); +} + +} // namespace tmc_collision_detecting_validator + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/tmc_collision_detecting_validator/validator_plugins.xml b/tmc_collision_detecting_validator/validator_plugins.xml new file mode 100644 index 0000000..c089344 --- /dev/null +++ b/tmc_collision_detecting_validator/validator_plugins.xml @@ -0,0 +1,7 @@ + + + simple validator + + diff --git a/tmc_local_path_evaluator/CHANGELOG.rst b/tmc_local_path_evaluator/CHANGELOG.rst new file mode 100644 index 0000000..fb37438 --- /dev/null +++ b/tmc_local_path_evaluator/CHANGELOG.rst @@ -0,0 +1,9 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package tmc_local_path_evaluator +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.0.0 (2024-10-15) +------------------- +* Initial release +* Contributors: Keisuke Takeshita, Satoru Onoda + diff --git a/tmc_local_path_evaluator/CMakeLists.txt b/tmc_local_path_evaluator/CMakeLists.txt new file mode 100644 index 0000000..67a53f0 --- /dev/null +++ b/tmc_local_path_evaluator/CMakeLists.txt @@ -0,0 +1,74 @@ +cmake_minimum_required(VERSION 3.5) +project(tmc_local_path_evaluator) + +find_package(ament_cmake REQUIRED) +find_package(Boost REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(moveit_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(tmc_manipulation_types REQUIRED) +find_package(tmc_manipulation_types_bridge REQUIRED) +find_package(tmc_planning_msgs REQUIRED) +find_package(tmc_robot_kinematics_model REQUIRED) +find_package(tmc_robot_local_planner REQUIRED) +find_package(tmc_robot_local_planner_utils REQUIRED) +find_package(tmc_utils REQUIRED) + +add_library(${PROJECT_NAME} SHARED + src/${PROJECT_NAME}/joint_weights.cpp + src/${PROJECT_NAME}/length_base_score_calculation.cpp + src/${PROJECT_NAME}/local_path_evaluator.cpp + src/${PROJECT_NAME}/soft_joint_constraint_score_calculation.cpp + src/${PROJECT_NAME}/soft_link_constraint_score_calculation.cpp + src/${PROJECT_NAME}/time_base_score_calculation.cpp + src/${PROJECT_NAME}/trajectory_score_calculation_base.cpp + src/${PROJECT_NAME}/utils.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC include ${EIGEN3_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) +ament_target_dependencies(${PROJECT_NAME} moveit_msgs pluginlib rclcpp rclcpp_action tmc_manipulation_types tmc_manipulation_types_bridge tmc_planning_msgs tmc_robot_kinematics_model tmc_robot_local_planner tmc_robot_local_planner_utils tmc_utils) + +add_executable(local_path_evaluator src/${PROJECT_NAME}/node_main.cpp) +target_link_libraries(local_path_evaluator ${PROJECT_NAME}) +target_include_directories(local_path_evaluator PRIVATE include) +ament_target_dependencies(local_path_evaluator rclcpp) + +pluginlib_export_plugin_description_file(tmc_robot_local_planner evaluator_plugins.xml) +pluginlib_export_plugin_description_file(tmc_local_path_evaluator score_evaluation_plugins.xml) + +install(TARGETS ${PROJECT_NAME} local_path_evaluator + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) +install(DIRECTORY include/ + DESTINATION include +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(tmc_manipulation_tests REQUIRED) + + ament_add_gtest(length_base_score_calculation_test test/length_base_score_calculation-test.cpp) + target_link_libraries(length_base_score_calculation_test ${PROJECT_NAME}) + + ament_add_gtest(local_path_evaluator_test test/local_path_evaluator-test.cpp) + target_link_libraries(local_path_evaluator_test ${PROJECT_NAME}) + + ament_add_gtest(soft_joint_constraint_score_calculation_test test/soft_joint_constraint_score_calculation-test.cpp) + target_link_libraries(soft_joint_constraint_score_calculation_test ${PROJECT_NAME}) + + ament_add_gtest(soft_link_constraint_score_calculation_test test/soft_link_constraint_score_calculation-test.cpp) + target_link_libraries(soft_link_constraint_score_calculation_test ${PROJECT_NAME}) + ament_target_dependencies(soft_link_constraint_score_calculation_test tmc_manipulation_tests) + + ament_add_gtest(time_base_score_calculation_test test/time_base_score_calculation-test.cpp) + target_link_libraries(time_base_score_calculation_test ${PROJECT_NAME}) +endif() + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies(moveit_msgs rclcpp rclcpp_action tmc_manipulation_types tmc_manipulation_types_bridge tmc_planning_msgs tmc_robot_local_planner tmc_robot_local_planner_utils tmc_utils) + +ament_package() diff --git a/tmc_local_path_evaluator/NOTICE b/tmc_local_path_evaluator/NOTICE new file mode 100644 index 0000000..581a182 --- /dev/null +++ b/tmc_local_path_evaluator/NOTICE @@ -0,0 +1,20 @@ +Copyright (c) 2012 Jakob Progsch, Václav Zeman + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any damages +arising from the use of this software. + +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it +freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + + 3. This notice may not be removed or altered from any source + distribution. \ No newline at end of file diff --git a/tmc_local_path_evaluator/evaluator_plugins.xml b/tmc_local_path_evaluator/evaluator_plugins.xml new file mode 100644 index 0000000..b1c21b5 --- /dev/null +++ b/tmc_local_path_evaluator/evaluator_plugins.xml @@ -0,0 +1,7 @@ + + + simple evaluator + + \ No newline at end of file diff --git a/tmc_local_path_evaluator/include/tmc_local_path_evaluator/joint_weights.hpp b/tmc_local_path_evaluator/include/tmc_local_path_evaluator/joint_weights.hpp new file mode 100644 index 0000000..b2098b3 --- /dev/null +++ b/tmc_local_path_evaluator/include/tmc_local_path_evaluator/joint_weights.hpp @@ -0,0 +1,67 @@ +/* +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. +*/ +#ifndef TMC_LOCAL_PATH_EVALUATOR_JOINT_WEIGHTS_HPP_ +#define TMC_LOCAL_PATH_EVALUATOR_JOINT_WEIGHTS_HPP_ + +#include +#include +#include +#include + +#include + +#include + +#include +#include + +#include +#include + +namespace tmc_local_path_evaluator { + +class JointWeights { + public: + // I want to return the success or failure, so I define the Initialize function instead of the constructor. + bool Initialize(rclcpp::Node::SharedPtr node, tmc_manipulation_types::BaseMovementType type, + const std::string& parameter_namespace); + boost::optional GetWeightVector( + const tmc_manipulation_types::TimedRobotTrajectory& trajectory) const; + + private: + std::map joint_weights_map_; + std::vector base_weights_; + + void UpdateCallback(const std::shared_ptr request, + std::shared_ptr response); + rclcpp::Service::SharedPtr update_srv_; +}; + +} // namespace tmc_local_path_evaluator + +#endif // TMC_LOCAL_PATH_EVALUATOR_LENGTH_BASE_SCORE_CALCULATION_HPP_ diff --git a/tmc_local_path_evaluator/include/tmc_local_path_evaluator/length_base_score_calculation.hpp b/tmc_local_path_evaluator/include/tmc_local_path_evaluator/length_base_score_calculation.hpp new file mode 100644 index 0000000..8807a6c --- /dev/null +++ b/tmc_local_path_evaluator/include/tmc_local_path_evaluator/length_base_score_calculation.hpp @@ -0,0 +1,75 @@ +/* +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. +*/ +#ifndef TMC_LOCAL_PATH_EVALUATOR_LENGTH_BASE_SCORE_CALCULATION_HPP_ +#define TMC_LOCAL_PATH_EVALUATOR_LENGTH_BASE_SCORE_CALCULATION_HPP_ + +#include +#include +#include + +#include + +#include + +#include "joint_weights.hpp" +#include "trajectory_score_calculation_base.hpp" + +namespace tmc_local_path_evaluator { + +class LengthBaseScoreCalculation : public TrajectoryScoreCalculationBase { + public: + /// @brief constructor + LengthBaseScoreCalculation() {} + + /// @brief Destructor + virtual ~LengthBaseScoreCalculation() = default; + + protected: + /// @brief Initialization + bool InitializeImpl(const rclcpp::Node::SharedPtr& node, + const std::string& parameter_namespace) override; + + /// @brief Score calculation + boost::optional CalculateImpl( + const tmc_manipulation_types::TimedRobotTrajectory& trajectory, + const tmc_robot_local_planner::Constraints& constraint) override; + + private: + boost::optional IntegrateTrajectory(const tmc_manipulation_types::TimedRobotTrajectory& trajectory) const; + + boost::optional SerializeTrajectoryPoint( + const tmc_manipulation_types::TimedJointTrajectoryPoint& joint_point, + const tmc_manipulation_types::TimedMultiDOFJointTrajectoryPoint& base_point) const; + + tmc_manipulation_types::BaseMovementType base_movement_type_; + JointWeights weights_; +}; + +} // namespace tmc_local_path_evaluator + +#endif // TMC_LOCAL_PATH_EVALUATOR_LENGTH_BASE_SCORE_CALCULATION_HPP_ diff --git a/tmc_local_path_evaluator/include/tmc_local_path_evaluator/local_path_evaluator.hpp b/tmc_local_path_evaluator/include/tmc_local_path_evaluator/local_path_evaluator.hpp new file mode 100644 index 0000000..14c6b5d --- /dev/null +++ b/tmc_local_path_evaluator/include/tmc_local_path_evaluator/local_path_evaluator.hpp @@ -0,0 +1,120 @@ +/* +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. +*/ +#ifndef TMC_LOCAL_PATH_EVALUATOR_LOCAL_PATH_EVALUATOR_HPP_ +#define TMC_LOCAL_PATH_EVALUATOR_LOCAL_PATH_EVALUATOR_HPP_ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "trajectory_score_calculation.hpp" + +class ThreadPool; + +namespace tmc_local_path_evaluator { + +// For sorting, to save memory instead of having Trajectory directly, to save memory +struct ScoreWithIndex{ + uint32_t index; + double value; + + explicit ScoreWithIndex(uint32_t _index) : index(_index), value(0.0) {} + + bool operator<(const ScoreWithIndex& right) { + return this->value < right.value; + } +}; + +class LocalPathEvaluatorPlugin : public tmc_robot_local_planner::IEvaluator { + public: + LocalPathEvaluatorPlugin(); + virtual ~LocalPathEvaluatorPlugin() = default; + + void Initialize(const rclcpp::Node::SharedPtr& node) override; + + bool Evaluate(const tmc_robot_local_planner::Constraints& constraints, + const std::vector& trajectories_in, + std::function interrupt, + std::vector& trajectories_out) override; + + private: + void Reset(uint32_t size); + + void CalcTrajectoryScore(std::function interrupt, + const tmc_manipulation_types::TimedRobotTrajectory& trajectory, + const tmc_robot_local_planner::Constraints& constraint, + uint32_t index); + + pluginlib::ClassLoader trajectory_score_calculation_lodaer_; + std::vector trajectory_score_calculation_; + + std::shared_ptr pool_; + int thread_pool_size_; + + std::vector norms_; + std::deque score_results_; + std::mutex mutex_; +}; + +class LocalPathEvaluator : public rclcpp::Node { + public: + LocalPathEvaluator(); + explicit LocalPathEvaluator(const rclcpp::NodeOptions& options); + + // I want to do Shared_from_this, so initialize after instance generation + void Initialize(); + + ~LocalPathEvaluator() = default; + + private: + using ServerGoalHandle = rclcpp_action::ServerGoalHandle; + using ServerGoalHandlePtr = std::shared_ptr; + + rclcpp_action::GoalResponse GoalCallback( + const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal); + rclcpp_action::CancelResponse CancelCallback(const ServerGoalHandlePtr goal_handle); + void FeedbackSetupCallback(ServerGoalHandlePtr goal_handle); + + void Execute(const ServerGoalHandlePtr goal_handle); + + rclcpp_action::Server::SharedPtr server_; + + LocalPathEvaluatorPlugin impl_; +}; + +} // namespace tmc_local_path_evaluator + +#endif // TMC_LOCAL_PATH_EVALUATOR_LOCAL_PATH_EVALUATOR_HPP_ diff --git a/tmc_local_path_evaluator/include/tmc_local_path_evaluator/soft_joint_constraint_score_calculation.hpp b/tmc_local_path_evaluator/include/tmc_local_path_evaluator/soft_joint_constraint_score_calculation.hpp new file mode 100644 index 0000000..cb19a2f --- /dev/null +++ b/tmc_local_path_evaluator/include/tmc_local_path_evaluator/soft_joint_constraint_score_calculation.hpp @@ -0,0 +1,56 @@ +/* +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. +*/ +#ifndef TMC_LOCAL_PATH_EVALUATOR_SOFT_JOINT_CONSTRAINT_SCORE_CALCULATION_HPP_ +#define TMC_LOCAL_PATH_EVALUATOR_SOFT_JOINT_CONSTRAINT_SCORE_CALCULATION_HPP_ + +#include + +#include + +#include "trajectory_score_calculation_base.hpp" + +namespace tmc_local_path_evaluator { + +class SoftJointConstraintScoreCalculation : public TrajectoryScoreCalculationBase { + public: + /// @brief constructor + SoftJointConstraintScoreCalculation() {} + + /// @brief Destructor + virtual ~SoftJointConstraintScoreCalculation() = default; + + protected: + /// @brief Score calculation + boost::optional CalculateImpl( + const tmc_manipulation_types::TimedRobotTrajectory& trajectory, + const tmc_robot_local_planner::Constraints& constraint) override; +}; + +} // namespace tmc_local_path_evaluator + +#endif // TMC_LOCAL_PATH_EVALUATOR_SOFT_JOINT_CONSTRAINT_SCORE_CALCULATION_HPP_ diff --git a/tmc_local_path_evaluator/include/tmc_local_path_evaluator/soft_link_constraint_score_calculation.hpp b/tmc_local_path_evaluator/include/tmc_local_path_evaluator/soft_link_constraint_score_calculation.hpp new file mode 100644 index 0000000..b7d08ea --- /dev/null +++ b/tmc_local_path_evaluator/include/tmc_local_path_evaluator/soft_link_constraint_score_calculation.hpp @@ -0,0 +1,69 @@ +/* +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. +*/ +#ifndef TMC_LOCAL_PATH_EVALUATOR_SOFT_LINK_CONSTRAINT_SCORE_CALCULATION_HPP_ +#define TMC_LOCAL_PATH_EVALUATOR_SOFT_LINK_CONSTRAINT_SCORE_CALCULATION_HPP_ + +#include + +#include + +#include + +#include +#include + +#include "trajectory_score_calculation_base.hpp" + +namespace tmc_local_path_evaluator { + +class SoftLinkConstraintScoreCalculation : public TrajectoryScoreCalculationBase { + public: + /// @brief constructor + SoftLinkConstraintScoreCalculation() + : loader_("tmc_robot_kinematics_model", "tmc_robot_kinematics_model::IRobotKinematicsModel") {} + + /// @brief Destructor + virtual ~SoftLinkConstraintScoreCalculation() = default; + + protected: + /// @brief Initialization + bool InitializeImpl(const rclcpp::Node::SharedPtr& node, + const std::string& parameter_namespace) override; + + /// @brief Score calculation + boost::optional CalculateImpl( + const tmc_manipulation_types::TimedRobotTrajectory& trajectory, + const tmc_robot_local_planner::Constraints& constraint) override; + + private: + pluginlib::ClassLoader loader_; + tmc_robot_kinematics_model::IRobotKinematicsModel::Ptr robot_; +}; +} // namespace tmc_local_path_evaluator + +#endif // TMC_LOCAL_PATH_EVALUATOR_SOFT_LINK_CONSTRAINT_SCORE_CALCULATION_HPP_ diff --git a/tmc_local_path_evaluator/include/tmc_local_path_evaluator/time_base_score_calculation.hpp b/tmc_local_path_evaluator/include/tmc_local_path_evaluator/time_base_score_calculation.hpp new file mode 100644 index 0000000..a3e2e56 --- /dev/null +++ b/tmc_local_path_evaluator/include/tmc_local_path_evaluator/time_base_score_calculation.hpp @@ -0,0 +1,56 @@ +/* +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. +*/ +#ifndef TMC_LOCAL_PATH_EVALUATOR_TIME_BASE_SCORE_CALCULATION_HPP_ +#define TMC_LOCAL_PATH_EVALUATOR_TIME_BASE_SCORE_CALCULATION_HPP_ + +#include + +#include + +#include "trajectory_score_calculation_base.hpp" + +namespace tmc_local_path_evaluator { + +class TimeBaseScoreCalculation : public TrajectoryScoreCalculationBase { + public: + /// @brief constructor + TimeBaseScoreCalculation() {} + + /// @brief Destructor + virtual ~TimeBaseScoreCalculation() = default; + + protected: + /// @brief Score calculation + boost::optional CalculateImpl( + const tmc_manipulation_types::TimedRobotTrajectory& trajectory, + const tmc_robot_local_planner::Constraints& constraint) override; +}; + +} // namespace tmc_local_path_evaluator + +#endif // TMC_LOCAL_PATH_EVALUATOR_TIME_BASE_SCORE_CALCULATION_HPP_ diff --git a/tmc_local_path_evaluator/include/tmc_local_path_evaluator/trajectory_score_calculation.hpp b/tmc_local_path_evaluator/include/tmc_local_path_evaluator/trajectory_score_calculation.hpp new file mode 100644 index 0000000..6025abf --- /dev/null +++ b/tmc_local_path_evaluator/include/tmc_local_path_evaluator/trajectory_score_calculation.hpp @@ -0,0 +1,59 @@ +/* +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. +*/ +#ifndef TMC_LOCAL_PATH_EVALUATOR_TRAJECTORY_SCORE_CALCULATION_HPP_ +#define TMC_LOCAL_PATH_EVALUATOR_TRAJECTORY_SCORE_CALCULATION_HPP_ + +#include +#include + +#include + +#include + +#include +#include + +namespace tmc_local_path_evaluator { + +class ITrajectoryScoreCalculation { + public: + using Ptr = std::shared_ptr; + + virtual ~ITrajectoryScoreCalculation() = default; + + virtual bool Initialize(const rclcpp::Node::SharedPtr& node, + const std::string& parameter_namespace) = 0; + + virtual boost::optional Calculate( + const tmc_manipulation_types::TimedRobotTrajectory& trajectory, + const tmc_robot_local_planner::Constraints& constraint) = 0; +}; + +} // namespace tmc_local_path_evaluator + +#endif // TMC_LOCAL_PATH_EVALUATOR_TRAJECTORY_SCORE_CALCULATION_HPP_ diff --git a/tmc_local_path_evaluator/include/tmc_local_path_evaluator/trajectory_score_calculation_base.hpp b/tmc_local_path_evaluator/include/tmc_local_path_evaluator/trajectory_score_calculation_base.hpp new file mode 100644 index 0000000..583c4c3 --- /dev/null +++ b/tmc_local_path_evaluator/include/tmc_local_path_evaluator/trajectory_score_calculation_base.hpp @@ -0,0 +1,59 @@ +/* +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. +*/ +#ifndef TMC_LOCAL_PATH_EVALUATOR_TRAJECTORY_SCORE_CALCULATION_BASE_HPP_ +#define TMC_LOCAL_PATH_EVALUATOR_TRAJECTORY_SCORE_CALCULATION_BASE_HPP_ + +#include + +#include "trajectory_score_calculation.hpp" + +namespace tmc_local_path_evaluator { +class TrajectoryScoreCalculationBase : public ITrajectoryScoreCalculation { + public: + virtual ~TrajectoryScoreCalculationBase() = default; + + bool Initialize(const rclcpp::Node::SharedPtr& node, + const std::string& parameter_namespace) override; + + boost::optional Calculate(const tmc_manipulation_types::TimedRobotTrajectory& trajectory, + const tmc_robot_local_planner::Constraints& constraint) override; + + protected: + virtual bool InitializeImpl(const rclcpp::Node::SharedPtr& node, + const std::string& parameter_namespace) { return true; } + + virtual boost::optional CalculateImpl( + const tmc_manipulation_types::TimedRobotTrajectory& trajectory, + const tmc_robot_local_planner::Constraints& constraint) = 0; + + private: + double score_weight_; +}; +} // namespace tmc_local_path_evaluator + +#endif // TMC_LOCAL_PATH_EVALUATOR_TRAJECTORY_SCORE_CALCULATION_BASE_HPP_ diff --git a/tmc_local_path_evaluator/package.xml b/tmc_local_path_evaluator/package.xml new file mode 100644 index 0000000..175db7e --- /dev/null +++ b/tmc_local_path_evaluator/package.xml @@ -0,0 +1,36 @@ + + tmc_local_path_evaluator + 2.0.0 + Evaluate local trajectory + + HSR Support + + Keisuke Takeshita + Satoru Onoda + + BSD 3-clause Clear License + + ament_cmake + + eigen + + boost + moveit_msgs + pluginlib + rclcpp + rclcpp_action + tmc_manipulation_types + tmc_manipulation_types_bridge + tmc_planning_msgs + tmc_robot_kinematics_model + tmc_robot_local_planner + tmc_robot_local_planner_utils + tmc_utils + + ament_cmake_gtest + tmc_manipulation_tests + + + ament_cmake + + \ No newline at end of file diff --git a/tmc_local_path_evaluator/score_evaluation_plugins.xml b/tmc_local_path_evaluator/score_evaluation_plugins.xml new file mode 100644 index 0000000..138d8dd --- /dev/null +++ b/tmc_local_path_evaluator/score_evaluation_plugins.xml @@ -0,0 +1,22 @@ + + + length base score calculation + + + soft joint constraint score calculation + + + soft link constraint score calculation + + + time base score calculation + + diff --git a/tmc_local_path_evaluator/src/tmc_local_path_evaluator/ThreadPool.h b/tmc_local_path_evaluator/src/tmc_local_path_evaluator/ThreadPool.h new file mode 100644 index 0000000..0475bdb --- /dev/null +++ b/tmc_local_path_evaluator/src/tmc_local_path_evaluator/ThreadPool.h @@ -0,0 +1,98 @@ +#ifndef THREAD_POOL_H +#define THREAD_POOL_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class ThreadPool { +public: + ThreadPool(size_t); + template + auto enqueue(F&& f, Args&&... args) + -> std::future::type>; + ~ThreadPool(); +private: + // need to keep track of threads so we can join them + std::vector< std::thread > workers; + // the task queue + std::queue< std::function > tasks; + + // synchronization + std::mutex queue_mutex; + std::condition_variable condition; + bool stop; +}; + +// the constructor just launches some amount of workers +inline ThreadPool::ThreadPool(size_t threads) + : stop(false) +{ + for(size_t i = 0;i task; + + { + std::unique_lock lock(this->queue_mutex); + this->condition.wait(lock, + [this]{ return this->stop || !this->tasks.empty(); }); + if(this->stop && this->tasks.empty()) + return; + task = std::move(this->tasks.front()); + this->tasks.pop(); + } + + task(); + } + } + ); +} + +// add new work item to the pool +template +auto ThreadPool::enqueue(F&& f, Args&&... args) + -> std::future::type> +{ + using return_type = typename std::result_of::type; + + auto task = std::make_shared< std::packaged_task >( + std::bind(std::forward(f), std::forward(args)...) + ); + + std::future res = task->get_future(); + { + std::unique_lock lock(queue_mutex); + + // don't allow enqueueing after stopping the pool + if(stop) + throw std::runtime_error("enqueue on stopped ThreadPool"); + + tasks.emplace([task](){ (*task)(); }); + } + condition.notify_one(); + return res; +} + +// the destructor joins all threads +inline ThreadPool::~ThreadPool() +{ + { + std::unique_lock lock(queue_mutex); + stop = true; + } + condition.notify_all(); + for(std::thread &worker: workers) + worker.join(); +} + +#endif diff --git a/tmc_local_path_evaluator/src/tmc_local_path_evaluator/joint_weights.cpp b/tmc_local_path_evaluator/src/tmc_local_path_evaluator/joint_weights.cpp new file mode 100644 index 0000000..78b5826 --- /dev/null +++ b/tmc_local_path_evaluator/src/tmc_local_path_evaluator/joint_weights.cpp @@ -0,0 +1,105 @@ +/* +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. +*/ +#include + +#include + +#include + +namespace tmc_local_path_evaluator { + +bool JointWeights::Initialize(rclcpp::Node::SharedPtr node, tmc_manipulation_types::BaseMovementType type, + const std::string& parameter_namespace) { + const auto joint_names = tmc_utils::GetParameter>( + node, parameter_namespace + ".joint_names", {}); + const auto joint_weights = tmc_utils::GetParameter>( + node, parameter_namespace + ".joint_weights", {}); + if (joint_names.empty() || joint_names.size() != joint_weights.size()) { + RCLCPP_WARN(node->get_logger(), "Parameter joint_weights is invalid"); + return false; + } + for (auto i = 0; i < joint_names.size(); ++i) { + joint_weights_map_.insert(std::make_pair(joint_names[i], joint_weights[i])); + } + + base_weights_ = tmc_utils::GetParameter>(node, parameter_namespace + ".base_weights", {}); + if (base_weights_.size() != tmc_manipulation_types::GetBaseDof(type)) { + RCLCPP_WARN(node->get_logger(), "Inconsistency between base_weights size and base_movement_type Dof"); + return false; + } + update_srv_ = node->create_service( + "~/update_weights", std::bind(&JointWeights::UpdateCallback, this, std::placeholders::_1, std::placeholders::_2)); + return true; +} + +boost::optional +JointWeights::GetWeightVector(const tmc_manipulation_types::TimedRobotTrajectory& trajectory) const { + Eigen::VectorXd arm_weight_eigen(trajectory.joint_trajectory.joint_names.size()); + for (const auto& joint_name : trajectory.joint_trajectory.joint_names | boost::adaptors::indexed()) { + auto weight_it(joint_weights_map_.find(joint_name.value())); + if (weight_it == joint_weights_map_.end()) { + return boost::none; + } else { + arm_weight_eigen[joint_name.index()] = weight_it->second; + } + } + + Eigen::VectorXd base_weight_eigen; + if (trajectory.multi_dof_joint_trajectory.points.front().transforms.empty()) { + base_weight_eigen.resize(0); + } else { + base_weight_eigen.resize(base_weights_.size()); + for (uint32_t i = 0; i < base_weights_.size(); ++i) { + base_weight_eigen[i] = base_weights_[i]; + } + } + + Eigen::VectorXd dst_vector(arm_weight_eigen.size() + base_weight_eigen.size()); + dst_vector << arm_weight_eigen, base_weight_eigen; + return dst_vector; +} + +void JointWeights::UpdateCallback(const std::shared_ptr request, + std::shared_ptr response) { + for (const auto& new_weight : request->arm_weights) { + joint_weights_map_[new_weight.joint_name] = new_weight.weight; + } + if (request->base_weights.size() == base_weights_.size()) { + base_weights_ = request->base_weights; + } + + for (const auto& current_weight : joint_weights_map_) { + tmc_planning_msgs::msg::JointWeight weight; + weight.joint_name = current_weight.first; + weight.weight = current_weight.second; + response->arm_weights.push_back(weight); + } + response->base_weights = base_weights_; +} + +} // namespace tmc_local_path_evaluator diff --git a/tmc_local_path_evaluator/src/tmc_local_path_evaluator/length_base_score_calculation.cpp b/tmc_local_path_evaluator/src/tmc_local_path_evaluator/length_base_score_calculation.cpp new file mode 100644 index 0000000..d50a610 --- /dev/null +++ b/tmc_local_path_evaluator/src/tmc_local_path_evaluator/length_base_score_calculation.cpp @@ -0,0 +1,129 @@ +/* +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. +*/ +#include + +#include +#include + +#include + +#include +#include + +#include + +using tmc_manipulation_types::BaseMovementType; + +namespace tmc_local_path_evaluator { + +bool LengthBaseScoreCalculation::InitializeImpl(const rclcpp::Node::SharedPtr& node, + const std::string& parameter_namespace) { + int base_movement_type = tmc_utils::GetParameter(node, parameter_namespace + ".base_movement_type", -1); + if (base_movement_type < 0) { + RCLCPP_WARN(node->get_logger(), "Parameter base_movement_type is invalid"); + return false; + } + base_movement_type_ = (BaseMovementType)base_movement_type; + + if (!weights_.Initialize(node, base_movement_type_, parameter_namespace)) { + return false; + } + return true; +} + +boost::optional LengthBaseScoreCalculation::CalculateImpl( + const tmc_manipulation_types::TimedRobotTrajectory& trajectory, + const tmc_robot_local_planner::Constraints& constraint) { + return IntegrateTrajectory(trajectory); +} + +boost::optional LengthBaseScoreCalculation::IntegrateTrajectory( + const tmc_manipulation_types::TimedRobotTrajectory& trajectory) const { + if (trajectory.joint_trajectory.points.size() != + trajectory.multi_dof_joint_trajectory.points.size()) { + return boost::none; + } + if (trajectory.joint_trajectory.points.size() < 2) { + return boost::none; + } + + auto weight = weights_.GetWeightVector(trajectory); + if (!weight) { + return boost::none; + } + + auto previous_positions = SerializeTrajectoryPoint(trajectory.joint_trajectory.points.front(), + trajectory.multi_dof_joint_trajectory.points.front()); + if (!previous_positions || previous_positions.value().size() != weight.value().size()) { + return boost::none; + } + + Eigen::VectorXd diff_vector(Eigen::VectorXd::Zero(previous_positions.value().size())); + for (uint32_t i = 1; i < trajectory.joint_trajectory.points.size(); ++i) { + if (weight.value().size() != previous_positions.value().size()) { + return boost::none; + } + auto current_positions = SerializeTrajectoryPoint(trajectory.joint_trajectory.points[i], + trajectory.multi_dof_joint_trajectory.points[i]); + if (!current_positions || current_positions.value().size() != weight.value().size()) { + return boost::none; + } + diff_vector += (current_positions.value() - previous_positions.value()).cwiseAbs(); + previous_positions.value() = current_positions.value(); + } + return Eigen::VectorXd(diff_vector.array() * weight.value().array()).norm(); +} + +boost::optional LengthBaseScoreCalculation::SerializeTrajectoryPoint( + const tmc_manipulation_types::TimedJointTrajectoryPoint& joint_point, + const tmc_manipulation_types::TimedMultiDOFJointTrajectoryPoint& base_point) const { + Eigen::VectorXd joint_positions = joint_point.positions; + + Eigen::VectorXd dst_vector; + if (base_point.transforms.empty()) { + dst_vector = joint_positions; + return dst_vector; + } else if (base_point.transforms.size() == 1 && + base_movement_type_ == tmc_manipulation_types::BaseMovementType::kPlanar) { + dst_vector.resize(joint_point.positions.size() + tmc_manipulation_types::GetBaseDof(base_movement_type_)); + dst_vector << joint_positions, + base_point.transforms[0].translation().x(), + base_point.transforms[0].translation().y(), + Eigen::AngleAxisd(base_point.transforms[0].linear()).angle(); + return dst_vector; + } else { + return boost::none; + } +} + +} // namespace tmc_local_path_evaluator + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(tmc_local_path_evaluator::LengthBaseScoreCalculation, + tmc_local_path_evaluator::ITrajectoryScoreCalculation); diff --git a/tmc_local_path_evaluator/src/tmc_local_path_evaluator/local_path_evaluator.cpp b/tmc_local_path_evaluator/src/tmc_local_path_evaluator/local_path_evaluator.cpp new file mode 100644 index 0000000..253383b --- /dev/null +++ b/tmc_local_path_evaluator/src/tmc_local_path_evaluator/local_path_evaluator.cpp @@ -0,0 +1,236 @@ +/* +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. +*/ +#include + +#include +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include + +#include "ThreadPool.h" + +namespace { +const char* const kScoreCalculationNameSpace = "score_calculations"; +} // namespace + +namespace tmc_local_path_evaluator { + +LocalPathEvaluatorPlugin::LocalPathEvaluatorPlugin() + : trajectory_score_calculation_lodaer_("tmc_local_path_evaluator", + "tmc_local_path_evaluator::ITrajectoryScoreCalculation") {} + +void LocalPathEvaluatorPlugin::Initialize(const rclcpp::Node::SharedPtr& node) { + thread_pool_size_ = tmc_utils::GetParameter(node, "thread_pool_size", 8); + + const auto parameter_namespace = std::string(kScoreCalculationNameSpace); + const auto names = tmc_utils::GetParameter>(node, parameter_namespace + ".names", {}); + for (auto& name : names) { + // Get score calculation class + const std::string plugin_namespace = parameter_namespace + "." + name; + const auto score_calculator_type = tmc_utils::GetParameter(node, plugin_namespace + ".type", ""); + if (score_calculator_type.empty()) { + RCLCPP_WARN(node->get_logger(), "Failed to get %s's score_calculator name.", name.c_str()); + continue; + } + try { + ITrajectoryScoreCalculation::Ptr score_calculator = + trajectory_score_calculation_lodaer_.createSharedInstance(score_calculator_type); + if (!score_calculator->Initialize(node, plugin_namespace)) { + RCLCPP_WARN(node->get_logger(), "Failed to create %s's score calculator.", name.c_str()); + continue; + } + trajectory_score_calculation_.push_back(score_calculator); + } catch (...) { + RCLCPP_WARN(node->get_logger(), "Failed to create %s's score calculator.", name.c_str()); + continue; + } + } + + if (trajectory_score_calculation_.empty()) { + RCLCPP_FATAL(node->get_logger(), "There is no score calculation object."); + exit(EXIT_FAILURE); + } + + pool_.reset(new ThreadPool(thread_pool_size_)); +} + +bool LocalPathEvaluatorPlugin::Evaluate( + const tmc_robot_local_planner::Constraints& constraints, + const std::vector& trajectories_in, + std::function interrupt, + std::vector& trajectories_out) { + Reset(trajectories_in.size()); + + std::vector> results; + for (const auto& trajectory : trajectories_in | boost::adaptors::indexed()) { + results.emplace_back(pool_->enqueue(&LocalPathEvaluatorPlugin::CalcTrajectoryScore, this, + interrupt, + std::ref(trajectory.value()), + std::ref(constraints), trajectory.index())); + } + for (const auto& result : results) { + result.wait(); + } + + if (interrupt()) { + return false; + } + if (std::all_of(score_results_.begin(), score_results_.end(), [](bool x) { return !x; })) { + return false; + } + + trajectories_out.reserve(trajectories_in.size()); + std::sort(norms_.begin(), norms_.end()); + for (const auto& norm : norms_) { + trajectories_out.push_back(trajectories_in[norm.index]); + } + return true; +} + +void LocalPathEvaluatorPlugin::Reset(uint32_t size) { + norms_.clear(); + norms_.reserve(size); + score_results_.clear(); +} + +void LocalPathEvaluatorPlugin::CalcTrajectoryScore( + std::function interrupt, + const tmc_manipulation_types::TimedRobotTrajectory& trajectory, + const tmc_robot_local_planner::Constraints& constraint, + uint32_t index) { + ScoreWithIndex norm(index); + for (const auto& calculation : trajectory_score_calculation_) { + if (interrupt()) { + return; + } + auto score = calculation->Calculate(trajectory, constraint); + if (!score) { + mutex_.lock(); + score_results_.push_back(false); + mutex_.unlock(); + return; + } + norm.value += score.value(); + } + mutex_.lock(); + norms_.push_back(norm); + score_results_.push_back(true); + mutex_.unlock(); +} + + +LocalPathEvaluator::LocalPathEvaluator() : LocalPathEvaluator(rclcpp::NodeOptions()) {} + +LocalPathEvaluator::LocalPathEvaluator(const rclcpp::NodeOptions& options) : Node("evaluator", options) {} + +void LocalPathEvaluator::Initialize() { + const auto node = shared_from_this(); + impl_.Initialize(node); + + server_ = rclcpp_action::create_server( + this, "~/evaluate", + std::bind(&LocalPathEvaluator::GoalCallback, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&LocalPathEvaluator::CancelCallback, this, std::placeholders::_1), + std::bind(&LocalPathEvaluator::FeedbackSetupCallback, this, std::placeholders::_1)); +} + +rclcpp_action::GoalResponse LocalPathEvaluator::GoalCallback( + const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal) { + if (goal->robot_trajectories.empty()) { + return rclcpp_action::GoalResponse::REJECT; + } else { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } +} + +rclcpp_action::CancelResponse LocalPathEvaluator::CancelCallback(const ServerGoalHandlePtr goal_handle) { + return rclcpp_action::CancelResponse::ACCEPT; +} + +void LocalPathEvaluator::FeedbackSetupCallback(ServerGoalHandlePtr goal_handle) { + const auto goal = goal_handle->get_goal(); + + if (goal->robot_trajectories.size() == 1) { + auto result = std::make_shared(); + result->robot_trajectories = goal->robot_trajectories; + goal_handle->succeed(result); + return; + } + std::thread{std::bind(&LocalPathEvaluator::Execute, this, std::placeholders::_1), goal_handle}.detach(); +} + +void LocalPathEvaluator::Execute(const ServerGoalHandlePtr goal_handle) { + const auto goal = goal_handle->get_goal(); + + tmc_robot_local_planner::Constraints constraints; + tmc_robot_local_planner_utils::ConvertConstraints(goal->constraints, constraints); + + std::vector trajectories_in; + trajectories_in.reserve(goal->robot_trajectories.size()); + for (const auto& trajectory_msg : goal->robot_trajectories) { + tmc_manipulation_types::TimedRobotTrajectory trajectory; + tmc_manipulation_types_bridge::RobotTrajectoryMsgToTimedRobotTrajectory(trajectory_msg, trajectory); + trajectories_in.emplace_back(trajectory); + } + + std::vector trajectories_out; + auto interrupt_func = [goal_handle]() { return goal_handle->is_canceling(); }; + const auto eval_result = impl_.Evaluate(constraints, trajectories_in, interrupt_func, trajectories_out); + + auto action_result = std::make_shared(); + if (eval_result) { + action_result->robot_trajectories.reserve(trajectories_in.size()); + for (const auto trajectory : trajectories_out) { + moveit_msgs::msg::RobotTrajectory trajectory_msg; + tmc_manipulation_types_bridge::TimedRobotTrajectoryToRobotTrajectoryMsg(trajectory, trajectory_msg); + action_result->robot_trajectories.emplace_back(trajectory_msg); + } + goal_handle->succeed(action_result); + } else if (goal_handle->is_canceling()) { + goal_handle->canceled(action_result); + } else { + goal_handle->abort(action_result); + } +} + +} // namespace tmc_local_path_evaluator + +#include // NOLINT + +PLUGINLIB_EXPORT_CLASS(tmc_local_path_evaluator::LocalPathEvaluatorPlugin, + tmc_robot_local_planner::IEvaluator) diff --git a/tmc_local_path_evaluator/src/tmc_local_path_evaluator/node_main.cpp b/tmc_local_path_evaluator/src/tmc_local_path_evaluator/node_main.cpp new file mode 100644 index 0000000..05dea3b --- /dev/null +++ b/tmc_local_path_evaluator/src/tmc_local_path_evaluator/node_main.cpp @@ -0,0 +1,39 @@ +/* +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. +*/ + +#include +#include + +int main(int argc, char* argv[]) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + node->Initialize(); + rclcpp::spin(node); + rclcpp::shutdown(); + return EXIT_SUCCESS; +} diff --git a/tmc_local_path_evaluator/src/tmc_local_path_evaluator/soft_joint_constraint_score_calculation.cpp b/tmc_local_path_evaluator/src/tmc_local_path_evaluator/soft_joint_constraint_score_calculation.cpp new file mode 100644 index 0000000..831802d --- /dev/null +++ b/tmc_local_path_evaluator/src/tmc_local_path_evaluator/soft_joint_constraint_score_calculation.cpp @@ -0,0 +1,61 @@ +/* +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. +*/ + +#include + +#include "utils.hpp" + +namespace tmc_local_path_evaluator { + +boost::optional SoftJointConstraintScoreCalculation::CalculateImpl( + const tmc_manipulation_types::TimedRobotTrajectory& trajectory, + const tmc_robot_local_planner::Constraints& constraint) { + if (trajectory.joint_trajectory.points.empty()) { + return boost::none; + } + if (trajectory.joint_trajectory.points.size() != trajectory.multi_dof_joint_trajectory.points.size()) { + return boost::none; + } + + // Soft_joint_constraints says Path Constraints (Soft), but the original implementation was Goal. + // Furthermore, in the current naming regularly, Soft_joint_constraints seems better to mean Goal. + const auto state = ExtractLastState(trajectory); + + double score = 0.0; + for (const auto& joint_constraint : constraint.soft_joint_constraints) { + score += joint_constraint->CalcDisplacement(state); + } + return score; +} + +} // namespace tmc_local_path_evaluator + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(tmc_local_path_evaluator::SoftJointConstraintScoreCalculation, + tmc_local_path_evaluator::ITrajectoryScoreCalculation); diff --git a/tmc_local_path_evaluator/src/tmc_local_path_evaluator/soft_link_constraint_score_calculation.cpp b/tmc_local_path_evaluator/src/tmc_local_path_evaluator/soft_link_constraint_score_calculation.cpp new file mode 100644 index 0000000..1c79ca5 --- /dev/null +++ b/tmc_local_path_evaluator/src/tmc_local_path_evaluator/soft_link_constraint_score_calculation.cpp @@ -0,0 +1,89 @@ +/* +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. +*/ + +#include + +#include +#include + +#include "utils.hpp" + +namespace tmc_local_path_evaluator { + +bool SoftLinkConstraintScoreCalculation::InitializeImpl(const rclcpp::Node::SharedPtr& node, + const std::string& parameter_namespace) { + try { + const auto kinematics_type = tmc_utils::GetParameter(node, "kinematics_type", ""); + if (kinematics_type.empty()) { + robot_ = std::make_shared(); + } else { + robot_ = loader_.createSharedInstance(kinematics_type); + } + + auto robot_description = tmc_utils::GetParameter(node, "robot_description_kinematics", ""); + if (robot_description.empty()) { + robot_description = tmc_utils::GetParameter(node, "robot_description", ""); + } + robot_->Initialize(robot_description); + } catch (const std::exception& e) { + RCLCPP_FATAL(node->get_logger(), e.what()); + return false; + } + + return true; +} + +boost::optional SoftLinkConstraintScoreCalculation::CalculateImpl( + const tmc_manipulation_types::TimedRobotTrajectory& trajectory, + const tmc_robot_local_planner::Constraints& constraint) { + if (trajectory.joint_trajectory.points.empty()) { + return boost::none; + } + if (trajectory.joint_trajectory.points.size() != trajectory.multi_dof_joint_trajectory.points.size()) { + return boost::none; + } + + const auto state = ExtractLastState(trajectory); + robot_->SetNamedAngle(state.joint_state); + // I'm doing it in DoCoMo [0], so I'll do that too, but I want to do something + robot_->SetRobotTransform(state.multi_dof_joint_state.poses[0]); + + double score = 0.0; + for (const auto& link_constraint : constraint.soft_link_constraints) { + const auto origin_to_end = robot_->GetObjectTransform(link_constraint->GetLinkName()); + score += link_constraint->CalcDisplacement(origin_to_end); + } + return score; +} + +} // namespace tmc_local_path_evaluator + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(tmc_local_path_evaluator::SoftLinkConstraintScoreCalculation, + tmc_local_path_evaluator::ITrajectoryScoreCalculation); diff --git a/tmc_local_path_evaluator/src/tmc_local_path_evaluator/time_base_score_calculation.cpp b/tmc_local_path_evaluator/src/tmc_local_path_evaluator/time_base_score_calculation.cpp new file mode 100644 index 0000000..3e9ae61 --- /dev/null +++ b/tmc_local_path_evaluator/src/tmc_local_path_evaluator/time_base_score_calculation.cpp @@ -0,0 +1,54 @@ +/* +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. +*/ +#include + +namespace tmc_local_path_evaluator { + +boost::optional TimeBaseScoreCalculation::CalculateImpl( + const tmc_manipulation_types::TimedRobotTrajectory& trajectory, + const tmc_robot_local_planner::Constraints& constraint) { + if (trajectory.joint_trajectory.points.size() != trajectory.multi_dof_joint_trajectory.points.size()) { + return boost::none; + } + if (trajectory.joint_trajectory.points.empty()) { + return boost::none; + } + // Double type == judgment is not good, but due to the nature of Time_from_start, it should be expected that the same value is strictly included. + if (trajectory.joint_trajectory.points.back().time_from_start != + trajectory.multi_dof_joint_trajectory.points.back().time_from_start) { + return boost::none; + } + return trajectory.joint_trajectory.points.back().time_from_start; +} + +} // namespace tmc_local_path_evaluator + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(tmc_local_path_evaluator::TimeBaseScoreCalculation, + tmc_local_path_evaluator::ITrajectoryScoreCalculation); diff --git a/tmc_local_path_evaluator/src/tmc_local_path_evaluator/trajectory_score_calculation_base.cpp b/tmc_local_path_evaluator/src/tmc_local_path_evaluator/trajectory_score_calculation_base.cpp new file mode 100644 index 0000000..e433d7e --- /dev/null +++ b/tmc_local_path_evaluator/src/tmc_local_path_evaluator/trajectory_score_calculation_base.cpp @@ -0,0 +1,51 @@ +/* +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. +*/ + +#include + +#include + +namespace tmc_local_path_evaluator { + +bool TrajectoryScoreCalculationBase::Initialize(const rclcpp::Node::SharedPtr& node, + const std::string& parameter_namespace) { + score_weight_ = tmc_utils::GetParameter(node, parameter_namespace + ".score_weight", 1.0); + return InitializeImpl(node, parameter_namespace); +} + +boost::optional TrajectoryScoreCalculationBase::Calculate( + const tmc_manipulation_types::TimedRobotTrajectory& trajectory, + const tmc_robot_local_planner::Constraints& constraint) { + auto score = CalculateImpl(trajectory, constraint); + if (!score) { + return boost::none; + } + return score.value() * score_weight_; +} + +} // namespace tmc_local_path_evaluator diff --git a/tmc_local_path_evaluator/src/tmc_local_path_evaluator/utils.cpp b/tmc_local_path_evaluator/src/tmc_local_path_evaluator/utils.cpp new file mode 100644 index 0000000..067f292 --- /dev/null +++ b/tmc_local_path_evaluator/src/tmc_local_path_evaluator/utils.cpp @@ -0,0 +1,42 @@ +/* +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. +*/ + +#include "utils.hpp" + +namespace tmc_local_path_evaluator { + +tmc_manipulation_types::RobotState ExtractLastState(const tmc_manipulation_types::TimedRobotTrajectory& trajectory) { + tmc_manipulation_types::RobotState state; + state.joint_state.name = trajectory.joint_trajectory.joint_names; + state.joint_state.position = trajectory.joint_trajectory.points.back().positions; + state.multi_dof_joint_state.names = trajectory.multi_dof_joint_trajectory.joint_names; + state.multi_dof_joint_state.poses = trajectory.multi_dof_joint_trajectory.points.back().transforms; + return state; +} + +} // namespace tmc_local_path_evaluator diff --git a/tmc_local_path_evaluator/src/tmc_local_path_evaluator/utils.hpp b/tmc_local_path_evaluator/src/tmc_local_path_evaluator/utils.hpp new file mode 100644 index 0000000..2496beb --- /dev/null +++ b/tmc_local_path_evaluator/src/tmc_local_path_evaluator/utils.hpp @@ -0,0 +1,39 @@ +/* +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. +*/ +#ifndef TMC_LOCAL_PATH_EVALUATOR_SRC_UTILS_HPP_ +#define TMC_LOCAL_PATH_EVALUATOR_SRC_UTILS_HPP_ + +#include + +namespace tmc_local_path_evaluator { + +tmc_manipulation_types::RobotState ExtractLastState(const tmc_manipulation_types::TimedRobotTrajectory& trajectory); + +} // namespace tmc_local_path_evaluator + +#endif // TMC_LOCAL_PATH_EVALUATOR_SRC_UTILS_HPP_ diff --git a/tmc_local_path_evaluator/test/length_base_score_calculation-test.cpp b/tmc_local_path_evaluator/test/length_base_score_calculation-test.cpp new file mode 100644 index 0000000..130ce14 --- /dev/null +++ b/tmc_local_path_evaluator/test/length_base_score_calculation-test.cpp @@ -0,0 +1,234 @@ +/* +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. +*/ + +#include +#include + +#include + +#include +#include + +#include + +#include +#include + +#include "test_util.hpp" + +namespace { +const std::vector kJointNames = {"arm", "wrist", "head"}; +const std::vector kJointWeights = {5.0, 1.0, 0.5}; +const std::vector kBaseWeights = {5.0, 5.0, 1.0}; + +rclcpp::NodeOptions RemoveParameterOverride(const rclcpp::NodeOptions& input_options, + const std::string& parameter_name) { + rclcpp::NodeOptions output_options; + for (const auto& parameter : input_options.parameter_overrides()) { + if (parameter.get_name() != parameter_name) { + output_options.parameter_overrides().push_back(parameter); + } + } + return output_options; +} + +template +rclcpp::NodeOptions UpdateParameterOverride(const rclcpp::NodeOptions& input_options, + const std::string& parameter_name, + const TYPE& value) { + rclcpp::NodeOptions output_options; + for (const auto& parameter : input_options.parameter_overrides()) { + if (parameter.get_name() == parameter_name) { + output_options.parameter_overrides().push_back(rclcpp::Parameter(parameter_name, value)); + } else { + output_options.parameter_overrides().push_back(parameter); + } + } + return output_options; +} +} // namespace + +namespace tmc_local_path_evaluator { + +class LengthBaseScoreCalculationTest : public ::testing::Test { + protected: + void SetUp() override; + + ITrajectoryScoreCalculation::Ptr length_base_score_calculation_; + + rclcpp::NodeOptions default_options_; + rclcpp::Node::SharedPtr server_node_; + + tmc_manipulation_types::TimedRobotTrajectory robot_trajectory_; +}; + +void LengthBaseScoreCalculationTest::SetUp() { + length_base_score_calculation_ = std::make_shared(); + + default_options_.parameter_overrides() = {rclcpp::Parameter("server.base_movement_type", 1), + rclcpp::Parameter("server.score_weight", 2.0), + rclcpp::Parameter("server.joint_names", kJointNames), + rclcpp::Parameter("server.joint_weights", kJointWeights), + rclcpp::Parameter("server.base_weights", kBaseWeights)}; + server_node_ = rclcpp::Node::make_shared("server", default_options_); + + std::vector> point_positions = { {0.0, 0.0, 0.0}, + {0.1, 0.2, 0.3}, + {0.4, 0.5, 0.6} }; + geometry_msgs::msg::Transform transform; + transform.rotation.w = 1.0; + std::vector transforms; + for (uint32_t i = 0; i < point_positions.size(); ++i) { + transform.translation.x = 0.1 * i; + transform.translation.y = 0.2 * i; + transforms.push_back(transform); + } + tmc_manipulation_types_bridge::RobotTrajectoryMsgToTimedRobotTrajectory( + CreateRobotTrajectory(kJointNames, point_positions, transforms), robot_trajectory_); +} + +TEST(LengthBaseScoreCalculationPluginTest, CreateInstance) { + pluginlib::ClassLoader class_loader( + "tmc_local_path_evaluator", "tmc_local_path_evaluator::ITrajectoryScoreCalculation"); + auto calculator = class_loader.createSharedInstance("tmc_local_path_evaluator/LengthBaseScoreCalculation"); +} + +TEST_F(LengthBaseScoreCalculationTest, NotFoundBaseMovementType) { + server_node_ = rclcpp::Node::make_shared( + "server", RemoveParameterOverride(default_options_, "server.base_movement_type")); + ASSERT_FALSE(length_base_score_calculation_->Initialize(server_node_, "server")); +} + +TEST_F(LengthBaseScoreCalculationTest, NotFoundJointNames) { + server_node_ = rclcpp::Node::make_shared( + "server", RemoveParameterOverride(default_options_, "server.joint_names")); + ASSERT_FALSE(length_base_score_calculation_->Initialize(server_node_, "server")); +} + +TEST_F(LengthBaseScoreCalculationTest, NotFoundJointWeights) { + server_node_ = rclcpp::Node::make_shared( + "server", RemoveParameterOverride(default_options_, "server.joint_weights")); + ASSERT_FALSE(length_base_score_calculation_->Initialize(server_node_, "server")); +} + +TEST_F(LengthBaseScoreCalculationTest, NotFoundBaseWeights) { + server_node_ = rclcpp::Node::make_shared( + "server", RemoveParameterOverride(default_options_, "server.base_weights")); + ASSERT_FALSE(length_base_score_calculation_->Initialize(server_node_, "server")); +} + +TEST_F(LengthBaseScoreCalculationTest, MismatchBaseMovementType) { + server_node_ = rclcpp::Node::make_shared( + "server", UpdateParameterOverride(default_options_, "server.base_movement_type", 0)); + ASSERT_FALSE(length_base_score_calculation_->Initialize(server_node_, "server")); +} + +TEST_F(LengthBaseScoreCalculationTest, Calculate) { + ASSERT_TRUE(length_base_score_calculation_->Initialize(server_node_, "server")); + auto score = length_base_score_calculation_->Calculate(robot_trajectory_, tmc_robot_local_planner::Constraints()); + EXPECT_TRUE(static_cast(score)); + EXPECT_NEAR(score.value(), 6.112, 0.01); +} + +TEST_F(LengthBaseScoreCalculationTest, CalculateByChangeJointWeights) { + server_node_ = rclcpp::Node::make_shared( + "server", UpdateParameterOverride>(default_options_, "server.joint_weights", + {2.0, 2.0, 2.0})); + + ASSERT_TRUE(length_base_score_calculation_->Initialize(server_node_, "server")); + auto score = length_base_score_calculation_->Calculate(robot_trajectory_, tmc_robot_local_planner::Constraints()); + EXPECT_TRUE(static_cast(score)); + EXPECT_NEAR(score.value(), 5.685, 0.01); +} + +TEST_F(LengthBaseScoreCalculationTest, CalculateByChangeBaseWeights) { + server_node_ = rclcpp::Node::make_shared( + "server", UpdateParameterOverride>(default_options_, "server.base_weights", {2.0, 2.0, 2.0})); + + ASSERT_TRUE(length_base_score_calculation_->Initialize(server_node_, "server")); + auto score = length_base_score_calculation_->Calculate(robot_trajectory_, tmc_robot_local_planner::Constraints()); + EXPECT_TRUE(static_cast(score)); + EXPECT_NEAR(score.value(), 4.534, 0.01); +} + +TEST_F(LengthBaseScoreCalculationTest, CalculateByChangeScoreWeights) { + server_node_ = rclcpp::Node::make_shared( + "server", UpdateParameterOverride(default_options_, "server.score_weight", 3.0)); + + ASSERT_TRUE(length_base_score_calculation_->Initialize(server_node_, "server")); + auto score = length_base_score_calculation_->Calculate(robot_trajectory_, tmc_robot_local_planner::Constraints()); + EXPECT_TRUE(static_cast(score)); + EXPECT_NEAR(score.value(), 9.168, 0.01); +} + +TEST_F(LengthBaseScoreCalculationTest, MismatchPointsSize) { + robot_trajectory_.joint_trajectory.points.pop_back(); + + ASSERT_TRUE(length_base_score_calculation_->Initialize(server_node_, "server")); + auto score = length_base_score_calculation_->Calculate(robot_trajectory_, tmc_robot_local_planner::Constraints()); + EXPECT_FALSE(static_cast(score)); +} + +TEST_F(LengthBaseScoreCalculationTest, OnePointTrajectory) { + robot_trajectory_.joint_trajectory.points.erase( + robot_trajectory_.joint_trajectory.points.begin() + 1, + robot_trajectory_.joint_trajectory.points.end()); + robot_trajectory_.multi_dof_joint_trajectory.points.erase( + robot_trajectory_.multi_dof_joint_trajectory.points.begin() + 1, + robot_trajectory_.multi_dof_joint_trajectory.points.end()); + + ASSERT_TRUE(length_base_score_calculation_->Initialize(server_node_, "server")); + auto score = length_base_score_calculation_->Calculate(robot_trajectory_, tmc_robot_local_planner::Constraints()); + EXPECT_FALSE(static_cast(score)); +} + +TEST_F(LengthBaseScoreCalculationTest, InvalidJointWeightMap) { + auto options = UpdateParameterOverride>(default_options_, "server.joint_names", {"hoge"}); + options = UpdateParameterOverride>(options, "server.joint_weights", {2.0}); + server_node_ = rclcpp::Node::make_shared("server", options); + + ASSERT_TRUE(length_base_score_calculation_->Initialize(server_node_, "server")); + auto score = length_base_score_calculation_->Calculate(robot_trajectory_, tmc_robot_local_planner::Constraints()); + EXPECT_FALSE(static_cast(score)); +} + +TEST_F(LengthBaseScoreCalculationTest, InvalidTransforms) { + robot_trajectory_.multi_dof_joint_trajectory.points[0].transforms.push_back(Eigen::Affine3d::Identity()); + + ASSERT_TRUE(length_base_score_calculation_->Initialize(server_node_, "server")); + auto score = length_base_score_calculation_->Calculate(robot_trajectory_, tmc_robot_local_planner::Constraints()); + EXPECT_FALSE(static_cast(score)); +} + +} // namespace tmc_local_path_evaluator + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/tmc_local_path_evaluator/test/local_path_evaluator-test.cpp b/tmc_local_path_evaluator/test/local_path_evaluator-test.cpp new file mode 100644 index 0000000..7506f1a --- /dev/null +++ b/tmc_local_path_evaluator/test/local_path_evaluator-test.cpp @@ -0,0 +1,377 @@ +/* +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. +*/ + +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include + +#include "test_util.hpp" + +namespace { +const std::vector kJointNames = {"arm", "wrist", "head"}; +} // namespace + +namespace tmc_local_path_evaluator { + +// Differences in behavior due to differences in parameters are performed by node testing. +TEST(LocalPathEvaluatorPluginTest, Evaluate) { + pluginlib::ClassLoader loader( + "tmc_robot_local_planner", "tmc_robot_local_planner::IEvaluator"); + auto evaluator = loader.createSharedInstance("tmc_local_path_evaluator/LocalPathEvaluatorPlugin"); + + auto node = rclcpp::Node::make_shared("plugins"); + node->declare_parameter("score_calculations.names", std::vector({"length_base"})); + node->declare_parameter("score_calculations.length_base.type", "tmc_local_path_evaluator/LengthBaseScoreCalculation"); + node->declare_parameter("score_calculations.length_base.base_movement_type", 1); + node->declare_parameter("score_calculations.length_base.joint_names", kJointNames); + node->declare_parameter("score_calculations.length_base.joint_weights", std::vector({5.0, 1.0, 0.5})); + node->declare_parameter("score_calculations.length_base.base_weights", std::vector({5.0, 5.0, 1.0})); + evaluator->Initialize(node); + + std::vector trajectories_in; + trajectories_in.resize(2); + + std::vector> point_positions = { {0.0, 0.0, 0.0}, + {0.1, 0.2, 0.3}, + {0.4, 0.5, 0.6} }; + geometry_msgs::msg::Transform transform; + transform.rotation.w = 1.0; + std::vector transforms; + for (uint32_t i = 0; i < point_positions.size(); ++i) { + transform.translation.x = 0.1 * i; + transform.translation.y = 0.2 * i; + transforms.push_back(transform); + } + tmc_manipulation_types_bridge::RobotTrajectoryMsgToTimedRobotTrajectory( + CreateRobotTrajectory(kJointNames, point_positions, transforms), trajectories_in[0]); + tmc_manipulation_types_bridge::RobotTrajectoryMsgToTimedRobotTrajectory( + CreateRobotTrajectory(kJointNames, point_positions, transforms), trajectories_in[1]); + + std::vector trajectories_out; + std::function func = []() -> bool{ return false; }; + EXPECT_TRUE(evaluator->Evaluate(tmc_robot_local_planner::Constraints(), + trajectories_in, func, trajectories_out)); + EXPECT_EQ(trajectories_out.size(), 2); +} + + +class LocalPathEvaluatorTest : public ::testing::Test { + protected: + void SetUp() override; + void TearDown() override; + + void SpinSome() { + rclcpp::spin_some(client_node_); + rclcpp::spin_some(server_node_); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + template + void WaitForFutureComplete(const FutureT& future) { + while (future.wait_for(std::chrono::microseconds(1)) == std::future_status::timeout) { + SpinSome(); + } + } + + tmc_planning_msgs::action::EvaluateRobotTrajectories::Result::SharedPtr SendGoalSync( + const std::vector& robot_trajectory, int8_t expected) { + tmc_planning_msgs::action::EvaluateRobotTrajectories::Goal goal; + goal.robot_trajectories = robot_trajectory; + + auto future_goal_handle = evaluate_client_->async_send_goal(goal); + WaitForFutureComplete(future_goal_handle); + + auto goal_handle = future_goal_handle.get(); + EXPECT_TRUE(goal_handle.get()); + + auto future_result = evaluate_client_->async_get_result(goal_handle); + WaitForFutureComplete(future_result); + + auto wrapped_result = future_result.get(); + EXPECT_EQ(static_cast(wrapped_result.code), expected); + EXPECT_TRUE(wrapped_result.result); + return wrapped_result.result; + } + + rclcpp::Node::SharedPtr client_node_; + std::shared_ptr server_node_; + + std::vector>> joint_torajectories_; + std::vector robot_trajectories_; + + rclcpp::Client::SharedPtr update_weight_client_; + tmc_planning_msgs::srv::UpdateJointWeights::Request::SharedPtr default_weight_req_; + + rclcpp_action::Client::SharedPtr evaluate_client_; +}; + +void LocalPathEvaluatorTest::SetUp() { + rclcpp::NodeOptions options; + options.parameter_overrides() = { + rclcpp::Parameter("score_calculations.names", std::vector({"length_base"})), + rclcpp::Parameter("score_calculations.length_base.type", "tmc_local_path_evaluator/LengthBaseScoreCalculation"), + rclcpp::Parameter("score_calculations.length_base.base_movement_type", 1), + rclcpp::Parameter("score_calculations.length_base.score_weight", 1.0), + rclcpp::Parameter("score_calculations.length_base.joint_names", kJointNames), + rclcpp::Parameter("score_calculations.length_base.joint_weights", std::vector({5.0, 1.0, 0.5})), + rclcpp::Parameter("score_calculations.length_base.base_weights", std::vector({5.0, 5.0, 1.0}))}; + server_node_ = std::make_shared(options); + server_node_->Initialize(); + + client_node_ = rclcpp::Node::make_shared("client"); + + update_weight_client_ = client_node_->create_client( + "evaluator/update_weights"); + while (!update_weight_client_->service_is_ready()) { + SpinSome(); + } + + evaluate_client_ = rclcpp_action::create_client( + client_node_, "evaluator/evaluate"); + while (!evaluate_client_->action_server_is_ready()) { + SpinSome(); + } + + joint_torajectories_.clear(); + for (uint32_t i = 0; i < 3; ++i) { + std::vector> point_positions = + {{0.0, 0.0, 0.0}, + {0.1 + i, 0.2 + i, 0.3 + i}, + {0.4 + (i * 2), 0.5 + (i * 2), 0.6 + (i * 2)}}; + joint_torajectories_.push_back(point_positions); + } + + geometry_msgs::msg::Transform transform; + transform.translation.x = 0.0; + transform.translation.y = 0.0; + transform.rotation.w = 1.0; + std::vector transforms; + for (uint32_t i = 0; i < joint_torajectories_[0].size(); ++i) { + transforms.push_back(transform); + } + robot_trajectories_.clear(); + for (const auto& point_positions : joint_torajectories_) { + robot_trajectories_.push_back(CreateRobotTrajectory(kJointNames, point_positions, transforms)); + } + + auto request = std::make_shared(); + auto future_response = update_weight_client_->async_send_request(request); + WaitForFutureComplete(future_response); + auto response = future_response.get(); + + default_weight_req_ = std::make_shared(); + default_weight_req_->arm_weights = response->arm_weights; + default_weight_req_->base_weights = response->base_weights; +} + +void LocalPathEvaluatorTest::TearDown() { + auto future_response = update_weight_client_->async_send_request(default_weight_req_); + WaitForFutureComplete(future_response); +} + +// 3 joints 3 points 3 track score calculation .lengthbase only +TEST_F(LocalPathEvaluatorTest, ThreeTrajectories) { + auto result = SendGoalSync(robot_trajectories_, action_msgs::msg::GoalStatus::STATUS_SUCCEEDED); + + EXPECT_EQ(result->robot_trajectories.size(), 3); + for (const auto& trajectory : result->robot_trajectories | boost::adaptors::indexed()) { + EXPECT_EQ(trajectory.value().joint_trajectory.points.size(), 3); + EXPECT_EQ(trajectory.value().multi_dof_joint_trajectory.points.size(), 3); + for (const auto& point : trajectory.value().joint_trajectory.points | boost::adaptors::indexed()) { + for (uint32_t i = 0; i < trajectory.value().joint_trajectory.joint_names.size(); ++i) { + EXPECT_NEAR(point.value().positions[i], joint_torajectories_[trajectory.index()][point.index()][i], 0.01); + } + } + } +} + +// Change the weight of the arm joint +TEST_F(LocalPathEvaluatorTest, UpdateArmWeight) { + robot_trajectories_[0].joint_trajectory.points[0].positions[2] = 0.0; + robot_trajectories_[1].joint_trajectory.points[0].positions[2] = 1.1; + robot_trajectories_[2].joint_trajectory.points[0].positions[2] = 2.2; + robot_trajectories_[0].joint_trajectory.points[2].positions[2] = 0.6; + robot_trajectories_[1].joint_trajectory.points[2].positions[2] = 1.5; + robot_trajectories_[2].joint_trajectory.points[2].positions[2] = 2.4; + + auto result = SendGoalSync(robot_trajectories_, action_msgs::msg::GoalStatus::STATUS_SUCCEEDED); + auto robot_trajectories = result->robot_trajectories; + EXPECT_NEAR(robot_trajectories[0].joint_trajectory.points[0].positions[2], 0.0, 0.01); + EXPECT_NEAR(robot_trajectories[1].joint_trajectory.points[0].positions[2], 1.1, 0.01); + EXPECT_NEAR(robot_trajectories[2].joint_trajectory.points[0].positions[2], 2.2, 0.01); + + auto request = std::make_shared(); + request->arm_weights.resize(3); + request->arm_weights[0].joint_name = "arm"; + request->arm_weights[0].weight = 1e-9; + request->arm_weights[1].joint_name = "wrist"; + request->arm_weights[1].weight = 1e-9; + request->arm_weights[2].joint_name = "head"; + request->arm_weights[2].weight = 1.0; + + auto future_response = update_weight_client_->async_send_request(request); + WaitForFutureComplete(future_response); + + result = SendGoalSync(robot_trajectories_, action_msgs::msg::GoalStatus::STATUS_SUCCEEDED); + robot_trajectories = result->robot_trajectories; + EXPECT_NEAR(robot_trajectories[0].joint_trajectory.points[0].positions[2], 2.2, 0.01); + EXPECT_NEAR(robot_trajectories[1].joint_trajectory.points[0].positions[2], 1.1, 0.01); + EXPECT_NEAR(robot_trajectories[2].joint_trajectory.points[0].positions[2], 0.0, 0.01); +} + +// Change the weight of the bogie +TEST_F(LocalPathEvaluatorTest, UpdateBaseWeight) { + robot_trajectories_[0].multi_dof_joint_trajectory.points[0].transforms[0].translation.x = 0.3; + robot_trajectories_[1].multi_dof_joint_trajectory.points[0].transforms[0].translation.x = 0.2; + robot_trajectories_[2].multi_dof_joint_trajectory.points[0].transforms[0].translation.x = 0.1; + + auto result = SendGoalSync(robot_trajectories_, action_msgs::msg::GoalStatus::STATUS_SUCCEEDED); + auto robot_trajectories = result->robot_trajectories; + EXPECT_NEAR(robot_trajectories[0].joint_trajectory.points[2].positions[2], 0.6, 0.01); + EXPECT_NEAR(robot_trajectories[1].joint_trajectory.points[2].positions[2], 2.6, 0.01); + EXPECT_NEAR(robot_trajectories[2].joint_trajectory.points[2].positions[2], 4.6, 0.01); + + auto request = std::make_shared(); + request->base_weights = {1000.0, 1.0, 1.0}; + + auto future_response = update_weight_client_->async_send_request(request); + WaitForFutureComplete(future_response); + + result = SendGoalSync(robot_trajectories_, action_msgs::msg::GoalStatus::STATUS_SUCCEEDED); + robot_trajectories = result->robot_trajectories; + EXPECT_NEAR(robot_trajectories[0].joint_trajectory.points[2].positions[2], 4.6, 0.01); + EXPECT_NEAR(robot_trajectories[1].joint_trajectory.points[2].positions[2], 2.6, 0.01); + EXPECT_NEAR(robot_trajectories[2].joint_trajectory.points[2].positions[2], 0.6, 0.01); +} + +// 1 In the case of orbit input, return it as it is +TEST_F(LocalPathEvaluatorTest, OneTrajectory) { + robot_trajectories_.resize(1); + auto result = SendGoalSync(robot_trajectories_, action_msgs::msg::GoalStatus::STATUS_SUCCEEDED); + + EXPECT_EQ(result->robot_trajectories.size(), 1); + EXPECT_EQ(result->robot_trajectories[0].joint_trajectory.points.size(), 3); + EXPECT_EQ(result->robot_trajectories[0].multi_dof_joint_trajectory.points.size(), 3); + for (const auto& point : result->robot_trajectories[0].joint_trajectory.points | boost::adaptors::indexed()) { + for (uint32_t i = 0; i < result->robot_trajectories[0].joint_trajectory.joint_names.size(); ++i) { + EXPECT_DOUBLE_EQ(point.value().positions[i], joint_torajectories_[0][point.index()][i]); + } + } +} + +// Failure if the orbit is empty +TEST_F(LocalPathEvaluatorTest, EmptyTrajectories) { + tmc_planning_msgs::action::EvaluateRobotTrajectories::Goal goal; + + auto future_goal_handle = evaluate_client_->async_send_goal(goal); + WaitForFutureComplete(future_goal_handle); + + auto goal_handle = future_goal_handle.get(); + EXPECT_FALSE(goal_handle.get()); +} + +// 3 Failure if you could not calculate the score in all orbit +TEST_F(LocalPathEvaluatorTest, InvalidAllTrajectories) { + geometry_msgs::msg::Transform transform; + transform.translation.x = 2.0; + transform.translation.y = 3.0; + transform.rotation.w = 1.0; + for (uint32_t i = 0; i < 3; ++i) { + for (auto& point : robot_trajectories_[i].multi_dof_joint_trajectory.points) { + point.transforms.push_back(transform); + } + } + SendGoalSync(robot_trajectories_, action_msgs::msg::GoalStatus::STATUS_ABORTED); +} + +// 3 If the orbit score is not possible, return the remaining 2 orbit. +TEST_F(LocalPathEvaluatorTest, InvalidOneTrajectory) { + geometry_msgs::msg::Transform transform; + transform.translation.x = 2.0; + transform.translation.y = 3.0; + transform.rotation.w = 1.0; + for (auto& point : robot_trajectories_[2].multi_dof_joint_trajectory.points) { + point.transforms.push_back(transform); + } + auto result = SendGoalSync(robot_trajectories_, action_msgs::msg::GoalStatus::STATUS_SUCCEEDED); + + EXPECT_EQ(result->robot_trajectories.size(), 2); + for (const auto& trajectory : result->robot_trajectories | boost::adaptors::indexed()) { + EXPECT_EQ(trajectory.value().joint_trajectory.points.size(), 3); + EXPECT_EQ(trajectory.value().multi_dof_joint_trajectory.points.size(), 3); + for (const auto& point : trajectory.value().joint_trajectory.points | boost::adaptors::indexed()) { + for (uint32_t i = 0; i < trajectory.value().joint_trajectory.joint_names.size(); ++i) { + EXPECT_NEAR(point.value().positions[i], joint_torajectories_[trajectory.index()][point.index()][i], 0.01); + } + } + } +} + +// Become a preempted in CancelGoal +TEST_F(LocalPathEvaluatorTest, EvaluatePreempted) { + // Throw about 100,000 tracks to prevent it from becoming a SUCCEEDEDED before throwing a cancellation + // With i9-10900K, I was able to cancel properly. + for (auto i = 0; i < 15; ++i) { + robot_trajectories_.insert(robot_trajectories_.end(), robot_trajectories_.begin(), robot_trajectories_.end()); + } + + tmc_planning_msgs::action::EvaluateRobotTrajectories::Goal goal; + goal.robot_trajectories = robot_trajectories_; + + auto future_goal_handle = evaluate_client_->async_send_goal(goal); + WaitForFutureComplete(future_goal_handle); + + auto goal_handle = future_goal_handle.get(); + EXPECT_TRUE(goal_handle.get()); + + auto future_cancel_response = evaluate_client_->async_cancel_goal(goal_handle); + WaitForFutureComplete(future_cancel_response); + + auto future_result = evaluate_client_->async_get_result(goal_handle); + WaitForFutureComplete(future_result); + + auto wrapped_result = future_result.get(); + EXPECT_EQ(static_cast(wrapped_result.code), action_msgs::msg::GoalStatus::STATUS_CANCELED); +} + +} // namespace tmc_local_path_evaluator + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/tmc_local_path_evaluator/test/soft_joint_constraint_score_calculation-test.cpp b/tmc_local_path_evaluator/test/soft_joint_constraint_score_calculation-test.cpp new file mode 100644 index 0000000..c77ba64 --- /dev/null +++ b/tmc_local_path_evaluator/test/soft_joint_constraint_score_calculation-test.cpp @@ -0,0 +1,194 @@ +/* +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. +*/ + +#include + +#include + +#include + +#include + +namespace { +tmc_robot_local_planner::Constraints GenerateTestConstraints() { + tmc_manipulation_types::RobotState min_state; + min_state.joint_state.name = {"arm", "head"}; + min_state.joint_state.position.resize(2); + min_state.joint_state.position << -1.0, -1.0; + min_state.multi_dof_joint_state.names = {"world_joint"}; + min_state.multi_dof_joint_state.poses = { + Eigen::Translation3d(-0.1, -0.1, 0.0) * Eigen::AngleAxisd(-0.5, Eigen::Vector3d::UnitZ())}; + + tmc_manipulation_types::RobotState max_state; + max_state.joint_state.name = min_state.joint_state.name; + max_state.joint_state.position.resize(2); + max_state.joint_state.position << 1.0, 1.0; + max_state.multi_dof_joint_state.names = min_state.multi_dof_joint_state.names; + max_state.multi_dof_joint_state.poses = { + Eigen::Translation3d(0.1, 0.1, 0.0) * Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitZ())}; + + const auto joint_constraint = std::make_shared( + min_state, max_state, 0); + tmc_robot_local_planner::Constraints constraints; + constraints.soft_joint_constraints = {joint_constraint}; + return constraints; +} + +tmc_manipulation_types::TimedRobotTrajectory GenerateTestTrajectory() { + tmc_manipulation_types::TimedRobotTrajectory trajectory; + trajectory.joint_trajectory.joint_names = {"arm", "head", "wrist"}; + trajectory.joint_trajectory.points.resize(2); + trajectory.joint_trajectory.points[0].positions.resize(3); + trajectory.joint_trajectory.points[0].positions << 2.0, 2.0, 2.0; + trajectory.joint_trajectory.points[1].positions.resize(3); + trajectory.joint_trajectory.points[1].positions << 0.0, 0.0, 2.0; + trajectory.multi_dof_joint_trajectory.joint_names = {"world_joint"}; + trajectory.multi_dof_joint_trajectory.points.resize(2); + trajectory.multi_dof_joint_trajectory.points[0].transforms = { + Eigen::Translation3d(2.0, 2.0, 0.0) * Eigen::AngleAxisd(2.0, Eigen::Vector3d::UnitZ())}; + trajectory.multi_dof_joint_trajectory.points[1].transforms = {Eigen::Affine3d::Identity()}; + return trajectory; +} +} // namespace + +namespace tmc_local_path_evaluator { + +class SoftJointConstraintScoreCalculationTest : public ::testing::Test { + protected: + void SetUp() override; + + ITrajectoryScoreCalculation::Ptr score_calculation_; + rclcpp::Node::SharedPtr node_; +}; + +void SoftJointConstraintScoreCalculationTest::SetUp() { + score_calculation_ = std::make_shared(); + + rclcpp::NodeOptions default_options; + default_options.parameter_overrides() = {rclcpp::Parameter("server.score_weight", 1.0)}; + node_ = rclcpp::Node::make_shared("server", default_options); + + ASSERT_TRUE(score_calculation_->Initialize(node_, "server")); +} + +TEST(SoftJointConstraintScoreCalculationPluginTest, CreateInstance) { + pluginlib::ClassLoader class_loader( + "tmc_local_path_evaluator", "tmc_local_path_evaluator::ITrajectoryScoreCalculation"); + auto calculator = class_loader.createSharedInstance("tmc_local_path_evaluator/SoftJointConstraintScoreCalculation"); +} + +TEST_F(SoftJointConstraintScoreCalculationTest, GoalInJointConstraint) { + const auto score = score_calculation_->Calculate(GenerateTestTrajectory(), GenerateTestConstraints()); + EXPECT_TRUE(static_cast(score)); + EXPECT_DOUBLE_EQ(score.value(), 0.0); +} + +TEST_F(SoftJointConstraintScoreCalculationTest, OutsideJointState) { + auto trajectory = GenerateTestTrajectory(); + trajectory.joint_trajectory.points.back().positions[0] = 1.3; + + const auto score = score_calculation_->Calculate(trajectory, GenerateTestConstraints()); + EXPECT_TRUE(static_cast(score)); + EXPECT_DOUBLE_EQ(score.value(), 0.3); +} + +TEST_F(SoftJointConstraintScoreCalculationTest, CalculateByChangeScoreWeights) { + rclcpp::NodeOptions default_options; + default_options.parameter_overrides() = {rclcpp::Parameter("server.score_weight", 2.0)}; + node_ = rclcpp::Node::make_shared("server", default_options); + + ASSERT_TRUE(score_calculation_->Initialize(node_, "server")); + + auto trajectory = GenerateTestTrajectory(); + trajectory.joint_trajectory.points.back().positions[0] = 1.3; + + const auto score = score_calculation_->Calculate(trajectory, GenerateTestConstraints()); + EXPECT_TRUE(static_cast(score)); + EXPECT_DOUBLE_EQ(score.value(), 0.6); +} + +TEST_F(SoftJointConstraintScoreCalculationTest, OutsideBasePosition) { + auto trajectory = GenerateTestTrajectory(); + trajectory.multi_dof_joint_trajectory.points.back().transforms = { + Eigen::Translation3d(0.0, 0.5, 0.0) * Eigen::AngleAxisd::Identity()}; + + const auto score = score_calculation_->Calculate(trajectory, GenerateTestConstraints()); + EXPECT_TRUE(static_cast(score)); + EXPECT_DOUBLE_EQ(score.value(), 0.4); +} + +TEST_F(SoftJointConstraintScoreCalculationTest, OutsideBaseYaw) { + auto trajectory = GenerateTestTrajectory(); + trajectory.multi_dof_joint_trajectory.points.back().transforms = { + Eigen::Translation3d::Identity() * Eigen::AngleAxisd(1.0, Eigen::Vector3d::UnitZ())}; + + const auto score = score_calculation_->Calculate(trajectory, GenerateTestConstraints()); + EXPECT_TRUE(static_cast(score)); + EXPECT_DOUBLE_EQ(score.value(), 0.5); +} + +TEST_F(SoftJointConstraintScoreCalculationTest, OutsideJointAndBase) { + auto trajectory = GenerateTestTrajectory(); + trajectory.joint_trajectory.points.back().positions[0] = 1.1; + trajectory.joint_trajectory.points.back().positions[1] = -1.1; + trajectory.multi_dof_joint_trajectory.points[1].transforms = { + Eigen::Translation3d(0.2, -0.2, 0.0) * Eigen::AngleAxisd(0.6, Eigen::Vector3d::UnitZ())}; + + const auto score = score_calculation_->Calculate(trajectory, GenerateTestConstraints()); + EXPECT_TRUE(static_cast(score)); + // Regarding the specifications of RangeJointConstraint, the Normal, the Norm of each Norm + EXPECT_NEAR(score.value(), std::sqrt(0.1 * 0.1 * 2) + std::sqrt(0.1 * 0.1 * 3), 1e-3); +} + +TEST_F(SoftJointConstraintScoreCalculationTest, NoSoftJointConstraints) { + const auto score = score_calculation_->Calculate(GenerateTestTrajectory(), tmc_robot_local_planner::Constraints()); + EXPECT_TRUE(static_cast(score)); + EXPECT_DOUBLE_EQ(score.value(), 0.0); +} + +TEST_F(SoftJointConstraintScoreCalculationTest, MismatchPointsSize) { + auto trajectory = GenerateTestTrajectory(); + trajectory.joint_trajectory.points.pop_back(); + + auto score = score_calculation_->Calculate(trajectory, GenerateTestConstraints()); + EXPECT_FALSE(static_cast(score)); +} + +TEST_F(SoftJointConstraintScoreCalculationTest, EmptyTrajectory) { + const auto score = score_calculation_->Calculate( + tmc_manipulation_types::TimedRobotTrajectory(), GenerateTestConstraints()); + EXPECT_FALSE(static_cast(score)); +} + +} // namespace tmc_local_path_evaluator + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/tmc_local_path_evaluator/test/soft_link_constraint_score_calculation-test.cpp b/tmc_local_path_evaluator/test/soft_link_constraint_score_calculation-test.cpp new file mode 100644 index 0000000..e2779b8 --- /dev/null +++ b/tmc_local_path_evaluator/test/soft_link_constraint_score_calculation-test.cpp @@ -0,0 +1,168 @@ +/* +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. +*/ + +#include + +#include + +#include +#include + +#include + +namespace { +tmc_robot_local_planner::Constraints GenerateTestConstraints() { + tmc_manipulation_types::TaskSpaceRegion tsr; + tsr.end_frame_id = "link7"; + tsr.origin_frame_id = "odom"; + // A position in which each joint is added 0.1 from the Link7 position when each joint is 1.57, 1.57, 0.1, 1.57, 1.57, 1.57. + tsr.origin_to_tsr = Eigen::Translation3d(0.150358, 0.54996, 0.500279) + * Eigen::Quaterniond(0.706598, -0.0013312, -0.707606, -0.00334743); + tsr.tsr_to_end = Eigen::Affine3d::Identity(); + tsr.min_bounds << -0.05, -0.11, -0.05, -0.05, -0.05, -0.05; + tsr.max_bounds << 0.05, 0.0, 0.05, 0.05, 0.05, 0.05; + + const auto link_constraint = std::make_shared(tsr, 0); + tmc_robot_local_planner::Constraints constraints; + constraints.soft_link_constraints = {link_constraint}; + return constraints; +} + +tmc_manipulation_types::TimedRobotTrajectory GenerateTestTrajectory() { + tmc_manipulation_types::TimedRobotTrajectory trajectory; + trajectory.joint_trajectory.joint_names = {"joint1", "joint2", "joint3", "joint4", "joint5", "joint6"}; + trajectory.joint_trajectory.points.resize(2); + trajectory.joint_trajectory.points[0].positions.resize(6); + trajectory.joint_trajectory.points[0].positions << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + trajectory.joint_trajectory.points[1].positions.resize(6); + trajectory.joint_trajectory.points[1].positions << 1.57, 1.57, 0.1, 1.57, 1.57, 1.57; + trajectory.multi_dof_joint_trajectory.joint_names = {"world_joint"}; + trajectory.multi_dof_joint_trajectory.points.resize(2); + trajectory.multi_dof_joint_trajectory.points[0].transforms = {Eigen::Affine3d::Identity()}; + trajectory.multi_dof_joint_trajectory.points[1].transforms = {Eigen::Affine3d::Identity()}; + return trajectory; +} +} // namespace + +namespace tmc_local_path_evaluator { + +class SoftLinkConstraintScoreCalculationTest : public ::testing::Test { + protected: + void SetUp() override; + + ITrajectoryScoreCalculation::Ptr score_calculation_; + rclcpp::Node::SharedPtr node_; +}; + +void SoftLinkConstraintScoreCalculationTest::SetUp() { + score_calculation_ = std::make_shared(); + + rclcpp::NodeOptions default_options; + default_options.parameter_overrides() = { + rclcpp::Parameter("server.score_weight", 1.0), + rclcpp::Parameter("robot_description", tmc_manipulation_tests::stanford_manipulator::GetUrdf())}; + node_ = rclcpp::Node::make_shared("server", default_options); + + ASSERT_TRUE(score_calculation_->Initialize(node_, "server")); +} + +TEST(SoftLinkConstraintScoreCalculationPluginTest, CreateInstance) { + pluginlib::ClassLoader class_loader( + "tmc_local_path_evaluator", "tmc_local_path_evaluator::ITrajectoryScoreCalculation"); + auto calculator = class_loader.createSharedInstance("tmc_local_path_evaluator/SoftLinkConstraintScoreCalculation"); +} + +TEST_F(SoftLinkConstraintScoreCalculationTest, GoalInLinkConstraint) { + const auto score = score_calculation_->Calculate(GenerateTestTrajectory(), GenerateTestConstraints()); + EXPECT_TRUE(static_cast(score)); + EXPECT_DOUBLE_EQ(score.value(), 0.0); +} + +TEST_F(SoftLinkConstraintScoreCalculationTest, OutsideLinkConstraint) { + auto trajectory = GenerateTestTrajectory(); + trajectory.multi_dof_joint_trajectory.points.back().transforms = { + Eigen::Translation3d(0.0, 0.5, 0.0) * Eigen::AngleAxisd::Identity()}; + + const auto score = score_calculation_->Calculate(trajectory, GenerateTestConstraints()); + EXPECT_TRUE(static_cast(score)); + EXPECT_NEAR(score.value(), 0.4, 1.0e-3); +} + +TEST_F(SoftLinkConstraintScoreCalculationTest, CalculateByChangeScoreWeights) { + rclcpp::NodeOptions default_options; + default_options.parameter_overrides() = { + rclcpp::Parameter("server.score_weight", 2.0), + rclcpp::Parameter("robot_description", tmc_manipulation_tests::stanford_manipulator::GetUrdf())}; + node_ = rclcpp::Node::make_shared("server", default_options); + + ASSERT_TRUE(score_calculation_->Initialize(node_, "server")); + + auto trajectory = GenerateTestTrajectory(); + trajectory.multi_dof_joint_trajectory.points.back().transforms = { + Eigen::Translation3d(0.0, 0.5, 0.0) * Eigen::AngleAxisd::Identity()}; + + const auto score = score_calculation_->Calculate(trajectory, GenerateTestConstraints()); + EXPECT_TRUE(static_cast(score)); + EXPECT_NEAR(score.value(), 0.8, 1.0e-3); +} + +TEST_F(SoftLinkConstraintScoreCalculationTest, NoRobotDescription) { + rclcpp::NodeOptions default_options; + default_options.parameter_overrides() = {rclcpp::Parameter("server.score_weight", 1.0)}; + node_ = rclcpp::Node::make_shared("server", default_options); + + EXPECT_FALSE(score_calculation_->Initialize(node_, "server")); +} + +TEST_F(SoftLinkConstraintScoreCalculationTest, NoSoftLinkConstraints) { + const auto score = score_calculation_->Calculate(GenerateTestTrajectory(), tmc_robot_local_planner::Constraints()); + EXPECT_TRUE(static_cast(score)); + EXPECT_DOUBLE_EQ(score.value(), 0.0); +} + +TEST_F(SoftLinkConstraintScoreCalculationTest, MismatchPointsSize) { + auto trajectory = GenerateTestTrajectory(); + trajectory.joint_trajectory.points.pop_back(); + + auto score = score_calculation_->Calculate(trajectory, GenerateTestConstraints()); + EXPECT_FALSE(static_cast(score)); +} + +TEST_F(SoftLinkConstraintScoreCalculationTest, EmptyTrajectory) { + const auto score = score_calculation_->Calculate( + tmc_manipulation_types::TimedRobotTrajectory(), GenerateTestConstraints()); + EXPECT_FALSE(static_cast(score)); +} + +} // namespace tmc_local_path_evaluator + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/tmc_local_path_evaluator/test/test_util.hpp b/tmc_local_path_evaluator/test/test_util.hpp new file mode 100644 index 0000000..2234c84 --- /dev/null +++ b/tmc_local_path_evaluator/test/test_util.hpp @@ -0,0 +1,74 @@ +/* +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. +*/ +#ifndef TMC_LOCAL_PATH_EVALUATOR_TEST_UTIL_HPP_ +#define TMC_LOCAL_PATH_EVALUATOR_TEST_UTIL_HPP_ + +#include +#include + +#include + +#include +#include +#include +#include + +trajectory_msgs::msg::JointTrajectory CreateJointTrajectory( + const std::vector& joint_names, + const std::vector>& positions) { + trajectory_msgs::msg::JointTrajectory joint_trajectory; + joint_trajectory.joint_names = joint_names; + joint_trajectory.points.resize(positions.size()); + for (const auto& position : positions | boost::adaptors::indexed()) { + joint_trajectory.points[position.index()].positions = position.value(); + } + return joint_trajectory; +} + +trajectory_msgs::msg::MultiDOFJointTrajectory CreateMultiDOFJointTrajectory( + const std::vector& transforms) { + trajectory_msgs::msg::MultiDOFJointTrajectory multi_dof_joint_trajectory; + multi_dof_joint_trajectory.joint_names.push_back("world_joint"); + multi_dof_joint_trajectory.points.resize(transforms.size()); + for (const auto& transform : transforms | boost::adaptors::indexed()) { + multi_dof_joint_trajectory.points[transform.index()].transforms.push_back(transform.value()); + } + return multi_dof_joint_trajectory; +} + +moveit_msgs::msg::RobotTrajectory CreateRobotTrajectory( + const std::vector& joint_names, + const std::vector>& positions, + const std::vector& transforms) { + moveit_msgs::msg::RobotTrajectory robot_trajectory; + robot_trajectory.joint_trajectory = CreateJointTrajectory(joint_names, positions); + robot_trajectory.multi_dof_joint_trajectory = CreateMultiDOFJointTrajectory(transforms); + return robot_trajectory; +} + +#endif // TMC_LOCAL_PATH_EVALUATOR_TEST_UTIL_HPP_ diff --git a/tmc_local_path_evaluator/test/time_base_score_calculation-test.cpp b/tmc_local_path_evaluator/test/time_base_score_calculation-test.cpp new file mode 100644 index 0000000..e90843f --- /dev/null +++ b/tmc_local_path_evaluator/test/time_base_score_calculation-test.cpp @@ -0,0 +1,120 @@ +/* +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. +*/ + +#include + +#include + +#include + +namespace { +tmc_manipulation_types::TimedRobotTrajectory GenerateTestTrajectory() { + tmc_manipulation_types::TimedRobotTrajectory trajectory; + trajectory.joint_trajectory.points.resize(2); + trajectory.joint_trajectory.points[0].time_from_start = 1.0; + trajectory.joint_trajectory.points[1].time_from_start = 3.0; + trajectory.multi_dof_joint_trajectory.points.resize(2); + trajectory.multi_dof_joint_trajectory.points[0].time_from_start = 1.0; + trajectory.multi_dof_joint_trajectory.points[1].time_from_start = 3.0; + return trajectory; +} +} // namespace + +namespace tmc_local_path_evaluator { + +class TimeBaseScoreCalculationTest : public ::testing::Test { + protected: + void SetUp() override; + + ITrajectoryScoreCalculation::Ptr score_calculation_; + rclcpp::Node::SharedPtr node_; +}; + +void TimeBaseScoreCalculationTest::SetUp() { + score_calculation_ = std::make_shared(); + + rclcpp::NodeOptions default_options; + default_options.parameter_overrides() = {rclcpp::Parameter("server.score_weight", 2.0)}; + node_ = rclcpp::Node::make_shared("server", default_options); + + ASSERT_TRUE(score_calculation_->Initialize(node_, "server")); +} + +TEST(TimeBaseScoreCalculationPluginTest, CreateInstance) { + pluginlib::ClassLoader class_loader( + "tmc_local_path_evaluator", "tmc_local_path_evaluator::ITrajectoryScoreCalculation"); + auto calculator = class_loader.createSharedInstance("tmc_local_path_evaluator/TimeBaseScoreCalculation"); +} + +TEST_F(TimeBaseScoreCalculationTest, Calculate) { + auto score = score_calculation_->Calculate(GenerateTestTrajectory(), tmc_robot_local_planner::Constraints()); + EXPECT_TRUE(static_cast(score)); + EXPECT_DOUBLE_EQ(score.value(), 6.0); +} + +TEST_F(TimeBaseScoreCalculationTest, CalculateByChangeScoreWeights) { + rclcpp::NodeOptions default_options; + default_options.parameter_overrides() = {rclcpp::Parameter("server.score_weight", 4.0)}; + node_ = rclcpp::Node::make_shared("server", default_options); + + ASSERT_TRUE(score_calculation_->Initialize(node_, "server")); + + auto score = score_calculation_->Calculate(GenerateTestTrajectory(), tmc_robot_local_planner::Constraints()); + EXPECT_TRUE(static_cast(score)); + EXPECT_DOUBLE_EQ(score.value(), 12.0); +} + +TEST_F(TimeBaseScoreCalculationTest, MismatchPointsSize) { + auto trajectory = GenerateTestTrajectory(); + trajectory.joint_trajectory.points.pop_back(); + + auto score = score_calculation_->Calculate(trajectory, tmc_robot_local_planner::Constraints()); + EXPECT_FALSE(static_cast(score)); +} + +TEST_F(TimeBaseScoreCalculationTest, MismatchTimeFromStart) { + auto trajectory = GenerateTestTrajectory(); + trajectory.joint_trajectory.points.back().time_from_start += 1.0; + + auto score = score_calculation_->Calculate(trajectory, tmc_robot_local_planner::Constraints()); + EXPECT_FALSE(static_cast(score)); +} + +TEST_F(TimeBaseScoreCalculationTest, EmptyTrajectory) { + auto score = score_calculation_->Calculate(tmc_manipulation_types::TimedRobotTrajectory(), + tmc_robot_local_planner::Constraints()); + EXPECT_FALSE(static_cast(score)); +} + +} // namespace tmc_local_path_evaluator + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/tmc_robot_local_planner/CHANGELOG.rst b/tmc_robot_local_planner/CHANGELOG.rst new file mode 100644 index 0000000..61403b4 --- /dev/null +++ b/tmc_robot_local_planner/CHANGELOG.rst @@ -0,0 +1,9 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package tmc_robot_local_planner +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.0.0 (2024-10-15) +------------------- +* Initial release +* Contributors: Keisuke Takeshita, Satoru Onoda, Yuta Watanabe + diff --git a/tmc_robot_local_planner/CMakeLists.txt b/tmc_robot_local_planner/CMakeLists.txt new file mode 100644 index 0000000..2e45cce --- /dev/null +++ b/tmc_robot_local_planner/CMakeLists.txt @@ -0,0 +1,60 @@ +cmake_minimum_required(VERSION 3.5.0) +project(tmc_robot_local_planner) + +find_package(ament_cmake REQUIRED) +find_package(angles REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(tmc_eigen_utils REQUIRED) +find_package(tmc_manipulation_types REQUIRED) +find_package(tmc_manipulation_types_bridge REQUIRED) +find_package(tmc_planning_msgs REQUIRED) +find_package(tmc_utils REQUIRED) + +add_library(${PROJECT_NAME} SHARED + src/${PROJECT_NAME}/range_joint_constraint.cpp + src/${PROJECT_NAME}/robot_local_planner.cpp + src/${PROJECT_NAME}/tsr_link_constraint.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC include ${EIGEN3_INCLUDE_DIRS}) +ament_target_dependencies(${PROJECT_NAME} angles pluginlib rclcpp rclcpp_action tmc_eigen_utils tmc_manipulation_types tmc_manipulation_types_bridge tmc_planning_msgs tmc_utils) + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) +install(DIRECTORY include/ + DESTINATION include +) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest(range_joint_constraint_test test/range_joint_constraint-test.cpp) + target_link_libraries(range_joint_constraint_test ${PROJECT_NAME}) + + ament_add_gtest(trajectory_merger_test test/trajectory_merger-test.cpp) + target_link_libraries(trajectory_merger_test ${PROJECT_NAME}) + + ament_add_gtest(robot_local_planner_test test/robot_local_planner-test.cpp) + target_link_libraries(robot_local_planner_test ${PROJECT_NAME}) + + ament_add_gmock(robot_local_planner_composition_test test/robot_local_planner_composition-test.cpp) + target_link_libraries(robot_local_planner_composition_test ${PROJECT_NAME}) + + ament_add_gtest(tsr_link_constraint_test test/tsr_link_constraint-test.cpp) + target_link_libraries(tsr_link_constraint_test ${PROJECT_NAME}) +endif() + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies(angles pluginlib rclcpp rclcpp_action tmc_manipulation_types tmc_planning_msgs tmc_utils) + +ament_package() + + + diff --git a/tmc_robot_local_planner/include/tmc_robot_local_planner/component_interfaces.hpp b/tmc_robot_local_planner/include/tmc_robot_local_planner/component_interfaces.hpp new file mode 100644 index 0000000..6a842e9 --- /dev/null +++ b/tmc_robot_local_planner/include/tmc_robot_local_planner/component_interfaces.hpp @@ -0,0 +1,85 @@ +/* +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. +*/ + +#pragma once + +#include +#include +#include +#include +#include + +namespace tmc_robot_local_planner { + +class IGenerator { + public: + virtual ~IGenerator() = default; + + virtual void Initialize(const rclcpp::Node::SharedPtr& node) = 0; + virtual bool Generate(const Constraints& constraints, + const tmc_manipulation_types::RobotState& initial_state, + double normalized_velocity, + const std::vector& ignore_joints, + std::function interrupt, + std::vector& trajectories_out) = 0; +}; + +class IEvaluator { + public: + virtual ~IEvaluator() = default; + + virtual void Initialize(const rclcpp::Node::SharedPtr& node) = 0; + virtual bool Evaluate(const Constraints& constraints, + const std::vector& trajectories_in, + std::function interrupt, + std::vector& trajectories_out) = 0; +}; + +class IValidator { + public: + virtual ~IValidator() = default; + + virtual void Initialize(const rclcpp::Node::SharedPtr& node) = 0; + virtual bool Validate(const std::vector& trajectories_in, + std::function interrupt, + tmc_manipulation_types::TimedRobotTrajectory& trajectory_out) = 0; +}; + +class IOptimizer { + public: + virtual ~IOptimizer() = default; + + virtual void Initialize(const rclcpp::Node::SharedPtr& node) = 0; + virtual bool Optimize(const tmc_manipulation_types::TimedRobotTrajectory& trajectory_in, + std::function interrupt, + tmc_manipulation_types::TimedRobotTrajectory& trajectory_out) = 0; + virtual bool Optimize(const std::vector& trajectories_in, + std::function interrupt, + std::vector& trajectories_out) = 0; +}; +} // namespace tmc_robot_local_planner diff --git a/tmc_robot_local_planner/include/tmc_robot_local_planner/constraints.hpp b/tmc_robot_local_planner/include/tmc_robot_local_planner/constraints.hpp new file mode 100644 index 0000000..49012f8 --- /dev/null +++ b/tmc_robot_local_planner/include/tmc_robot_local_planner/constraints.hpp @@ -0,0 +1,68 @@ +/* +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. +*/ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace tmc_robot_local_planner { + +struct LinearConstraint { + std::string end_frame_id; + Eigen::Vector3d axis; + double distance; +}; + +/// \brief Constraints of wholebody trajectory interface +struct Constraints { + // Goal constraints + std::vector hard_link_constraints; + std::vector hard_joint_constraints; + + // Goal constraints(Soft) + std::vector soft_link_constraints; + std::vector soft_joint_constraints; + + // Path constraints(Hard) + LinearConstraint goal_relative_linear_constraint; + std::vector hard_path_link_constraints; + + // Path constraints(Soft) + std::vector soft_path_joint_constraints; + + // TODO(Yuta Watanabe) implement visivility constraints + // std::vector soft_visibility_constraints; + // std::vector hard_visibility_constraints; +}; // struct Constraints + +} // namespace tmc_robot_local_planner diff --git a/tmc_robot_local_planner/include/tmc_robot_local_planner/joint_constraint.hpp b/tmc_robot_local_planner/include/tmc_robot_local_planner/joint_constraint.hpp new file mode 100644 index 0000000..5fb66ec --- /dev/null +++ b/tmc_robot_local_planner/include/tmc_robot_local_planner/joint_constraint.hpp @@ -0,0 +1,82 @@ +/* +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. +*/ +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace tmc_robot_local_planner { + +/// \brief Constraints of wholebody trajectory interface +class IJointConstraint { + public: + typedef std::shared_ptr Ptr; + virtual ~IJointConstraint() = default; + + /// \brief Get joint name in joint constraint. + /// \return joint name in constraint. + virtual tmc_manipulation_types::NameSeq GetJointName() const = 0; + + /// \brief Get priority in joint constraint. + /// \return priority in constraint. + virtual uint32_t GetPriority() const = 0; + + /// \brief Generate sample from joint constraint. + /// \return A sample in constraint. + virtual tmc_manipulation_types::RobotState Sample() const = 0; + + /// \brief Calcurate closest point in joint constraint. + /// \param state pose from origin to query frame. + /// \return A state closeet to pose + // virtual tmc_manipulation_types::RobotState CalcClosest( + // const tmc_manipulation_types::RobotState& state) const = 0; + + /// \brief Return query state is in joint constraint or not. + /// \param state query state of robot. + /// \return bool value. + // virtual bool IsInConstraint( + // const tmc_manipulation_types::RobotState& state) const = 0; + + /// \brief calcurate displacement + /// \param state query state of robot. + /// \return displacement value. + virtual double CalcDisplacement( + const tmc_manipulation_types::RobotState& state) const = 0; + + /// \brief calcurate displacement + /// \param state query state of robot. + /// \return displacement values per joints. + virtual std::tuple CalcSeparateDisplacements( + const tmc_manipulation_types::RobotState& state) const = 0; +}; + +} // namespace tmc_robot_local_planner diff --git a/tmc_robot_local_planner/include/tmc_robot_local_planner/link_constraint.hpp b/tmc_robot_local_planner/include/tmc_robot_local_planner/link_constraint.hpp new file mode 100644 index 0000000..0329b5f --- /dev/null +++ b/tmc_robot_local_planner/include/tmc_robot_local_planner/link_constraint.hpp @@ -0,0 +1,87 @@ +/* +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. +*/ + +#pragma once + +#include +#include +#include +#include + +#include + +namespace tmc_robot_local_planner { + +/// \brief Link Constraints of wholebody trajectory interface +class ILinkConstraint { + public: + typedef std::shared_ptr Ptr; + virtual ~ILinkConstraint() = default; + + /// \brief Get end frame id in link constraint. + /// \return end frame in constraint. + virtual std::string GetLinkName() const = 0; + + /// \brief Get origin frame id in link constraint. + /// \return origin frame in constraint. + virtual std::string GetOriginName() const = 0; + + /// \brief Get priority in link constraint. + /// \return priority in constraint. + virtual uint32_t GetPriority() const = 0; + + /// \brief Generate sample from link constraint. + /// \return A sample in constraint. + virtual Eigen::Affine3d Sample() const = 0; + + /// \brief Calcurate closest point in link constraint. + /// \param origin_to_query pose from origin to query frame. + /// \return A pose from origin to closet frame to query frame. + virtual Eigen::Affine3d CalcClosest( + const Eigen::Affine3d& origin_to_query) const = 0; + + /// \brief Return query is in link constraint or not. + /// \param origin_to_query pose from origin to query frame. + /// \return If query is in constraint or not. + virtual bool IsInConstraint( + const Eigen::Affine3d& origin_to_query) const = 0; + + /// \brief Return displacement to link constraint + /// \param origin_to_query pose from origin to query frame. + /// \return displacemnt. + virtual double CalcDisplacement( + const Eigen::Affine3d& origin_to_query) const = 0; + + /// \brief calcurate displacement + /// \param state query state of robot. + /// \return displacemnt (x y z roll pitch yaw). + virtual tmc_manipulation_types::RegionValues CalcSeparateDisplacements( + const Eigen::Affine3d& origin_to_query) const = 0; +}; + +} // namespace tmc_robot_local_planner diff --git a/tmc_robot_local_planner/include/tmc_robot_local_planner/range_joint_constraint.hpp b/tmc_robot_local_planner/include/tmc_robot_local_planner/range_joint_constraint.hpp new file mode 100644 index 0000000..c8f6aea --- /dev/null +++ b/tmc_robot_local_planner/include/tmc_robot_local_planner/range_joint_constraint.hpp @@ -0,0 +1,125 @@ +/* +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. +*/ + +#pragma once + +#include +#include +#include +#include +#include +#include + +using tmc_manipulation_types::Pose2dSeq; +using tmc_manipulation_types::RobotState; +using tmc_manipulation_types::NameSeq; + +namespace tmc_robot_local_planner { + +/// \brief Constraints of wholebody trajectory interface +class RangeJointConstraint : public IJointConstraint { + public: + /// \brief Constructor. initialize each parameter. + /// \param min robot state + /// \param max robot state + /// \param priority. + /// \param seed. + /// \par demeanor. + /// - set robot state. + /// - set priority. + /// - set seed for srand function. + RangeJointConstraint( + const tmc_manipulation_types::RobotState& min, + const tmc_manipulation_types::RobotState& max, + uint32_t priority, + uint32_t seed); + + /// \brief Transfer Constructor. initialize each parameter. + /// \param min robot state + /// \param max robot state + /// \param priority. + /// \par demeanor. + /// - set robot state. + /// - set priority. + RangeJointConstraint( + const tmc_manipulation_types::RobotState& min, + const tmc_manipulation_types::RobotState& max, + uint32_t priority) + : RangeJointConstraint(min, max, priority, (unsigned int) time(NULL)) {} + + virtual ~RangeJointConstraint() = default; + + /// \brief Get joint name in range joint constraint. + /// \return end frame in constraint. + virtual tmc_manipulation_types::NameSeq GetJointName() const; + + /// \brief Get priority in range joint constraint. + /// \return priority in constraint. + virtual uint32_t GetPriority() const; + + /// \brief Generate sample from range joint constraint. + /// \return A sample in constraint. + virtual tmc_manipulation_types::RobotState Sample() const; + + /// \brief Calcurate closest point in range joint constraint. + /// \param state pose from origin to query frame. + /// \return A state closeet to pose + // virtual tmc_manipulation_types::RobotState CalcClosest( + // const tmc_manipulation_types::RobotState& state) const; + + /// \brief calcurate displacement + /// \param state query state of robot. + /// \return displacement value. + virtual double CalcDisplacement( + const tmc_manipulation_types::RobotState& state) const; + + /// \brief calcurate displacement + /// \param state query state of robot. + /// \return displacement values per joints. + virtual std::tuple CalcSeparateDisplacements( + const tmc_manipulation_types::RobotState& state) const; + + /// \brief Return query is in range joint constraint or not. + /// \param state query state of robot. + /// \return If query is in constraint or not. + // virtual bool IsInConstraint( + // const tmc_manipulation_types::RobotState& state) const; + + private: + tmc_manipulation_types::NameSeq joint_names_; + tmc_manipulation_types::NameSeq multi_dof_joint_names_; + Eigen::VectorXd min_pos_; + Eigen::VectorXd max_pos_; + Eigen::VectorXd diff_pos_; + tmc_manipulation_types::Pose2dSeq min_2d_poses_; + tmc_manipulation_types::Pose2dSeq max_2d_poses_; + tmc_manipulation_types::Pose2dSeq diff_2d_poses_; + uint32_t priority_; +}; + +} // namespace tmc_robot_local_planner diff --git a/tmc_robot_local_planner/include/tmc_robot_local_planner/robot_local_planner.hpp b/tmc_robot_local_planner/include/tmc_robot_local_planner/robot_local_planner.hpp new file mode 100644 index 0000000..287e327 --- /dev/null +++ b/tmc_robot_local_planner/include/tmc_robot_local_planner/robot_local_planner.hpp @@ -0,0 +1,293 @@ +/* +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. +*/ +#ifndef TMC_ROBOT_LOCAL_PLANNER_ROBOT_LOCAL_PLANNER_HPP_ +#define TMC_ROBOT_LOCAL_PLANNER_ROBOT_LOCAL_PLANNER_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace tmc_robot_local_planner { + +struct RobotLocalPlannerPlugins { + std::shared_ptr generator; + std::shared_ptr evaluator; + std::shared_ptr validator; + std::shared_ptr optimizer; + + void Initialize(const rclcpp::Node::SharedPtr& node); + + class Loader { + public: + struct PluginNames { + std::string generate_action; + std::string evaluate_action; + std::string validate_action; + std::string optimize_action; + }; + Loader(); + + RobotLocalPlannerPlugins CreatePluginInstances(const PluginNames& names); + + private: + pluginlib::ClassLoader generator_loader_; + pluginlib::ClassLoader evaluator_loader_; + pluginlib::ClassLoader validator_loader_; + pluginlib::ClassLoader optimizer_loader_; + }; +}; + +class VisualizationPublisher { + public: + explicit VisualizationPublisher(const rclcpp::Node::SharedPtr& node); + + void PublishGeneratedTrajectories( + const std::vector& trajectories) const; + void PublishGeneratedTrajectories( + const std::vector& trajectories) const; + + void PublishPlannedTrajectory(const tmc_manipulation_types::TimedRobotTrajectory& trajectory) const; + void PublishPlannedTrajectory(const moveit_msgs::msg::RobotTrajectory& trajectory) const; + + private: + tmc_utils::DynamicParameter::Ptr publish_generated_trajectories_; + tmc_utils::DynamicParameter::Ptr publish_planned_trajectory_; + std::vector set_param_handlers_; + + rclcpp::Publisher::SharedPtr generated_trajectories_pub_; + rclcpp::Publisher::SharedPtr planned_trajectory_pub_; +}; + +enum class RobotLocalPlannerErrorCode { + kSuccess, + kConstraintsEmpty, + // Consider if there is a request to divide mistakes in detail + kGenerationFailure, + kEvaluationFailure, + kValidationFailure, + kOptimizationFailure, +}; + +class RobotLocalPlannerComposition { + public: + using Ptr = std::shared_ptr; + + RobotLocalPlannerComposition( + const rclcpp::Node::SharedPtr& node, + const TrajectoryMerger::Ptr& trajectory_merger, + const RobotLocalPlannerPlugins& plugins); + + RobotLocalPlannerComposition( + const rclcpp::Node::SharedPtr& node, + const RobotLocalPlannerPlugins& plugins); + + ~RobotLocalPlannerComposition() {} + + /// @brief plan wholebody trajectory from constraints + /// @param goal_constraints The constraints to be satisifed at the terminal. + /// @param initial_state The status at initial. + /// @param normalized_vel Normalized velocity. [1/s]. + /// @return adopted_trajectory_out The adopted trajectory. + std::future PlanPath( + const tmc_robot_local_planner::Constraints& goal_constraints, + const tmc_manipulation_types::RobotState& initial_state, + double normalized_vel); + + /// @brief plan wholebody trajectory from constraints + /// @param goal_constraints The constraints to be satisifed at the terminal. + /// @param initial_state The status at initial. + /// @param normalized_vel Normalized velocity. [1/s]. + /// @param ignore_joints Joint names which don't want to use. + /// @return adopted_trajectory_out The adopted trajectory. + std::future PlanPath( + const tmc_robot_local_planner::Constraints& goal_constraints, + const tmc_manipulation_types::RobotState& initial_state, + double normalized_vel, + const std::vector& ignore_joints); + + void InterruptPlanPath(); + + RobotLocalPlannerErrorCode last_error_code() { + std::lock_guard lock(mutex_); + return last_error_code_; + } + + private: + tmc_manipulation_types::TimedRobotTrajectory PlanPathImpl( + const Constraints& goal_constraints, + const tmc_manipulation_types::RobotState& initial_state, + double normalized_vel, + const std::vector& ignore_joints); + + RobotLocalPlannerPlugins plugins_; + + TrajectoryMerger::Ptr trajectory_merger_; + + rclcpp::Logger logger_; + + std::mutex mutex_; + bool is_interrupt_requested_; + + void set_is_interrupt_requested(bool new_value) { + std::lock_guard lock(mutex_); + is_interrupt_requested_ = new_value; + } + bool is_interrupt_requested() { + std::lock_guard lock(mutex_); + return is_interrupt_requested_; + } + + RobotLocalPlannerErrorCode last_error_code_; + void UpdateLastErrorCode(RobotLocalPlannerErrorCode new_value) { + std::lock_guard lock(mutex_); + last_error_code_ = new_value; + } + + std::shared_ptr visualization_pub_; + + bool optimize_second_; +}; + + +/// @brief Top level class of local planner +class RobotLocalPlanner { + public: + using Ptr = std::shared_ptr; + + struct ActionNames { + std::string generate_action; + std::string evaluate_action; + std::string validate_action; + std::string optimize_action; + + ActionNames(); + }; + + RobotLocalPlanner(const rclcpp::Node::SharedPtr& node, + const TrajectoryMerger::Ptr& trajectory_merger, + const ActionNames& action_names); + RobotLocalPlanner(const rclcpp::Node::SharedPtr& node, + const TrajectoryMerger::Ptr& trajectory_merger); + RobotLocalPlanner(const rclcpp::Node::SharedPtr& node, + const ActionNames& action_names); + explicit RobotLocalPlanner(const rclcpp::Node::SharedPtr& node); + + ~RobotLocalPlanner() {} + + bool AreActionServersReady() const; + + /// @brief plan wholebody trajectory from constraints + /// @param goal_constraints The constraints to be satisifed at the terminal. + /// @param initial_state The status at initial. + /// @param normalized_vel Normalized velocity. [1/s]. + /// @return The future object of planning trajectory + std::future PlanPath( + const tmc_planning_msgs::msg::Constraints& goal_constraints, + const moveit_msgs::msg::RobotState& initial_state, + double normalized_vel); + + /// @brief plan wholebody trajectory from constraints + /// @param goal_constraints The constraints to be satisifed at the terminal. + /// @param initial_state The status at initial. + /// @param normalized_vel Normalized velocity. [1/s]. + /// @param ignore_joints Joint names which don't want to use. + /// @return The future object of planning trajectory + std::future PlanPath( + const tmc_planning_msgs::msg::Constraints& goal_constraints, + const moveit_msgs::msg::RobotState& initial_state, + double normalized_vel, + const std::vector& ignore_joints); + + void InterruptPlanPath(); + + void set_action_timeout(double new_value) { action_timeout_ = new_value; } + + RobotLocalPlannerErrorCode last_error_code() { + std::lock_guard lock(mutex_); + return last_error_code_; + } + + private: + moveit_msgs::msg::RobotTrajectory PlanPathImpl( + const tmc_planning_msgs::msg::Constraints& goal_constraints, + const moveit_msgs::msg::RobotState& initial_state, + double normalized_vel, + const std::vector& ignore_joints); + + rclcpp_action::Client::SharedPtr generator_client_; + rclcpp_action::Client::SharedPtr evaluator_client_; + rclcpp_action::Client::SharedPtr validator_client_; + rclcpp_action::Client::SharedPtr optimizer_client_; + + TrajectoryMerger::Ptr trajectory_merger_; + + rclcpp::Clock::SharedPtr clock_; + double action_timeout_; + + rclcpp::Logger logger_; + + std::mutex mutex_; + bool is_interrupt_requested_; + + void set_is_interrupt_requested(bool new_value) { + std::lock_guard lock(mutex_); + is_interrupt_requested_ = new_value; + } + bool is_interrupt_requested() { + std::lock_guard lock(mutex_); + return is_interrupt_requested_; + } + + RobotLocalPlannerErrorCode last_error_code_; + void UpdateLastErrorCode(RobotLocalPlannerErrorCode new_value) { + std::lock_guard lock(mutex_); + last_error_code_ = new_value; + } + + std::shared_ptr visualization_pub_; +}; + +} // namespace tmc_robot_local_planner + +#endif // TMC_ROBOT_LOCAL_PLANNER_ROBOT_LOCAL_PLANNER_HPP_ diff --git a/tmc_robot_local_planner/include/tmc_robot_local_planner/trajectory_merger.hpp b/tmc_robot_local_planner/include/tmc_robot_local_planner/trajectory_merger.hpp new file mode 100644 index 0000000..6b9c7e7 --- /dev/null +++ b/tmc_robot_local_planner/include/tmc_robot_local_planner/trajectory_merger.hpp @@ -0,0 +1,76 @@ +/* +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. +*/ +#ifndef TMC_ROBOT_LOCAL_PLANNER_TRAJECTORY_MIXER_HPP_ +#define TMC_ROBOT_LOCAL_PLANNER_TRAJECTORY_MIXER_HPP_ + +#include +#include +#include + +namespace tmc_robot_local_planner { + +template +class TrajectoryMerger { + public: + using Ptr = std::shared_ptr; + + /// @brief constructor + TrajectoryMerger() {} + /// @brief Destructor + ~TrajectoryMerger() {} + /// @brief Set the orbit + bool SetTrajectory(const T& trajectory) { + if (!IsValidTrajectory(trajectory)) { + return false; + } + trajectory_ = trajectory; + return true; + } + /// @brief Unlock the set orbit + void ClearTrajectory() { + trajectory_ = T(); + } + /// @brief Mix the orbit + void MergeTrajectory(std::vector& trajectories) { + if (!IsValidTrajectory(trajectory_)) { + return; + } + trajectories.push_back(trajectory_); + } + + private: + bool IsValidTrajectory(const T& trajectory) const { + return !(trajectory.joint_trajectory.joint_names.empty() && + trajectory.joint_trajectory.points.empty()); + } + + T trajectory_; +}; +} // namespace tmc_robot_local_planner + +#endif // TMC_ROBOT_LOCAL_PLANNER_TRAJECTORY_MIXER_HPP_ diff --git a/tmc_robot_local_planner/include/tmc_robot_local_planner/tsr_link_constraint.hpp b/tmc_robot_local_planner/include/tmc_robot_local_planner/tsr_link_constraint.hpp new file mode 100644 index 0000000..ee2044e --- /dev/null +++ b/tmc_robot_local_planner/include/tmc_robot_local_planner/tsr_link_constraint.hpp @@ -0,0 +1,165 @@ +/* +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. +*/ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace tmc_robot_local_planner { + +/// \brief Tsr Link Constraints of wholebody trajectory interface +class TsrLinkConstraint : public ILinkConstraint { + public: + /// \brief Constructor. initialize each parameter. + /// \param task space region. + /// \param priority. + /// \param seed. + /// \par demeanor. + /// - set task space region. + /// - set priority. + /// - set seed for srand function. + TsrLinkConstraint( + const tmc_manipulation_types::TaskSpaceRegion& tsr, + uint32_t priority, + uint32_t seed); + + /// \brief Constructor. initialize each parameter. + /// \param task space region. + /// \param priority. + /// \par demeanor. + /// - set task space region. + /// - set priority. + TsrLinkConstraint( + const tmc_manipulation_types::TaskSpaceRegion& tsr, uint32_t priority) + : TsrLinkConstraint(tsr, priority, (unsigned int) time(NULL)) {} + + virtual ~TsrLinkConstraint() = default; + + /// \brief Get end frame id in tsr link constraint. + /// \return end frame in constraint. + virtual std::string GetLinkName() const; + + /// \brief Get origin frame id in tsr link constraint. + /// \return origin frame in constraint. + virtual std::string GetOriginName() const; + + /// \brief Get priority in tsr link constraint. + /// \return priority in constraint. + virtual uint32_t GetPriority() const; + + /// \brief Generate sample from tsr link constraint. + /// \return A sample in constraint. + virtual Eigen::Affine3d Sample() const; + + /// \brief Calcurate closest point in tsr link constraint. + /// \param origin_to_query pose from origin to query frame. + /// \return A pose from origin to closet frame to query frame. + virtual Eigen::Affine3d CalcClosest(const Eigen::Affine3d& query) const; + + /// \brief Return query is in tsr link constraint or not. + /// \param origin_to_query pose from origin to query frame. + /// \return If query is in constraint or not. + virtual bool IsInConstraint(const Eigen::Affine3d& query) const; + + /// \brief Return displacement to tsr link constraint + /// \param origin_to_query pose from origin to query frame. + /// \return displacemnt. + virtual double CalcDisplacement( + const Eigen::Affine3d& origin_to_query) const; + + /// \brief calcurate displacement + /// \param state query state of robot. + /// \return displacemnt (x y z roll pitch yaw). + virtual tmc_manipulation_types::RegionValues CalcSeparateDisplacements( + const Eigen::Affine3d& origin_to_query) const; + + protected: + tmc_manipulation_types::TaskSpaceRegion tsr_; + + /// \brief Return displacement region values to tsr link constraint + /// \param origin_to_query pose from origin to query frame. + /// \return displacemnt region value. + virtual tmc_manipulation_types::RegionValues CalcDisplacementToTsr( + const Eigen::Affine3d& origin_to_query) const; + + private: + uint32_t priority_; +}; + +/// \brief Rotation-first Tsr Link Constraints of wholebody trajectory interface +class RotationFirstTsrLinkConstraint : public TsrLinkConstraint { + public: + /// \brief Constructor. initialize each parameter. + /// \param task space region. + /// \param priority. + /// \param seed. + /// \par demeanor. + /// - set task space region. + /// - set priority. + /// - set seed for srand function. + RotationFirstTsrLinkConstraint( + const tmc_manipulation_types::TaskSpaceRegion& tsr, + uint32_t priority, + uint32_t seed) + : TsrLinkConstraint(tsr, priority, seed) {} + + /// \brief Constructor. initialize each parameter. + /// \param task space region. + /// \param priority. + /// \par demeanor. + /// - set task space region. + /// - set priority. + RotationFirstTsrLinkConstraint( + const tmc_manipulation_types::TaskSpaceRegion& tsr, uint32_t priority) + : TsrLinkConstraint(tsr, priority) {} + + virtual ~RotationFirstTsrLinkConstraint() = default; + /// \brief Generate sample from tsr link constraint. + /// \return A sample in constraint. + + Eigen::Affine3d Sample() const override; + /// \brief Calcurate closest point in tsr link constraint. + /// \param origin_to_query pose from origin to query frame. + /// \return A pose from origin to closet frame to query frame. + Eigen::Affine3d CalcClosest(const Eigen::Affine3d& query) const override; + + protected: + /// \brief Return displacement region values to tsr link constraint + /// \param origin_to_query pose from origin to query frame. + /// \return displacemnt region value. + tmc_manipulation_types::RegionValues CalcDisplacementToTsr( + const Eigen::Affine3d& origin_to_query) const override; +}; + +} // namespace tmc_robot_local_planner + diff --git a/tmc_robot_local_planner/package.xml b/tmc_robot_local_planner/package.xml new file mode 100644 index 0000000..49488c3 --- /dev/null +++ b/tmc_robot_local_planner/package.xml @@ -0,0 +1,31 @@ + + tmc_robot_local_planner + 2.0.0 + The tmc_robot_local_planner package + HSR Support + + Keisuke Takeshita + Satoru Onoda + Yuta Watanabe + + BSD 3-clause Clear License + + ament_cmake + + angles + pluginlib + rclcpp + rclcpp_action + tmc_eigen_utils + tmc_manipulation_types + tmc_manipulation_types_bridge + tmc_planning_msgs + tmc_utils + + ament_cmake_gmock + ament_cmake_gtest + + + ament_cmake + + \ No newline at end of file diff --git a/tmc_robot_local_planner/src/tmc_robot_local_planner/range_joint_constraint.cpp b/tmc_robot_local_planner/src/tmc_robot_local_planner/range_joint_constraint.cpp new file mode 100644 index 0000000..6d35672 --- /dev/null +++ b/tmc_robot_local_planner/src/tmc_robot_local_planner/range_joint_constraint.cpp @@ -0,0 +1,187 @@ +/* +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. +*/ + +#include + +#include +#include +#include +#include + +#include + +using tmc_manipulation_types::PoseSeq; +using tmc_manipulation_types::Pose2dSeq; +using tmc_manipulation_types::RobotState; +using tmc_manipulation_types::ExtractJointPos; +using tmc_manipulation_types::ExtractMultiJointPos; +using tmc_manipulation_types::NameSeq; +using Eigen::Affine3d; +using Eigen::Vector3d; +using Eigen::VectorXd; +using Eigen::Translation3d; +using Eigen::AngleAxisd; + +namespace { + +/// \breif Convert Affine3d to 2D Pose +/// \return pose +Vector3d Get2DPose(const Affine3d& transform) { + return Vector3d( + transform.translation().x(), + transform.translation().y(), + transform.rotation().eulerAngles(0, 1, 2).z()); +} + +/// \breif Convert Affine2d to 3D Pose +/// \return pose +Affine3d GetTransform(const Vector3d& pose2d) { + return Translation3d(pose2d.x(), pose2d.y(), 0) * + AngleAxisd(pose2d.z(), Eigen::Vector3d::UnitZ()); +} +} // anonymous namespace + +namespace tmc_robot_local_planner { + +RangeJointConstraint::RangeJointConstraint( + const RobotState& min, + const RobotState& max, + uint32_t priority, + uint32_t seed) : priority_(priority) { + // check size joint state name. + if (min.joint_state.name.size() < 1 && min.multi_dof_joint_state.names.size() < 1 && + max.joint_state.name.size() < 1 && max.multi_dof_joint_state.names.size() < 1) { + throw std::range_error("input error. no input in the joint state name"); + } + // compare size joint state name. + if (min.joint_state.name.size() != max.joint_state.name.size()) { + throw std::range_error("input error. joint state range is different between min and max"); + } + // compare joint name between max and min. + for (uint32_t i = 0; i < min.joint_state.name.size(); ++i) { + if (max.joint_state.name[i] != min.joint_state.name[i]) { + throw std::runtime_error("input error. joint state vector component is different between min and max."); + } + } + // size joint state value compare. + if (min.joint_state.name.size() != min.joint_state.position.size() || + max.joint_state.name.size() != max.joint_state.position.size()) { + throw std::range_error("size error. differnt size between joint state name and position"); + } + srand(seed); + joint_names_ = min.joint_state.name; + multi_dof_joint_names_ = min.multi_dof_joint_state.names; + ExtractJointPos(min.joint_state, joint_names_, min_pos_); + ExtractJointPos(max.joint_state, joint_names_, max_pos_); + tmc_manipulation_types::PoseSeq min_poses; + tmc_manipulation_types::PoseSeq max_poses; + ExtractMultiJointPos(min.multi_dof_joint_state, multi_dof_joint_names_, min_poses); + ExtractMultiJointPos(max.multi_dof_joint_state, multi_dof_joint_names_, max_poses); + diff_pos_ = max_pos_ - min_pos_; + for (uint32_t i = 0; i < min_poses.size(); ++i) { + auto min = min_poses[i]; + auto max = max_poses[i]; + auto min_2d = Get2DPose(min); + auto max_2d = Get2DPose(max); + min_2d_poses_.push_back(min_2d); + max_2d_poses_.push_back(max_2d); + Vector3d diff_2d = max_2d - min_2d; + diff_2d[2] = angles::shortest_angular_distance(min_2d[2], max_2d[2]); + diff_2d_poses_.push_back(diff_2d); + } +} + +NameSeq RangeJointConstraint::GetJointName() const { + return joint_names_; +} + +uint32_t RangeJointConstraint::GetPriority() const { + return priority_; +} + +RobotState RangeJointConstraint::Sample() const { + VectorXd random_pos = min_pos_.array() + diff_pos_.array() * 0.5 * (VectorXd::Random(min_pos_.size()).array() + 1); + PoseSeq random_poses; + // note: implemented only 2D pose. + for (uint32_t i = 0; i < min_2d_poses_.size(); ++i) { + auto random_2d = min_2d_poses_[i].array() + diff_2d_poses_[i].array() * 0.5 * (Vector3d::Random().array() + 1); + auto random_pose = GetTransform(random_2d); + random_poses.push_back(random_pose); + } + RobotState sample; + sample.joint_state.name = joint_names_; + sample.joint_state.position = random_pos; + sample.multi_dof_joint_state.names = multi_dof_joint_names_; + sample.multi_dof_joint_state.poses = random_poses; + return sample; +} + +// RobotState RangeJointConstraint::CalcClosest(const RobotState& state) const { +// // not implemeted yet. +// return RobotState(); +// } + +// bool RangeJointConstraint::IsInConstraint(const RobotState& state) const { +// return true; +// } + +double RangeJointConstraint::CalcDisplacement(const RobotState& state) const { + const auto [disp_pos, disp_pose] = CalcSeparateDisplacements(state); + return disp_pos.norm() + disp_pose.norm(); +} + +std::tuple RangeJointConstraint::CalcSeparateDisplacements( + const tmc_manipulation_types::RobotState& state) const { + VectorXd state_pos; + ExtractJointPos(state.joint_state, joint_names_, state_pos); + VectorXd disp_pos; + disp_pos.resize(min_pos_.size()); + for (uint32_t i = 0; i < min_pos_.size(); ++i) { + disp_pos(i) = std::max(min_pos_(i) - state_pos(i), 0.0) + + std::max(state_pos(i) - max_pos_(i), 0.0); + } + + // TODO(Yuta watanabe): should implement multijoint_pose in correct angle norm. + VectorXd disp_pose; + PoseSeq state_poses; + ExtractMultiJointPos(state.multi_dof_joint_state, multi_dof_joint_names_, state_poses); + disp_pose.resize(min_2d_poses_.size()); + for (uint32_t i = 0; i < min_2d_poses_.size(); ++i) { + auto state_pose = state_poses[i]; + auto state_2d = Get2DPose(state_pose); + Vector3d disp_2d; + for (uint32_t j = 0; j < 3; ++j) { + disp_2d(j) = std::max(min_2d_poses_[i](j) - state_2d(j), 0.0) + + std::max(state_2d(j) - max_2d_poses_[i](j), 0.0); + } + disp_pose(i) = disp_2d.norm(); + } + return std::make_tuple(disp_pos, disp_pose); +} + +} // namespace tmc_robot_local_planner diff --git a/tmc_robot_local_planner/src/tmc_robot_local_planner/robot_local_planner.cpp b/tmc_robot_local_planner/src/tmc_robot_local_planner/robot_local_planner.cpp new file mode 100644 index 0000000..05cc755 --- /dev/null +++ b/tmc_robot_local_planner/src/tmc_robot_local_planner/robot_local_planner.cpp @@ -0,0 +1,430 @@ +/* +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. +*/ +#include "tmc_robot_local_planner/robot_local_planner.hpp" + +#include + +#include + +using tmc_planning_msgs::action::EvaluateRobotTrajectories; +using tmc_planning_msgs::action::GenerateRobotTrajectories; +using tmc_planning_msgs::action::OptimizeRobotTrajectory; +using tmc_planning_msgs::action::ValidateRobotTrajectories; + +namespace { +constexpr double kDefaultWaitForResultTimeout = 30.0; + +template +bool WaitForFutureComplete(const typename std::shared_future& future, + const rclcpp::Clock::SharedPtr& clock, double timeout, std::function interrupt) { + const auto timeout_stamp = clock->now() + rclcpp::Duration::from_seconds(timeout); + while (future.wait_for(std::chrono::microseconds(100)) == std::future_status::timeout) { + if (clock->now() > timeout_stamp) { + return false; + } + if (interrupt()) { + return false; + } + } + return true; +} + +template +bool ActionProcess(const Goal& goal, Client& client, Result& result, + const rclcpp::Clock::SharedPtr& clock, double timeout, std::function interrupt) { + if (interrupt()) { + return false; + } + auto future_goal_handle = client->async_send_goal(goal); + if (!WaitForFutureComplete(future_goal_handle, clock, timeout, []() { return false; })) { + return false; + } + + auto goal_handle = future_goal_handle.get(); + if (!goal_handle) { + return false; + } + + auto future_result = client->async_get_result(goal_handle); + if (!WaitForFutureComplete(future_result, clock, timeout, interrupt)) { + client->async_cancel_goal(goal_handle); + if (!WaitForFutureComplete(future_result, clock, timeout, []() { return false; })) { + return false; + } + } + + auto wrapped_result = future_result.get(); + if (static_cast(wrapped_result.code) != action_msgs::msg::GoalStatus::STATUS_SUCCEEDED) { + return false; + } + + result = wrapped_result.result; + return true; +} +} // namespace + +namespace tmc_robot_local_planner { + +void RobotLocalPlannerPlugins::Initialize(const rclcpp::Node::SharedPtr& node) { + generator->Initialize(node); + evaluator->Initialize(node); + validator->Initialize(node); + optimizer->Initialize(node); +} + +RobotLocalPlannerPlugins::Loader::Loader() + : generator_loader_("tmc_robot_local_planner", "tmc_robot_local_planner::IGenerator"), + evaluator_loader_("tmc_robot_local_planner", "tmc_robot_local_planner::IEvaluator"), + validator_loader_("tmc_robot_local_planner", "tmc_robot_local_planner::IValidator"), + optimizer_loader_("tmc_robot_local_planner", "tmc_robot_local_planner::IOptimizer") {} + +RobotLocalPlannerPlugins RobotLocalPlannerPlugins::Loader::CreatePluginInstances(const PluginNames& names) { + RobotLocalPlannerPlugins plugins; + plugins.generator = generator_loader_.createSharedInstance(names.generate_action); + plugins.evaluator = evaluator_loader_.createSharedInstance(names.evaluate_action); + plugins.validator = validator_loader_.createSharedInstance(names.validate_action); + plugins.optimizer = optimizer_loader_.createSharedInstance(names.optimize_action); + return plugins; +} + + +VisualizationPublisher::VisualizationPublisher(const rclcpp::Node::SharedPtr& node) { + publish_generated_trajectories_ = std::make_shared>( + node, "publish_generated_trajectories", true); + set_param_handlers_.emplace_back(node->add_on_set_parameters_callback( + std::bind(&tmc_utils::DynamicParameter::SetParameterCallback, + publish_generated_trajectories_, std::placeholders::_1))); + + publish_planned_trajectory_ = std::make_shared>( + node, "publish_planned_trajectory", true); + set_param_handlers_.emplace_back(node->add_on_set_parameters_callback( + std::bind(&tmc_utils::DynamicParameter::SetParameterCallback, + publish_planned_trajectory_, std::placeholders::_1))); + + generated_trajectories_pub_ = node->create_publisher( + "~/generated_trajectories", rclcpp::SensorDataQoS()); + planned_trajectory_pub_ = node->create_publisher( + "~/planned_trajectory", rclcpp::SensorDataQoS()); +} + +void VisualizationPublisher::PublishGeneratedTrajectories( + const std::vector& trajectories) const { + // If you do not issue unnecessary conversions, you will get out immediately + if (!publish_generated_trajectories_->value()) { + return; + } + + std::vector trajectory_msgs(trajectories.size()); + for (auto i = 0u; i < trajectories.size(); ++i) { + tmc_manipulation_types_bridge::TimedRobotTrajectoryToRobotTrajectoryMsg(trajectories[i], trajectory_msgs[i]); + } + PublishGeneratedTrajectories(trajectory_msgs); +} + +void VisualizationPublisher::PublishGeneratedTrajectories( + const std::vector& trajectories) const { + if (publish_generated_trajectories_->value()) { + moveit_msgs::msg::DisplayTrajectory display_trajectory; + display_trajectory.trajectory = trajectories; + generated_trajectories_pub_->publish(display_trajectory); + } +} + +void VisualizationPublisher::PublishPlannedTrajectory( + const tmc_manipulation_types::TimedRobotTrajectory& trajectory) const { + if (!publish_planned_trajectory_->value()) { + return; + } + + moveit_msgs::msg::RobotTrajectory trajectory_msg; + tmc_manipulation_types_bridge::TimedRobotTrajectoryToRobotTrajectoryMsg(trajectory, trajectory_msg); + planned_trajectory_pub_->publish(trajectory_msg); +} + +void VisualizationPublisher::PublishPlannedTrajectory(const moveit_msgs::msg::RobotTrajectory& trajectory) const { + if (!publish_planned_trajectory_->value()) { + return; + } + planned_trajectory_pub_->publish(trajectory); +} + +RobotLocalPlannerComposition::RobotLocalPlannerComposition( + const rclcpp::Node::SharedPtr& node, + const TrajectoryMerger::Ptr& trajectory_merger, + const RobotLocalPlannerPlugins& plugins) + : plugins_(plugins), + trajectory_merger_(trajectory_merger), + logger_(node->get_logger()) { + optimize_second_ = tmc_utils::GetParameter(node, "optimize_second", false); + visualization_pub_ = std::make_shared(node); +} + +RobotLocalPlannerComposition::RobotLocalPlannerComposition( + const rclcpp::Node::SharedPtr& node, const RobotLocalPlannerPlugins& plugins) + : RobotLocalPlannerComposition(node, nullptr, plugins) {} + +std::future RobotLocalPlannerComposition::PlanPath( + const Constraints& goal_constraints, + const tmc_manipulation_types::RobotState& initial_state, + double normalized_vel) { + return PlanPath(goal_constraints, initial_state, normalized_vel, {}); +} + +std::future RobotLocalPlannerComposition::PlanPath( + const Constraints& goal_constraints, + const tmc_manipulation_types::RobotState& initial_state, + double normalized_vel, + const std::vector& ignore_joints) { + auto func = std::bind(&RobotLocalPlannerComposition::PlanPathImpl, this, + goal_constraints, initial_state, normalized_vel, ignore_joints); + return std::async(std::launch::async, func); +} + +tmc_manipulation_types::TimedRobotTrajectory RobotLocalPlannerComposition::PlanPathImpl( + const Constraints& goal_constraints, + const tmc_manipulation_types::RobotState& initial_state, + double normalized_vel, + const std::vector& ignore_joints) { + set_is_interrupt_requested(false); + if (goal_constraints.hard_link_constraints.empty() && goal_constraints.hard_joint_constraints.empty()) { + RCLCPP_WARN(logger_, "No hard constraints."); + UpdateLastErrorCode(RobotLocalPlannerErrorCode::kConstraintsEmpty); + return tmc_manipulation_types::TimedRobotTrajectory(); + } + + // generator + std::vector trajectories_gen; + auto interrupt_func = std::bind(&RobotLocalPlannerComposition::is_interrupt_requested, this); + const bool gen_result = plugins_.generator->Generate( + goal_constraints, initial_state, normalized_vel, ignore_joints, + interrupt_func, trajectories_gen); + if (!gen_result) { + RCLCPP_WARN(logger_, "Failed to generator."); + UpdateLastErrorCode(RobotLocalPlannerErrorCode::kGenerationFailure); + return tmc_manipulation_types::TimedRobotTrajectory(); + } + + std::vector trajectories_eval; + if (optimize_second_) { + // optimize -> merge -> evaluate + std::vector trajectories_opt; + bool opt_result = plugins_.optimizer->Optimize(trajectories_gen, interrupt_func, trajectories_opt); + if (!opt_result) { + RCLCPP_WARN(logger_, "Failed to optimzier."); + UpdateLastErrorCode(RobotLocalPlannerErrorCode::kOptimizationFailure); + return tmc_manipulation_types::TimedRobotTrajectory(); + } + if (trajectory_merger_) { + trajectory_merger_->MergeTrajectory(trajectories_opt); + } + bool eval_result = plugins_.evaluator->Evaluate( + goal_constraints, trajectories_opt, interrupt_func, trajectories_eval); + if (!eval_result) { + RCLCPP_WARN(logger_, "Failed to evaluator."); + UpdateLastErrorCode(RobotLocalPlannerErrorCode::kEvaluationFailure); + return tmc_manipulation_types::TimedRobotTrajectory(); + } + } else { + // merge -> evaluate + if (trajectory_merger_) { + trajectory_merger_->MergeTrajectory(trajectories_gen); + } + bool eval_result = plugins_.evaluator->Evaluate( + goal_constraints, trajectories_gen, interrupt_func, trajectories_eval); + if (!eval_result) { + RCLCPP_WARN(logger_, "Failed to evaluator."); + UpdateLastErrorCode(RobotLocalPlannerErrorCode::kEvaluationFailure); + return tmc_manipulation_types::TimedRobotTrajectory(); + } + } + visualization_pub_->PublishGeneratedTrajectories(trajectories_eval); + + // validator + tmc_manipulation_types::TimedRobotTrajectory trajectory_val; + bool val_result = plugins_.validator->Validate(trajectories_eval, interrupt_func, trajectory_val); + if (!val_result) { + RCLCPP_WARN(logger_, "Failed to validator."); + UpdateLastErrorCode(RobotLocalPlannerErrorCode::kValidationFailure); + return tmc_manipulation_types::TimedRobotTrajectory(); + } + + if (optimize_second_) { + UpdateLastErrorCode(RobotLocalPlannerErrorCode::kSuccess); + visualization_pub_->PublishPlannedTrajectory(trajectory_val); + return trajectory_val; + } else { + tmc_manipulation_types::TimedRobotTrajectory trajectory_opt; + bool opt_result = plugins_.optimizer->Optimize(trajectory_val, interrupt_func, trajectory_opt); + if (!opt_result) { + RCLCPP_WARN(logger_, "Failed to optimzier."); + UpdateLastErrorCode(RobotLocalPlannerErrorCode::kOptimizationFailure); + return tmc_manipulation_types::TimedRobotTrajectory(); + } + UpdateLastErrorCode(RobotLocalPlannerErrorCode::kSuccess); + visualization_pub_->PublishPlannedTrajectory(trajectory_opt); + return trajectory_opt; + } +} + +void RobotLocalPlannerComposition::InterruptPlanPath() { + set_is_interrupt_requested(true); +} + + +RobotLocalPlanner::ActionNames::ActionNames() + : generate_action("generate"), + evaluate_action("evaluate"), + validate_action("validate"), + optimize_action("optimize") {} + +RobotLocalPlanner::RobotLocalPlanner(const rclcpp::Node::SharedPtr& node, + const TrajectoryMerger::Ptr& trajectory_merger, + const ActionNames& action_names) + : trajectory_merger_(trajectory_merger), + clock_(node->get_clock()), + action_timeout_(kDefaultWaitForResultTimeout), + logger_(node->get_logger()) { + generator_client_ = rclcpp_action::create_client(node, action_names.generate_action); + evaluator_client_ = rclcpp_action::create_client(node, action_names.evaluate_action); + validator_client_ = rclcpp_action::create_client(node, action_names.validate_action); + optimizer_client_ = rclcpp_action::create_client(node, action_names.optimize_action); + visualization_pub_ = std::make_shared(node); +} + +RobotLocalPlanner::RobotLocalPlanner(const rclcpp::Node::SharedPtr& node, + const TrajectoryMerger::Ptr& trajectory_merger) + : RobotLocalPlanner(node, trajectory_merger, ActionNames()) {} + +RobotLocalPlanner::RobotLocalPlanner(const rclcpp::Node::SharedPtr& node, const ActionNames& action_names) + : RobotLocalPlanner(node, nullptr, action_names) {} + +RobotLocalPlanner::RobotLocalPlanner(const rclcpp::Node::SharedPtr& node) + : RobotLocalPlanner(node, nullptr, ActionNames()) {} + +bool RobotLocalPlanner::AreActionServersReady() const { + return generator_client_->action_server_is_ready() && evaluator_client_->action_server_is_ready() && + validator_client_->action_server_is_ready() && optimizer_client_->action_server_is_ready(); +} + +std::future RobotLocalPlanner::PlanPath( + const tmc_planning_msgs::msg::Constraints& goal_constraints, + const moveit_msgs::msg::RobotState& initial_state, + double normalized_vel) { + return PlanPath(goal_constraints, initial_state, normalized_vel, {}); +} + +std::future RobotLocalPlanner::PlanPath( + const tmc_planning_msgs::msg::Constraints& goal_constraints, + const moveit_msgs::msg::RobotState& initial_state, + double normalized_vel, + const std::vector& ignore_joints) { + auto func = std::bind(&RobotLocalPlanner::PlanPathImpl, this, + goal_constraints, initial_state, normalized_vel, ignore_joints); + return std::async(std::launch::async, func); +} + +moveit_msgs::msg::RobotTrajectory RobotLocalPlanner::PlanPathImpl( + const tmc_planning_msgs::msg::Constraints& goal_constraints, + const moveit_msgs::msg::RobotState& initial_state, + double normalized_vel, + const std::vector& ignore_joints) { + set_is_interrupt_requested(false); + if (goal_constraints.hard_link_constraints.empty() && goal_constraints.hard_joint_constraints.empty()) { + RCLCPP_WARN(logger_, "No hard constraints."); + UpdateLastErrorCode(RobotLocalPlannerErrorCode::kConstraintsEmpty); + return moveit_msgs::msg::RobotTrajectory(); + } + + // generator + tmc_planning_msgs::action::GenerateRobotTrajectories::Goal generator_goal; + generator_goal.constraints = goal_constraints; + generator_goal.initial_state = initial_state; + generator_goal.normalized_velocity = normalized_vel; + generator_goal.ignore_joints = ignore_joints; + + tmc_planning_msgs::action::GenerateRobotTrajectories::Result::SharedPtr generator_result; + if (!ActionProcess(generator_goal, generator_client_, generator_result, clock_, action_timeout_, + std::bind(&RobotLocalPlanner::is_interrupt_requested, this))) { + RCLCPP_WARN(logger_, "Failed to generator."); + UpdateLastErrorCode(RobotLocalPlannerErrorCode::kGenerationFailure); + return moveit_msgs::msg::RobotTrajectory(); + } + + // merge trajectory + if (trajectory_merger_) { + trajectory_merger_->MergeTrajectory(generator_result->robot_trajectories); + } + + // evaluator + tmc_planning_msgs::action::EvaluateRobotTrajectories::Goal evaluator_goal; + evaluator_goal.robot_trajectories.swap(generator_result->robot_trajectories); + evaluator_goal.constraints = goal_constraints; + + tmc_planning_msgs::action::EvaluateRobotTrajectories::Result::SharedPtr evaluator_result; + if (!ActionProcess(evaluator_goal, evaluator_client_, evaluator_result, clock_, action_timeout_, + std::bind(&RobotLocalPlanner::is_interrupt_requested, this))) { + RCLCPP_WARN(logger_, "Failed to evaluator."); + UpdateLastErrorCode(RobotLocalPlannerErrorCode::kEvaluationFailure); + return moveit_msgs::msg::RobotTrajectory(); + } + visualization_pub_->PublishGeneratedTrajectories(evaluator_result->robot_trajectories); + + // validator + tmc_planning_msgs::action::ValidateRobotTrajectories::Goal validator_goal; + validator_goal.robot_trajectories.swap(evaluator_result->robot_trajectories); + + tmc_planning_msgs::action::ValidateRobotTrajectories::Result::SharedPtr validator_result; + if (!ActionProcess(validator_goal, validator_client_, validator_result, clock_, action_timeout_, + std::bind(&RobotLocalPlanner::is_interrupt_requested, this))) { + RCLCPP_WARN(logger_, "Failed to validator."); + UpdateLastErrorCode(RobotLocalPlannerErrorCode::kValidationFailure); + return moveit_msgs::msg::RobotTrajectory(); + } + + // optimizer + tmc_planning_msgs::action::OptimizeRobotTrajectory::Goal optimizer_goal; + optimizer_goal.robot_trajectory = validator_result->robot_trajectory; + + tmc_planning_msgs::action::OptimizeRobotTrajectory::Result::SharedPtr optimizer_result; + if (!ActionProcess(optimizer_goal, optimizer_client_, optimizer_result, clock_, action_timeout_, + std::bind(&RobotLocalPlanner::is_interrupt_requested, this))) { + RCLCPP_WARN(logger_, "Failed to optimzier."); + UpdateLastErrorCode(RobotLocalPlannerErrorCode::kOptimizationFailure); + return moveit_msgs::msg::RobotTrajectory(); + } + + UpdateLastErrorCode(RobotLocalPlannerErrorCode::kSuccess); + visualization_pub_->PublishPlannedTrajectory(optimizer_result->robot_trajectory); + return optimizer_result->robot_trajectory; +} + +void RobotLocalPlanner::InterruptPlanPath() { + set_is_interrupt_requested(true); +} + +} // namespace tmc_robot_local_planner diff --git a/tmc_robot_local_planner/src/tmc_robot_local_planner/tsr_link_constraint.cpp b/tmc_robot_local_planner/src/tmc_robot_local_planner/tsr_link_constraint.cpp new file mode 100644 index 0000000..82debac --- /dev/null +++ b/tmc_robot_local_planner/src/tmc_robot_local_planner/tsr_link_constraint.cpp @@ -0,0 +1,229 @@ +/* +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. +*/ + +#include +#include +#include + +#include + +#include "tmc_robot_local_planner/tsr_link_constraint.hpp" + +using tmc_manipulation_types::RegionValues; +using tmc_manipulation_types::TaskSpaceRegion; +using Eigen::Affine3d; +using Eigen::Vector3d; +using Eigen::Quaterniond; +using Eigen::Translation3d; + +namespace { + +const double kTsrNearThreshold = 1e-3; +const double kThresholdSingularRPY = 0.99999999; + +/// \breif Convert RegionValues to Pose +/// \param region_vals x, y, z, roll, pitch, yaw +/// \return pose +Affine3d RegionValuesToPose(const RegionValues& region_vals) { + Vector3d pos = region_vals.segment<3>(0); + Vector3d rpy = region_vals.segment<3>(3); + return Translation3d(pos) * tmc_eigen_utils::RPYToQuaternion(rpy); +} + +/// \breif Convert Pose ot RegionValues +/// \param pose pose SE(3) +/// \return region x, y, z, roll, pitch, yaw +RegionValues PoseToRegionValues(const Affine3d& pose) { + Vector3d pos = pose.translation(); + Vector3d rpy = tmc_eigen_utils::QuaternionToRPY(Quaterniond(pose.linear())); + RegionValues region_val; + region_val << pos, rpy; + return region_val; +} + +/// \breif Convert RegionValues to Pose +/// \param region_vals x, y, z, roll, pitch, yaw +/// \return pose +Affine3d RotationFirstRegionValuesToPose(const RegionValues& region_vals) { + const Vector3d pos = region_vals.segment<3>(0); + const Vector3d rpy = region_vals.segment<3>(3); + const Affine3d linear = Translation3d::Identity() * tmc_eigen_utils::RPYToQuaternion(rpy); + const Affine3d trans = Translation3d(pos) * Quaterniond::Identity(); + return linear * trans; +} + +/// \breif Convert Pose ot RegionValues +/// \param pose pose SE(3) +/// \return region x, y, z, roll, pitch, yaw +RegionValues RotationFirstPoseToRegionValues(const Affine3d& pose) { + const Vector3d rpy = tmc_eigen_utils::QuaternionToRPY(Quaterniond(pose.linear())); + const Affine3d linear = Translation3d::Identity() * tmc_eigen_utils::RPYToQuaternion(rpy); + const Affine3d trans = linear.inverse() * pose; + const Vector3d pos = trans.translation(); + RegionValues region_val; + region_val << pos, rpy; + return region_val; +} + +RegionValues CalcDisplacementImpl(const TaskSpaceRegion& tsr, const Vector3d& disp_pos, const Vector3d& disp_rot) { + Vector3d displacement_pos; + for (int32_t i = 0; i < 3; ++i) { + if (disp_pos(i) < tsr.min_bounds(i)) { + displacement_pos(i) = disp_pos(i) - tsr.min_bounds(i); + } else if (disp_pos(i) > tsr.max_bounds(i)) { + displacement_pos(i) = disp_pos(i) - tsr.max_bounds(i); + } else { + displacement_pos(i) = 0.0; + } + } + + std::vector rpy_offsets(9); + rpy_offsets[0] << 0.0, 0.0, 0.0; + rpy_offsets[1] << M_PI, M_PI, M_PI; + rpy_offsets[2] << M_PI, M_PI, -M_PI; + rpy_offsets[3] << M_PI, -M_PI, M_PI; + rpy_offsets[4] << M_PI, -M_PI, -M_PI; + rpy_offsets[5] << -M_PI, M_PI, M_PI; + rpy_offsets[6] << -M_PI, M_PI, -M_PI; + rpy_offsets[7] << -M_PI, -M_PI, M_PI; + rpy_offsets[8] << -M_PI, -M_PI, -M_PI; + + Vector3d displacement_rot; + double min_norm = 1.0e10; + for (int32_t i = 0; i < rpy_offsets.size(); ++i) { + Vector3d disp_rot_with_offset = disp_rot + rpy_offsets[i]; + // Roll -m_pi requires pitch reversal + if (i > 4) { + disp_rot_with_offset[1] = -disp_rot_with_offset[1]; + } + + Vector3d disp_rot_with_bounds; + for (int32_t j = 0; j < 3; ++j) { + if (disp_rot_with_offset(j) < tsr.min_bounds(j + 3)) { + disp_rot_with_bounds(j) = disp_rot_with_offset(j) - tsr.min_bounds(j + 3); + } else if (disp_rot_with_offset(j) > tsr.max_bounds(j + 3)) { + disp_rot_with_bounds(j) = disp_rot_with_offset(j) - tsr.max_bounds(j + 3); + } else { + disp_rot_with_bounds(j) = 0.0; + } + } + + const double norm = disp_rot_with_bounds.norm(); + if (norm < min_norm) { + min_norm = norm; + displacement_rot = disp_rot_with_bounds; + } + } + RegionValues displacement; + displacement << displacement_pos, displacement_rot; + return displacement; +} + +} // anonymous namespace + +namespace tmc_robot_local_planner { + +TsrLinkConstraint::TsrLinkConstraint( + const tmc_manipulation_types::TaskSpaceRegion& tsr, + uint32_t priority, + uint32_t seed) : + tsr_(tsr), priority_(priority) { + srand(seed); +} + +std::string TsrLinkConstraint::GetLinkName() const { + return tsr_.end_frame_id; +} + +std::string TsrLinkConstraint::GetOriginName() const { + return tsr_.origin_frame_id; +} + +uint32_t TsrLinkConstraint::GetPriority() const { + return priority_; +} + +Affine3d TsrLinkConstraint::Sample() const { + RegionValues random = (tsr_.max_bounds - tsr_.min_bounds).array() * + RegionValues::Random().array().abs() + tsr_.min_bounds.array(); + return tsr_.origin_to_tsr * RegionValuesToPose(random) * tsr_.tsr_to_end; +} + +Affine3d TsrLinkConstraint::CalcClosest(const Affine3d& origin_to_query) const { + Affine3d origin_to_query_dash = origin_to_query * tsr_.tsr_to_end.inverse(); + Affine3d tsr_to_query_dash = tsr_.origin_to_tsr.inverse() * origin_to_query_dash; + RegionValues query_dash = PoseToRegionValues(tsr_to_query_dash); + RegionValues displacement = CalcDisplacementToTsr(origin_to_query); + return tsr_.origin_to_tsr * RegionValuesToPose(query_dash - displacement) * tsr_.tsr_to_end; +} + +bool TsrLinkConstraint::IsInConstraint(const Affine3d& origin_to_query) const { + return CalcDisplacement(origin_to_query) < kTsrNearThreshold; +} + +double TsrLinkConstraint::CalcDisplacement(const Affine3d& origin_to_query) const { + return CalcDisplacementToTsr(origin_to_query).norm(); +} + +RegionValues TsrLinkConstraint::CalcSeparateDisplacements(const Eigen::Affine3d& origin_to_query) const { + return CalcDisplacementToTsr(origin_to_query); +} + +tmc_manipulation_types::RegionValues +TsrLinkConstraint::CalcDisplacementToTsr(const Affine3d& origin_to_query) const { + Affine3d origin_to_query_dash = origin_to_query * tsr_.tsr_to_end.inverse(); + Affine3d tsr_to_sample_dash = tsr_.origin_to_tsr.inverse() * origin_to_query_dash; + Vector3d disp_pos = tsr_to_sample_dash.translation(); + Vector3d disp_rot = tsr_to_sample_dash.linear().eulerAngles(0, 1, 2); + return CalcDisplacementImpl(tsr_, disp_pos, disp_rot); +} + +Affine3d RotationFirstTsrLinkConstraint::Sample() const { + const RegionValues random = (tsr_.max_bounds - tsr_.min_bounds).array() * + RegionValues::Random().array().abs() + tsr_.min_bounds.array(); + return tsr_.origin_to_tsr * RotationFirstRegionValuesToPose(random) * tsr_.tsr_to_end; +} + +Affine3d RotationFirstTsrLinkConstraint::CalcClosest(const Affine3d& origin_to_query) const { + const Affine3d origin_to_query_dash = origin_to_query * tsr_.tsr_to_end.inverse(); + const Affine3d tsr_to_query_dash = tsr_.origin_to_tsr.inverse() * origin_to_query_dash; + RegionValues query_dash = RotationFirstPoseToRegionValues(tsr_to_query_dash); + RegionValues displacement = CalcDisplacementToTsr(origin_to_query); + return tsr_.origin_to_tsr * RotationFirstRegionValuesToPose(query_dash - displacement) * tsr_.tsr_to_end; +} + +RegionValues RotationFirstTsrLinkConstraint::CalcDisplacementToTsr(const Affine3d& origin_to_query) const { + const Affine3d origin_to_query_dash = origin_to_query * tsr_.tsr_to_end.inverse(); + const Affine3d tsr_to_query_dash = tsr_.origin_to_tsr.inverse() * origin_to_query_dash; + const RegionValues query_dash = RotationFirstPoseToRegionValues(tsr_to_query_dash); + const Vector3d disp_pos = query_dash.segment<3>(0); + const Vector3d disp_rot = query_dash.segment<3>(3); + return CalcDisplacementImpl(tsr_, disp_pos, disp_rot); +} + +} // namespace tmc_robot_local_planner diff --git a/tmc_robot_local_planner/test/range_joint_constraint-test.cpp b/tmc_robot_local_planner/test/range_joint_constraint-test.cpp new file mode 100644 index 0000000..87971bc --- /dev/null +++ b/tmc_robot_local_planner/test/range_joint_constraint-test.cpp @@ -0,0 +1,215 @@ +/* +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. +*/ + +#include +#include +#include +#include + +using Eigen::Affine3d; +using Eigen::AngleAxisd; +using Eigen::Translation3d; +using Eigen::Vector3d; +using tmc_manipulation_types::RobotState; + +namespace { +const uint32_t kSampleTestNumberOfTime = 100; +constexpr double kEpsilon = 1e-5; +} // anonymous namespace + +namespace tmc_robot_local_planner { + +// class for range joint constraint test +// set temp values for range joint constraint +class RangeJointConstraintTest : public ::testing::Test { + protected: + virtual void SetUp() { + RobotState min; + RobotState max; + min.joint_state.name = {"joint1", "joint2"}; + min.joint_state.position.resize(2); + min.joint_state.position << -0.1, -0.2; + max.joint_state.name = {"joint1", "joint2"}; + max.joint_state.position.resize(2); + max.joint_state.position << 0.1, 0.2; + min.multi_dof_joint_state.names = {"origin"}; + min.multi_dof_joint_state.poses.resize(1); + min.multi_dof_joint_state.poses[0] = Translation3d(-0.1, -0.2, 0) * + AngleAxisd(-0.5, Vector3d::UnitZ()); + max.multi_dof_joint_state.names = {"origin"}; + max.multi_dof_joint_state.poses.resize(1); + max.multi_dof_joint_state.poses[0] = Translation3d(0.1, 0.2, 0) * + AngleAxisd(0.5, Vector3d::UnitZ()); + joint_const_ = std::make_shared(min, max, 10, static_cast(time(NULL))); + } + IJointConstraint::Ptr joint_const_; +}; + +// ubnormal test. joint name size test +TEST_F(RangeJointConstraintTest, UbnormalNoJointNameTest) { + RobotState min; + RobotState max; + ASSERT_THROW(new RangeJointConstraint(min, max, 10, static_cast(time(NULL))), std::range_error); +} + +// ubnormal test. joint name range test +TEST_F(RangeJointConstraintTest, UbnormalJointNameSizeTest) { + RobotState min; + RobotState max; + min.joint_state.name = {"joint1", "joint2"}; + min.joint_state.name = {"joint1"}; + ASSERT_THROW(new RangeJointConstraint(min, max, 10, static_cast(time(NULL))), std::range_error); +} + +// ubnormal test. joint name comparison test +TEST_F(RangeJointConstraintTest, UbnormalJointNameComparisionTest) { + RobotState min; + RobotState max; + min.joint_state.name = {"joint1", "joint2"}; + min.joint_state.name = {"joint1", "joint3"}; + ASSERT_THROW(new RangeJointConstraint(min, max, 10, static_cast(time(NULL))), std::runtime_error); +} + +// ubnormal joint value comparison +TEST_F(RangeJointConstraintTest, UbnormalJointValueComparisonTest) { + RobotState min; + RobotState max; + min.joint_state.name = {"joint1", "joint2"}; + min.joint_state.position.resize(1); + min.joint_state.position << -0.1; + max.joint_state.name = {"joint1", "joint2"}; + max.joint_state.position.resize(1); + max.joint_state.position << 0.2; + ASSERT_THROW(new RangeJointConstraint(min, max, 10, static_cast(time(NULL))), std::range_error); +} + +TEST_F(RangeJointConstraintTest, JointNameTest) { + RobotState min; + RobotState max; + min.joint_state.name = {"joint1"}; + min.joint_state.position.resize(1); + min.joint_state.position << -0.1; + max.joint_state.name = {"joint1"}; + max.joint_state.position.resize(1); + max.joint_state.position << 0.1; + ASSERT_NO_THROW(new RangeJointConstraint(min, max, 10, static_cast(time(NULL)))); +} + +TEST_F(RangeJointConstraintTest, MultiDofJointNameTest) { + RobotState min; + RobotState max; + min.multi_dof_joint_state.names = {"origin"}; + min.multi_dof_joint_state.poses.resize(1); + min.multi_dof_joint_state.poses[0] = Translation3d(-0.1, -0.2, 0) * + AngleAxisd(-0.5, Vector3d::UnitZ()); + max.multi_dof_joint_state.names = {"origin"}; + max.multi_dof_joint_state.poses.resize(1); + max.multi_dof_joint_state.poses[0] = Translation3d(0.1, 0.2, 0) * + AngleAxisd(0.5, Vector3d::UnitZ()); + ASSERT_NO_THROW(new RangeJointConstraint(min, max, 10, static_cast(time(NULL)))); +} + +// check joint name in range joint constraint +TEST_F(RangeJointConstraintTest, GetJointNameTest) { + EXPECT_EQ("joint1", joint_const_->GetJointName()[0]); + EXPECT_EQ("joint2", joint_const_->GetJointName()[1]); +} + +// check priority in range joint constraint +TEST_F(RangeJointConstraintTest, GetPriorityTest) { + EXPECT_EQ(10, joint_const_->GetPriority()); +} + +// check range joint samples pointed if they are within region pointed +TEST_F(RangeJointConstraintTest, SampleTest) { + for (int i = 0; i < kSampleTestNumberOfTime; i++) { + EXPECT_LE(joint_const_->Sample().joint_state.position[0], 0.1); + EXPECT_GE(joint_const_->Sample().joint_state.position[0], -0.1); + EXPECT_LE(joint_const_->Sample().joint_state.position[1], 0.2); + EXPECT_GE(joint_const_->Sample().joint_state.position[1], -0.2); + Affine3d pose = joint_const_->Sample().multi_dof_joint_state.poses[0]; + EXPECT_LE(pose.translation().transpose()[0], 0.1); + EXPECT_GE(pose.translation().transpose()[0], -0.1); + EXPECT_LE(pose.translation().transpose()[1], 0.2); + EXPECT_GE(pose.translation().transpose()[1], -0.2); + EXPECT_LE(AngleAxisd(pose.rotation()).angle(), 0.5); + EXPECT_GE(AngleAxisd(pose.rotation()).angle(), -0.5); + } +} + +// check robot state samples pointed if they are within region pointed +// TEST_F(RangeJointConstraintTest, IsInConstraint) { +// RobotState temp_state; +// EXPECT_TRUE(joint_const_->IsInConstraint(temp_state)); +// } + +// check the calucuration result of displacement to constraint +TEST_F(RangeJointConstraintTest, DisplacementTest) { + RobotState zero_state; + zero_state.joint_state.name = {"joint1", "joint2"}; + zero_state.joint_state.position.resize(2); + zero_state.joint_state.position << 0.0, 0.0; + zero_state.multi_dof_joint_state.names = {"origin"}; + zero_state.multi_dof_joint_state.poses.resize(1); + zero_state.multi_dof_joint_state.poses[0] = Eigen::Affine3d::Identity(); + { + const auto [disp_pos, disp_pose] = joint_const_->CalcSeparateDisplacements(zero_state); + ASSERT_EQ(disp_pos.size(), 2); + EXPECT_NEAR(0.0, disp_pos[0], kEpsilon); + EXPECT_NEAR(0.0, disp_pos[1], kEpsilon); + ASSERT_EQ(disp_pose.size(), 1); + EXPECT_NEAR(0.0, disp_pose[0], kEpsilon); + } + EXPECT_NEAR(0.0, joint_const_->CalcDisplacement(zero_state), kEpsilon); + + RobotState sample_state; + sample_state.joint_state.name = {"joint1", "joint2"}; + sample_state.joint_state.position.resize(2); + sample_state.joint_state.position << 0.2, 0.0; + sample_state.multi_dof_joint_state.names = {"origin"}; + sample_state.multi_dof_joint_state.poses.resize(1); + sample_state.multi_dof_joint_state.poses[0] = Translation3d(0.0, 0.0, 0) * + AngleAxisd(0.0, Vector3d::UnitZ()); + { + const auto [disp_pos, disp_pose] = joint_const_->CalcSeparateDisplacements(sample_state); + ASSERT_EQ(disp_pos.size(), 2); + EXPECT_NEAR(0.1, disp_pos[0], kEpsilon); + EXPECT_NEAR(0.0, disp_pos[1], kEpsilon); + ASSERT_EQ(disp_pose.size(), 1); + EXPECT_NEAR(0.0, disp_pose[0], kEpsilon); + } + EXPECT_NEAR(0.1, joint_const_->CalcDisplacement(sample_state), kEpsilon); +} + + +} // namespace tmc_robot_local_planner + +int main(int argc, char **argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/tmc_robot_local_planner/test/robot_local_planner-test.cpp b/tmc_robot_local_planner/test/robot_local_planner-test.cpp new file mode 100644 index 0000000..4287846 --- /dev/null +++ b/tmc_robot_local_planner/test/robot_local_planner-test.cpp @@ -0,0 +1,660 @@ +/* +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. +*/ +#include + +#include +#include + +#include + +#include +#include + +namespace { + +template +class DummyBase { + public: + using Ptr = std::shared_ptr; + + DummyBase(const rclcpp::Node::SharedPtr& node, const std::string& action_name) + : node_(node), do_reject_(false), do_abort_(false), do_wait_(false), callback_time_(0) { + server_ = rclcpp_action::create_server( + node, action_name, + std::bind(&DummyBase::GoalCallback, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&DummyBase::CancelCallback, this, std::placeholders::_1), + std::bind(&DummyBase::FeedbackSetupCallback, this, std::placeholders::_1)); + } + + rclcpp::Time GetCallbackTime() const { return callback_time_; } + + void SetDoReject(bool value) { do_reject_ = value; } + void SetDoAbort(bool value) { do_abort_ = value; } + void SetDoWait(bool value) { do_wait_ = value; } + + protected: + using ServerGoalHandle = rclcpp_action::ServerGoalHandle; + using ServerGoalHandlePtr = std::shared_ptr; + + using GoalType = typename ActionType::Goal; + using ResultType = typename ActionType::Result; + + typename rclcpp_action::Server::SharedPtr server_; + + rclcpp_action::GoalResponse GoalCallback(const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal) { + if (do_reject_) { + return rclcpp_action::GoalResponse::REJECT; + } else { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + } + + rclcpp_action::CancelResponse CancelCallback(const ServerGoalHandlePtr goal_handle) { + return rclcpp_action::CancelResponse::ACCEPT; + } + + void FeedbackSetupCallback(ServerGoalHandlePtr goal_handle) { + std::thread{std::bind(&DummyBase::Execute, this, std::placeholders::_1), goal_handle}.detach(); + } + + void Execute(const ServerGoalHandlePtr goal_handle) { + callback_time_ = node_->now(); + + const auto goal = goal_handle->get_goal(); + // It is first to use result, but call it here because it will store the variables required for the test. + const auto result = ExecuteImpl(goal); + + if (do_abort_) { + goal_handle->abort(result); + return; + } + if (do_wait_) { + const auto timeout = node_->now() + rclcpp::Duration::from_seconds(1.0); + while (node_->now() < timeout) { + if (goal_handle->is_canceling()) { + goal_handle->canceled(result); + return; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + } + goal_handle->succeed(result); + } + + virtual std::shared_ptr ExecuteImpl(const std::shared_ptr& goal) = 0; + + private: + rclcpp::Node::SharedPtr node_; + + bool do_reject_; + bool do_abort_; + bool do_wait_; + + rclcpp::Time callback_time_; +}; + +class DummyGenerator : public DummyBase { + public: + using Ptr = std::shared_ptr; + + explicit DummyGenerator(const rclcpp::Node::SharedPtr& node, const std::string& prefix = "") + : DummyBase(node, prefix + "generate") {} + virtual ~DummyGenerator() = default; + + std::vector ignore_joints() const { return ignore_joints_; } + + protected: + std::shared_ptr ExecuteImpl(const std::shared_ptr& goal) override { + ignore_joints_ = goal->ignore_joints; + + moveit_msgs::msg::RobotTrajectory trajectory; + trajectory.joint_trajectory.joint_names.push_back( + goal->constraints.hard_joint_constraints[0].min.joint_state.name[0]); + trajectory_msgs::msg::JointTrajectoryPoint joint_trajectory; + joint_trajectory.positions.push_back( + goal->constraints.hard_joint_constraints[0].min.joint_state.position[0]); + trajectory.joint_trajectory.points.push_back(joint_trajectory); + + auto result = std::make_shared(); + result->robot_trajectories.push_back(trajectory); + return result; + } + + private: + std::vector ignore_joints_; +}; + +class DummyEvaluator : public DummyBase { + public: + using Ptr = std::shared_ptr; + + explicit DummyEvaluator(const rclcpp::Node::SharedPtr& node, const std::string& prefix = "") + : DummyBase(node, prefix + "evaluate") {} + virtual ~DummyEvaluator() = default; + + uint32_t GetTrajectorySize() { return trajectory_size_; } + + protected: + std::shared_ptr ExecuteImpl(const std::shared_ptr& goal) override { + trajectory_size_ = goal->robot_trajectories.size(); + + auto result = std::make_shared(); + result->robot_trajectories = goal->robot_trajectories; + return result; + } + + private: + uint32_t trajectory_size_; +}; + +class DummyValidator : public DummyBase { + public: + using Ptr = std::shared_ptr; + + explicit DummyValidator(const rclcpp::Node::SharedPtr& node, const std::string& prefix = "") + : DummyBase(node, prefix + "validate") {} + virtual ~DummyValidator() = default; + + protected: + std::shared_ptr ExecuteImpl(const std::shared_ptr& goal) override { + auto result = std::make_shared(); + result->robot_trajectory = goal->robot_trajectories[0]; + return result; + } +}; + +class DummyOptimizer : public DummyBase { + public: + using Ptr = std::shared_ptr; + + explicit DummyOptimizer(const rclcpp::Node::SharedPtr& node, const std::string& prefix = "") + : DummyBase(node, prefix + "optimize") {} + virtual ~DummyOptimizer() = default; + + protected: + std::shared_ptr ExecuteImpl(const std::shared_ptr& goal) override { + auto result = std::make_shared(); + result->robot_trajectory = goal->robot_trajectory; + return result; + } +}; + +} // namespace + +namespace tmc_robot_local_planner { + +class RobotLocalPlannerTest : public ::testing::Test { + protected: + void SetUp() override; + + void SpinSome() { + rclcpp::spin_some(client_node_); + rclcpp::spin_some(server_node_); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + + template + void WaitForFutureComplete(const typename std::future& future) { + while (future.wait_for(std::chrono::microseconds(1)) == std::future_status::timeout) { + SpinSome(); + } + } + + rclcpp::Node::SharedPtr client_node_; + rclcpp::Node::SharedPtr server_node_; + + DummyGenerator::Ptr generator_; + DummyEvaluator::Ptr evaluator_; + DummyValidator::Ptr validator_; + DummyOptimizer::Ptr optimizer_; + + RobotLocalPlanner::Ptr planner_; + TrajectoryMerger::Ptr trajectory_merger_; + tmc_planning_msgs::msg::RangeJointConstraint rjc_; + tmc_planning_msgs::msg::Constraints goal_constraints_; + + tmc_utils::CachingSubscriber::Ptr trajectory_candidates_cache_; +}; + +void RobotLocalPlannerTest::SetUp() { + trajectory_merger_ = std::make_shared>(); + + client_node_ = rclcpp::Node::make_shared("client"); + client_node_->declare_parameter("publish_generated_trajectories", true); + planner_ = std::make_shared(client_node_, trajectory_merger_); + + server_node_ = rclcpp::Node::make_shared("server"); + generator_ = std::make_shared(server_node_); + evaluator_ = std::make_shared(server_node_); + validator_ = std::make_shared(server_node_); + optimizer_ = std::make_shared(server_node_); + trajectory_candidates_cache_ = std::make_shared>( + server_node_, "client/generated_trajectories", rclcpp::SensorDataQoS()); + + while (!planner_->AreActionServersReady()) { + SpinSome(); + } + + rjc_.min.joint_state.name.push_back("joint1"); + rjc_.min.joint_state.position.push_back(0.1); + rjc_.min.multi_dof_joint_state.joint_names.push_back("world_joint"); + rjc_.min.multi_dof_joint_state.transforms.push_back(geometry_msgs::msg::Transform()); + rjc_.max = rjc_.min; + goal_constraints_.hard_joint_constraints.push_back(rjc_); +} + +// The four actions are called in order and PlanPath becomes true +TEST_F(RobotLocalPlannerTest, PlanPath) { + auto trajectory_future = planner_->PlanPath(goal_constraints_, moveit_msgs::msg::RobotState(), 0.1); + WaitForFutureComplete(trajectory_future); + EXPECT_EQ(RobotLocalPlannerErrorCode::kSuccess, planner_->last_error_code()); + + const auto trajectory = trajectory_future.get(); + + // The entered data returns over the four actions + EXPECT_EQ(trajectory.joint_trajectory.joint_names[0], rjc_.min.joint_state.name[0]); + EXPECT_EQ(trajectory.joint_trajectory.points[0].positions[0], rjc_.min.joint_state.position[0]); + + // Generator-> EVALUATOR-> Validator-> Optimizer + EXPECT_LT(0.0, generator_->GetCallbackTime().seconds()); + EXPECT_LT(generator_->GetCallbackTime(), evaluator_->GetCallbackTime()); + EXPECT_LT(evaluator_->GetCallbackTime(), validator_->GetCallbackTime()); + EXPECT_LT(validator_->GetCallbackTime(), optimizer_->GetCallbackTime()); + + EXPECT_TRUE(generator_->ignore_joints().empty()); + EXPECT_EQ(evaluator_->GetTrajectorySize(), 1); + + ASSERT_TRUE(trajectory_candidates_cache_->IsSubscribed()); + EXPECT_EQ(trajectory_candidates_cache_->GetValue().trajectory.size(), 1); +} + +// Specify a joint not used in the plan +TEST_F(RobotLocalPlannerTest, PlanPathWithIgnoreJoints) { + client_node_->set_parameter(rclcpp::Parameter("publish_generated_trajectories", false)); + planner_ = std::make_shared(client_node_, trajectory_merger_); + + auto trajectory_future = planner_->PlanPath(goal_constraints_, moveit_msgs::msg::RobotState(), 0.1, {"test_joint"}); + WaitForFutureComplete(trajectory_future); + EXPECT_EQ(RobotLocalPlannerErrorCode::kSuccess, planner_->last_error_code()); + + ASSERT_EQ(generator_->ignore_joints().size(), 1); + EXPECT_EQ(generator_->ignore_joints()[0], "test_joint"); + + EXPECT_FALSE(trajectory_candidates_cache_->IsSubscribed()); +} + +// The trajectory is merged with TrajectoryMerger and it is going to EVALUATOR +TEST_F(RobotLocalPlannerTest, PlanPathWithTrajectoryMerger) { + moveit_msgs::msg::RobotTrajectory robot_trajectory; + robot_trajectory.joint_trajectory.joint_names.push_back("joint0"); + robot_trajectory.joint_trajectory.points.resize(1); + robot_trajectory.joint_trajectory.points[0].positions.push_back(0.0); + trajectory_merger_->SetTrajectory(robot_trajectory); + + auto trajectory_future = planner_->PlanPath(goal_constraints_, moveit_msgs::msg::RobotState(), 0.1); + WaitForFutureComplete(trajectory_future); + EXPECT_EQ(RobotLocalPlannerErrorCode::kSuccess, planner_->last_error_code()); + + // The set trajectory should be merged and the number of trajectory should be 2 + EXPECT_EQ(evaluator_->GetTrajectorySize(), 2); +} + +// PlanPath turns without giving TrajectoryMerger +TEST_F(RobotLocalPlannerTest, PlanPathWithoutTrajectoryMerger) { + planner_ = std::make_shared(client_node_); + while (!planner_->AreActionServersReady()) { + SpinSome(); + } + + auto trajectory_future = planner_->PlanPath(goal_constraints_, moveit_msgs::msg::RobotState(), 0.1); + WaitForFutureComplete(trajectory_future); + EXPECT_EQ(RobotLocalPlannerErrorCode::kSuccess, planner_->last_error_code()); + + const auto trajectory = trajectory_future.get(); + EXPECT_FALSE(trajectory.joint_trajectory.points.empty()); +} + +// Explicitly give the action name +TEST_F(RobotLocalPlannerTest, PlanPathWithExplicitActionNames) { + RobotLocalPlanner::ActionNames action_names; + EXPECT_EQ(action_names.generate_action, "generate"); + EXPECT_EQ(action_names.evaluate_action, "evaluate"); + EXPECT_EQ(action_names.validate_action, "validate"); + EXPECT_EQ(action_names.optimize_action, "optimize"); + + const std::string prefix = "second_"; + action_names.generate_action = prefix + action_names.generate_action; + action_names.evaluate_action = prefix + action_names.evaluate_action; + action_names.validate_action = prefix + action_names.validate_action; + action_names.optimize_action = prefix + action_names.optimize_action; + + auto second_generator = std::make_shared(server_node_, prefix); + auto second_evaluator = std::make_shared(server_node_, prefix); + auto second_validator = std::make_shared(server_node_, prefix); + auto second_optimizer = std::make_shared(server_node_, prefix); + + auto planner = std::make_shared(client_node_, action_names); + + auto trajectory_future = planner->PlanPath(goal_constraints_, moveit_msgs::msg::RobotState(), 0.1); + WaitForFutureComplete(trajectory_future); + EXPECT_EQ(RobotLocalPlannerErrorCode::kSuccess, planner->last_error_code()); + + const auto trajectory = trajectory_future.get(); + EXPECT_FALSE(trajectory.joint_trajectory.points.empty()); + + EXPECT_LT(0.0, second_generator->GetCallbackTime().seconds()); + EXPECT_LT(second_generator->GetCallbackTime(), second_evaluator->GetCallbackTime()); + EXPECT_LT(second_evaluator->GetCallbackTime(), second_validator->GetCallbackTime()); + EXPECT_LT(second_validator->GetCallbackTime(), second_optimizer->GetCallbackTime()); + + EXPECT_DOUBLE_EQ(generator_->GetCallbackTime().seconds(), 0.0); + EXPECT_DOUBLE_EQ(evaluator_->GetCallbackTime().seconds(), 0.0); + EXPECT_DOUBLE_EQ(validator_->GetCallbackTime().seconds(), 0.0); + EXPECT_DOUBLE_EQ(optimizer_->GetCallbackTime().seconds(), 0.0); +} + +// Failed in the empty Constraints +TEST_F(RobotLocalPlannerTest, EmptyConstraints) { + auto trajectory_future = planner_->PlanPath(tmc_planning_msgs::msg::Constraints(), + moveit_msgs::msg::RobotState(), 0.1); + WaitForFutureComplete(trajectory_future); + EXPECT_EQ(RobotLocalPlannerErrorCode::kConstraintsEmpty, planner_->last_error_code()); + + const auto trajectory = trajectory_future.get(); + EXPECT_TRUE(trajectory.joint_trajectory.points.empty()); + + EXPECT_DOUBLE_EQ(generator_->GetCallbackTime().seconds(), 0.0); + EXPECT_DOUBLE_EQ(evaluator_->GetCallbackTime().seconds(), 0.0); + EXPECT_DOUBLE_EQ(validator_->GetCallbackTime().seconds(), 0.0); + EXPECT_DOUBLE_EQ(optimizer_->GetCallbackTime().seconds(), 0.0); +} + +// Cases to be Reject in GENERATOR +TEST_F(RobotLocalPlannerTest, RejectedByGenerator) { + generator_->SetDoReject(true); + + auto trajectory_future = planner_->PlanPath(goal_constraints_, moveit_msgs::msg::RobotState(), 0.1); + WaitForFutureComplete(trajectory_future); + EXPECT_EQ(RobotLocalPlannerErrorCode::kGenerationFailure, planner_->last_error_code()); + + const auto trajectory = trajectory_future.get(); + EXPECT_TRUE(trajectory.joint_trajectory.points.empty()); + + // If it's Reject, you can't even call a callback + EXPECT_DOUBLE_EQ(generator_->GetCallbackTime().seconds(), 0.0); + // EVALUATOR, VALIDATOR, Optimizer is not called because it should have failed in GENERATOR + EXPECT_DOUBLE_EQ(evaluator_->GetCallbackTime().seconds(), 0.0); + EXPECT_DOUBLE_EQ(validator_->GetCallbackTime().seconds(), 0.0); + EXPECT_DOUBLE_EQ(optimizer_->GetCallbackTime().seconds(), 0.0); +} + +// Case to be about to be done in GENERATOR +TEST_F(RobotLocalPlannerTest, AbortedByGenerator) { + generator_->SetDoAbort(true); + + auto trajectory_future = planner_->PlanPath(goal_constraints_, moveit_msgs::msg::RobotState(), 0.1); + WaitForFutureComplete(trajectory_future); + EXPECT_EQ(RobotLocalPlannerErrorCode::kGenerationFailure, planner_->last_error_code()); + + const auto trajectory = trajectory_future.get(); + EXPECT_TRUE(trajectory.joint_trajectory.points.empty()); + + EXPECT_LT(0.0, generator_->GetCallbackTime().seconds()); + + // EVALUATOR, VALIDATOR, Optimizer is not called because it should have failed in GENERATOR + EXPECT_DOUBLE_EQ(evaluator_->GetCallbackTime().seconds(), 0.0); + EXPECT_DOUBLE_EQ(validator_->GetCallbackTime().seconds(), 0.0); + EXPECT_DOUBLE_EQ(optimizer_->GetCallbackTime().seconds(), 0.0); +} + +// Cases to be rejected in EVALUATOR +TEST_F(RobotLocalPlannerTest, RejectedByEvaluator) { + evaluator_->SetDoReject(true); + + auto trajectory_future = planner_->PlanPath(goal_constraints_, moveit_msgs::msg::RobotState(), 0.1); + WaitForFutureComplete(trajectory_future); + EXPECT_EQ(RobotLocalPlannerErrorCode::kEvaluationFailure, planner_->last_error_code()); + + const auto trajectory = trajectory_future.get(); + EXPECT_TRUE(trajectory.joint_trajectory.points.empty()); + + EXPECT_LT(0.0, generator_->GetCallbackTime().seconds()); + + EXPECT_DOUBLE_EQ(evaluator_->GetCallbackTime().seconds(), 0.0); + EXPECT_DOUBLE_EQ(validator_->GetCallbackTime().seconds(), 0.0); + EXPECT_DOUBLE_EQ(optimizer_->GetCallbackTime().seconds(), 0.0); +} + +// Case to be about with EVALUATOR +TEST_F(RobotLocalPlannerTest, AbortedByEvaluator) { + evaluator_->SetDoAbort(true); + + auto trajectory_future = planner_->PlanPath(goal_constraints_, moveit_msgs::msg::RobotState(), 0.1); + WaitForFutureComplete(trajectory_future); + EXPECT_EQ(RobotLocalPlannerErrorCode::kEvaluationFailure, planner_->last_error_code()); + + const auto trajectory = trajectory_future.get(); + EXPECT_TRUE(trajectory.joint_trajectory.points.empty()); + + EXPECT_LT(0.0, generator_->GetCallbackTime().seconds()); + EXPECT_LT(generator_->GetCallbackTime(), evaluator_->GetCallbackTime()); + + EXPECT_DOUBLE_EQ(validator_->GetCallbackTime().seconds(), 0.0); + EXPECT_DOUBLE_EQ(optimizer_->GetCallbackTime().seconds(), 0.0); +} + +// Cases to be Reject in Validator +TEST_F(RobotLocalPlannerTest, RejectedByValidator) { + validator_->SetDoReject(true); + + auto trajectory_future = planner_->PlanPath(goal_constraints_, moveit_msgs::msg::RobotState(), 0.1); + WaitForFutureComplete(trajectory_future); + EXPECT_EQ(RobotLocalPlannerErrorCode::kValidationFailure, planner_->last_error_code()); + + const auto trajectory = trajectory_future.get(); + EXPECT_TRUE(trajectory.joint_trajectory.points.empty()); + + EXPECT_LT(0.0, generator_->GetCallbackTime().seconds()); + EXPECT_LT(generator_->GetCallbackTime(), evaluator_->GetCallbackTime()); + + EXPECT_DOUBLE_EQ(validator_->GetCallbackTime().seconds(), 0.0); + EXPECT_DOUBLE_EQ(optimizer_->GetCallbackTime().seconds(), 0.0); +} + +// Cases to be about in Validator +TEST_F(RobotLocalPlannerTest, AbortedByValidator) { + validator_->SetDoAbort(true); + + auto trajectory_future = planner_->PlanPath(goal_constraints_, moveit_msgs::msg::RobotState(), 0.1); + WaitForFutureComplete(trajectory_future); + EXPECT_EQ(RobotLocalPlannerErrorCode::kValidationFailure, planner_->last_error_code()); + + const auto trajectory = trajectory_future.get(); + EXPECT_TRUE(trajectory.joint_trajectory.points.empty()); + + EXPECT_LT(0.0, generator_->GetCallbackTime().seconds()); + EXPECT_LT(generator_->GetCallbackTime(), evaluator_->GetCallbackTime()); + EXPECT_LT(evaluator_->GetCallbackTime(), validator_->GetCallbackTime()); + + EXPECT_DOUBLE_EQ(optimizer_->GetCallbackTime().seconds(), 0.0); +} + +// Cases to be Reject in Optimizer +TEST_F(RobotLocalPlannerTest, RejectedByOptimizer) { + optimizer_->SetDoReject(true); + + auto trajectory_future = planner_->PlanPath(goal_constraints_, moveit_msgs::msg::RobotState(), 0.1); + WaitForFutureComplete(trajectory_future); + EXPECT_EQ(RobotLocalPlannerErrorCode::kOptimizationFailure, planner_->last_error_code()); + + const auto trajectory = trajectory_future.get(); + EXPECT_TRUE(trajectory.joint_trajectory.points.empty()); + + EXPECT_LT(0.0, generator_->GetCallbackTime().seconds()); + EXPECT_LT(generator_->GetCallbackTime(), evaluator_->GetCallbackTime()); + EXPECT_LT(evaluator_->GetCallbackTime(), validator_->GetCallbackTime()); + + EXPECT_DOUBLE_EQ(optimizer_->GetCallbackTime().seconds(), 0.0); +} + +// Case to be about with Optimizer +TEST_F(RobotLocalPlannerTest, AbortedByOptimizer) { + optimizer_->SetDoAbort(true); + + auto trajectory_future = planner_->PlanPath(goal_constraints_, moveit_msgs::msg::RobotState(), 0.1); + WaitForFutureComplete(trajectory_future); + EXPECT_EQ(RobotLocalPlannerErrorCode::kOptimizationFailure, planner_->last_error_code()); + + const auto trajectory = trajectory_future.get(); + EXPECT_TRUE(trajectory.joint_trajectory.points.empty()); + + EXPECT_LT(0.0, generator_->GetCallbackTime().seconds()); + EXPECT_LT(generator_->GetCallbackTime(), evaluator_->GetCallbackTime()); + EXPECT_LT(evaluator_->GetCallbackTime(), validator_->GetCallbackTime()); + EXPECT_LT(validator_->GetCallbackTime(), optimizer_->GetCallbackTime()); +} + +// Preempted case +TEST_F(RobotLocalPlannerTest, Timeout) { + generator_->SetDoWait(true); + + planner_->set_action_timeout(0.1); + + auto trajectory_future = planner_->PlanPath(goal_constraints_, moveit_msgs::msg::RobotState(), 0.1); + WaitForFutureComplete(trajectory_future); + EXPECT_EQ(RobotLocalPlannerErrorCode::kGenerationFailure, planner_->last_error_code()); + + const auto trajectory = trajectory_future.get(); + EXPECT_TRUE(trajectory.joint_trajectory.points.empty()); + + EXPECT_LT(0.0, generator_->GetCallbackTime().seconds()); + + EXPECT_DOUBLE_EQ(evaluator_->GetCallbackTime().seconds(), 0.0); + EXPECT_DOUBLE_EQ(validator_->GetCallbackTime().seconds(), 0.0); + EXPECT_DOUBLE_EQ(optimizer_->GetCallbackTime().seconds(), 0.0); +} + +// Generate interrupt while executing +TEST_F(RobotLocalPlannerTest, InterruptGenerator) { + generator_->SetDoWait(true); + + auto trajectory_future = planner_->PlanPath(goal_constraints_, moveit_msgs::msg::RobotState(), 0.1); + while (generator_->GetCallbackTime().seconds() <= 0.0) { + SpinSome(); + } + planner_->InterruptPlanPath(); + + WaitForFutureComplete(trajectory_future); + EXPECT_EQ(RobotLocalPlannerErrorCode::kGenerationFailure, planner_->last_error_code()); + + const auto trajectory = trajectory_future.get(); + EXPECT_TRUE(trajectory.joint_trajectory.points.empty()); + + EXPECT_LT(0.0, generator_->GetCallbackTime().seconds()); + + EXPECT_DOUBLE_EQ(evaluator_->GetCallbackTime().seconds(), 0.0); + EXPECT_DOUBLE_EQ(validator_->GetCallbackTime().seconds(), 0.0); + EXPECT_DOUBLE_EQ(optimizer_->GetCallbackTime().seconds(), 0.0); +} + +// EVALUATE interrupt +TEST_F(RobotLocalPlannerTest, InterruptEvaluator) { + evaluator_->SetDoWait(true); + + auto trajectory_future = planner_->PlanPath(goal_constraints_, moveit_msgs::msg::RobotState(), 0.1); + while (evaluator_->GetCallbackTime().seconds() <= 0.0) { + SpinSome(); + } + planner_->InterruptPlanPath(); + + WaitForFutureComplete(trajectory_future); + EXPECT_EQ(RobotLocalPlannerErrorCode::kEvaluationFailure, planner_->last_error_code()); + + const auto trajectory = trajectory_future.get(); + EXPECT_TRUE(trajectory.joint_trajectory.points.empty()); + + EXPECT_LT(0.0, generator_->GetCallbackTime().seconds()); + EXPECT_LT(generator_->GetCallbackTime(), evaluator_->GetCallbackTime()); + + EXPECT_DOUBLE_EQ(validator_->GetCallbackTime().seconds(), 0.0); + EXPECT_DOUBLE_EQ(optimizer_->GetCallbackTime().seconds(), 0.0); +} + +// Validate interrupt +TEST_F(RobotLocalPlannerTest, InterruptValidator) { + validator_->SetDoWait(true); + + auto trajectory_future = planner_->PlanPath(goal_constraints_, moveit_msgs::msg::RobotState(), 0.1); + while (validator_->GetCallbackTime().seconds() <= 0.0) { + SpinSome(); + } + planner_->InterruptPlanPath(); + + WaitForFutureComplete(trajectory_future); + EXPECT_EQ(RobotLocalPlannerErrorCode::kValidationFailure, planner_->last_error_code()); + + const auto trajectory = trajectory_future.get(); + EXPECT_TRUE(trajectory.joint_trajectory.points.empty()); + + EXPECT_LT(0.0, generator_->GetCallbackTime().seconds()); + EXPECT_LT(generator_->GetCallbackTime(), evaluator_->GetCallbackTime()); + EXPECT_LT(evaluator_->GetCallbackTime(), validator_->GetCallbackTime()); + + EXPECT_DOUBLE_EQ(optimizer_->GetCallbackTime().seconds(), 0.0); +} + +// Optimize interrupt during execution +TEST_F(RobotLocalPlannerTest, InterruptOptimizer) { + optimizer_->SetDoWait(true); + + auto trajectory_future = planner_->PlanPath(goal_constraints_, moveit_msgs::msg::RobotState(), 0.1); + while (optimizer_->GetCallbackTime().seconds() <= 0.0) { + SpinSome(); + } + planner_->InterruptPlanPath(); + + WaitForFutureComplete(trajectory_future); + EXPECT_EQ(RobotLocalPlannerErrorCode::kOptimizationFailure, planner_->last_error_code()); + + const auto trajectory = trajectory_future.get(); + EXPECT_TRUE(trajectory.joint_trajectory.points.empty()); + + EXPECT_LT(0.0, generator_->GetCallbackTime().seconds()); + EXPECT_LT(generator_->GetCallbackTime(), evaluator_->GetCallbackTime()); + EXPECT_LT(evaluator_->GetCallbackTime(), validator_->GetCallbackTime()); + EXPECT_LT(validator_->GetCallbackTime(), optimizer_->GetCallbackTime()); +} + +} // namespace tmc_robot_local_planner + +int main(int argc, char* argv[]) { + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/tmc_robot_local_planner/test/robot_local_planner_composition-test.cpp b/tmc_robot_local_planner/test/robot_local_planner_composition-test.cpp new file mode 100644 index 0000000..28aff58 --- /dev/null +++ b/tmc_robot_local_planner/test/robot_local_planner_composition-test.cpp @@ -0,0 +1,545 @@ +/* +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. +*/ +#include +#include + +#include + +#include + +#include +#include + +namespace { + +class MockGenerator : public tmc_robot_local_planner::IGenerator { + public: + MOCK_METHOD1(Initialize, void(const rclcpp::Node::SharedPtr&)); + MOCK_METHOD6(Generate, bool(const tmc_robot_local_planner::Constraints&, + const tmc_manipulation_types::RobotState&, + double, + const std::vector&, + std::function, + std::vector&)); +}; + +class MockEvaluator : public tmc_robot_local_planner::IEvaluator { + public: + MOCK_METHOD1(Initialize, void(const rclcpp::Node::SharedPtr&)); + MOCK_METHOD4(Evaluate, bool(const tmc_robot_local_planner::Constraints&, + const std::vector&, + std::function, + std::vector&)); +}; + +class MockValidator : public tmc_robot_local_planner::IValidator { + public: + MOCK_METHOD1(Initialize, void(const rclcpp::Node::SharedPtr&)); + MOCK_METHOD3(Validate, bool(const std::vector&, + std::function, + tmc_manipulation_types::TimedRobotTrajectory&)); +}; + +class MockOptimizer : public tmc_robot_local_planner::IOptimizer { + public: + MOCK_METHOD1(Initialize, void(const rclcpp::Node::SharedPtr&)); + MOCK_METHOD3(Optimize, bool(const tmc_manipulation_types::TimedRobotTrajectory&, + std::function, + tmc_manipulation_types::TimedRobotTrajectory&)); + MOCK_METHOD3(Optimize, bool(const std::vector&, + std::function, + std::vector&)); +}; + +bool SleepImpl(std::function interrupt) { + const auto end_time = std::chrono::system_clock::now() + std::chrono::milliseconds(500); + while (std::chrono::system_clock::now() < end_time) { + if (interrupt()) { + return false; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + return true; +} + +bool Sleep6(::testing::Unused, ::testing::Unused, ::testing::Unused, + ::testing::Unused, std::function interrupt, ::testing::Unused) { + return SleepImpl(interrupt); +} + +bool Sleep4(::testing::Unused, ::testing::Unused, std::function interrupt, ::testing::Unused) { + return SleepImpl(interrupt); +} + +bool Sleep3(::testing::Unused, std::function interrupt, ::testing::Unused) { + return SleepImpl(interrupt); +} + +} // namespace + +namespace tmc_robot_local_planner { + +class RobotLocalPlannerCompositionTest : public ::testing::Test { + protected: + void SetUp() override; + + std::shared_ptr gen_; + std::shared_ptr eval_; + std::shared_ptr val_; + std::shared_ptr opt_; + + RobotLocalPlannerPlugins plugins_; + + TrajectoryMerger::Ptr trajectory_merger_; + RobotLocalPlannerComposition::Ptr planner_; + + Constraints goal_constraints_; + tmc_manipulation_types::RobotState initial_state_; + + rclcpp::Node::SharedPtr server_node_; + tmc_utils::CachingSubscriber::Ptr trajectory_candidates_cache_; +}; + +void RobotLocalPlannerCompositionTest::SetUp() { + gen_ = std::make_shared(); + eval_ = std::make_shared(); + val_ = std::make_shared(); + opt_ = std::make_shared(); + + plugins_.generator = gen_; + plugins_.evaluator = eval_; + plugins_.validator = val_; + plugins_.optimizer = opt_; + + trajectory_merger_ = std::make_shared>(); + + const auto test_node = rclcpp::Node::make_shared("test_node"); + test_node->declare_parameter("publish_generated_trajectories", true); + planner_ = std::make_shared(test_node, trajectory_merger_, plugins_); + + goal_constraints_.hard_link_constraints.resize(1); + initial_state_.joint_state.name = {"test_joint"}; + + server_node_ = rclcpp::Node::make_shared("server"); + trajectory_candidates_cache_ = std::make_shared>( + server_node_, "test_node/generated_trajectories", rclcpp::SensorDataQoS()); +} + +// The four actions are called in order and PlanPath becomes true +TEST_F(RobotLocalPlannerCompositionTest, PlanPath) { + ::testing::InSequence mock_sequence; + + Constraints gen_input_constraints; + tmc_manipulation_types::RobotState gen_input_initial_state; + std::vector gen_output_trajectories(2); + EXPECT_CALL(*gen_, Generate(::testing::_, ::testing::_, 0.1, std::vector({"ignored"}), + ::testing::_, ::testing::_)) + .Times(1) + .WillOnce(::testing::DoAll(::testing::SaveArg<0>(&gen_input_constraints), + ::testing::SaveArg<1>(&gen_input_initial_state), + ::testing::SetArgReferee<5>(gen_output_trajectories), + ::testing::Return(true))); + + Constraints eval_input_constraints; + std::vector eval_input_trajectories; + std::vector eval_output_trajectories(3); + EXPECT_CALL(*eval_, Evaluate(::testing::_, ::testing::_, ::testing::_, ::testing::_)) + .Times(1) + .WillOnce(::testing::DoAll(::testing::SaveArg<0>(&eval_input_constraints), + ::testing::SaveArg<1>(&eval_input_trajectories), + ::testing::SetArgReferee<3>(eval_output_trajectories), + ::testing::Return(true))); + + std::vector val_input_trajectories; + tmc_manipulation_types::TimedRobotTrajectory val_output_trajectory; + val_output_trajectory.joint_trajectory.joint_names.resize(4); + EXPECT_CALL(*val_, Validate(::testing::_, ::testing::_, ::testing::_)) + .Times(1) + .WillOnce(::testing::DoAll(::testing::SaveArg<0>(&val_input_trajectories), + ::testing::SetArgReferee<2>(val_output_trajectory), + ::testing::Return(true))); + + tmc_manipulation_types::TimedRobotTrajectory opt_input_trajectory; + tmc_manipulation_types::TimedRobotTrajectory opt_output_trajectory; + opt_output_trajectory.joint_trajectory.joint_names.resize(5); + EXPECT_CALL(*opt_, Optimize(::testing::An(), + ::testing::_, ::testing::_)) + .Times(1) + .WillOnce(::testing::DoAll(::testing::SaveArg<0>(&opt_input_trajectory), + ::testing::SetArgReferee<2>(opt_output_trajectory), + ::testing::Return(true))); + + auto trajectory_future = planner_->PlanPath(goal_constraints_, initial_state_, 0.1, {"ignored"}); + EXPECT_EQ(trajectory_future.wait_for(std::chrono::seconds(1)), std::future_status::ready); + EXPECT_EQ(RobotLocalPlannerErrorCode::kSuccess, planner_->last_error_code()); + + EXPECT_EQ(gen_input_constraints.hard_link_constraints.size(), 1); + EXPECT_EQ(gen_input_initial_state.joint_state.name, std::vector({"test_joint"})); + + EXPECT_EQ(eval_input_constraints.hard_link_constraints.size(), 1); + EXPECT_EQ(eval_input_trajectories.size(), 2); + + EXPECT_EQ(val_input_trajectories.size(), 3); + + EXPECT_EQ(opt_input_trajectory.joint_trajectory.joint_names.size(), 4); + + EXPECT_EQ(trajectory_future.get().joint_trajectory.joint_names.size(), 5); + + rclcpp::spin_some(server_node_); + ASSERT_TRUE(trajectory_candidates_cache_->IsSubscribed()); + EXPECT_EQ(trajectory_candidates_cache_->GetValue().trajectory.size(), 3); +} + +// Four actions are called in order and PlanPath becomes true, Gen-> Opt-> EVAL-> Val cases +TEST_F(RobotLocalPlannerCompositionTest, OptimizeSecond) { + tmc_manipulation_types::TimedRobotTrajectory trajectory; + trajectory.joint_trajectory.joint_names.resize(1); + trajectory.joint_trajectory.points.resize(1); + ASSERT_TRUE(trajectory_merger_->SetTrajectory(trajectory)); + + const auto test_node = rclcpp::Node::make_shared("test_node"); + test_node->declare_parameter("optimize_second", true); + planner_ = std::make_shared(test_node, trajectory_merger_, plugins_); + + ::testing::InSequence mock_sequence; + + Constraints gen_input_constraints; + tmc_manipulation_types::RobotState gen_input_initial_state; + std::vector gen_output_trajectories(2); + EXPECT_CALL(*gen_, Generate(::testing::_, ::testing::_, 0.1, std::vector({"ignored"}), + ::testing::_, ::testing::_)) + .Times(1) + .WillOnce(::testing::DoAll(::testing::SaveArg<0>(&gen_input_constraints), + ::testing::SaveArg<1>(&gen_input_initial_state), + ::testing::SetArgReferee<5>(gen_output_trajectories), + ::testing::Return(true))); + + std::vector opt_input_trajectories; + std::vector opt_output_trajectories(3); + EXPECT_CALL(*opt_, Optimize(::testing::An&>(), + ::testing::_, ::testing::_)) + .Times(1) + .WillOnce(::testing::DoAll(::testing::SaveArg<0>(&opt_input_trajectories), + ::testing::SetArgReferee<2>(opt_output_trajectories), + ::testing::Return(true))); + + Constraints eval_input_constraints; + std::vector eval_input_trajectories; + std::vector eval_output_trajectories(4); + EXPECT_CALL(*eval_, Evaluate(::testing::_, ::testing::_, ::testing::_, ::testing::_)) + .Times(1) + .WillOnce(::testing::DoAll(::testing::SaveArg<0>(&eval_input_constraints), + ::testing::SaveArg<1>(&eval_input_trajectories), + ::testing::SetArgReferee<3>(eval_output_trajectories), + ::testing::Return(true))); + + std::vector val_input_trajectories; + tmc_manipulation_types::TimedRobotTrajectory val_output_trajectory; + val_output_trajectory.joint_trajectory.joint_names.resize(5); + EXPECT_CALL(*val_, Validate(::testing::_, ::testing::_, ::testing::_)) + .Times(1) + .WillOnce(::testing::DoAll(::testing::SaveArg<0>(&val_input_trajectories), + ::testing::SetArgReferee<2>(val_output_trajectory), + ::testing::Return(true))); + + auto trajectory_future = planner_->PlanPath(goal_constraints_, initial_state_, 0.1, {"ignored"}); + EXPECT_EQ(trajectory_future.wait_for(std::chrono::seconds(1)), std::future_status::ready); + EXPECT_EQ(RobotLocalPlannerErrorCode::kSuccess, planner_->last_error_code()); + + EXPECT_EQ(gen_input_constraints.hard_link_constraints.size(), 1); + EXPECT_EQ(gen_input_initial_state.joint_state.name, std::vector({"test_joint"})); + + EXPECT_EQ(opt_input_trajectories.size(), 2); + + EXPECT_EQ(eval_input_constraints.hard_link_constraints.size(), 1); + EXPECT_EQ(eval_input_trajectories.size(), 4); + + EXPECT_EQ(val_input_trajectories.size(), 4); + + EXPECT_EQ(trajectory_future.get().joint_trajectory.joint_names.size(), 5); +} + +// The trajectory is merged with TrajectoryMerger and it is going to EVALUATOR +TEST_F(RobotLocalPlannerCompositionTest, PlanPathWithTrajectoryMerger) { + tmc_manipulation_types::TimedRobotTrajectory trajectory; + trajectory.joint_trajectory.joint_names.resize(1); + trajectory.joint_trajectory.points.resize(1); + ASSERT_TRUE(trajectory_merger_->SetTrajectory(trajectory)); + + std::vector gen_output_trajectories(2); + EXPECT_CALL(*gen_, Generate(::testing::_, ::testing::_, ::testing::_, ::testing::_, ::testing::_, ::testing::_)) + .Times(1) + .WillOnce(::testing::DoAll(::testing::SetArgReferee<5>(gen_output_trajectories), + ::testing::Return(true))); + + std::vector eval_input_trajectories; + EXPECT_CALL(*eval_, Evaluate(::testing::_, ::testing::_, ::testing::_, ::testing::_)) + .Times(1) + .WillOnce(::testing::DoAll(::testing::SaveArg<1>(&eval_input_trajectories), + ::testing::Return(true))); + + EXPECT_CALL(*val_, Validate(::testing::_, ::testing::_, ::testing::_)) + .Times(1).WillOnce(::testing::Return(true)); + EXPECT_CALL(*opt_, Optimize(::testing::An(), + ::testing::_, ::testing::_)) + .Times(1).WillOnce(::testing::Return(true)); + + auto trajectory_future = planner_->PlanPath(goal_constraints_, initial_state_, 0.1, {"ignored"}); + EXPECT_EQ(trajectory_future.wait_for(std::chrono::seconds(1)), std::future_status::ready); + + // The set trajectory should be merged and the number of tracks should be 3 + EXPECT_EQ(eval_input_trajectories.size(), 3); +} + +// PlanPath turns without giving TrajectoryMerger +TEST_F(RobotLocalPlannerCompositionTest, PlanPathWithoutTrajectoryMerger) { + const auto test_node = rclcpp::Node::make_shared("test_node"); + planner_ = std::make_shared(test_node, plugins_); + + EXPECT_CALL(*gen_, Generate(::testing::_, ::testing::_, ::testing::_, ::testing::_, ::testing::_, ::testing::_)) + .Times(1).WillOnce(::testing::Return(true)); + EXPECT_CALL(*eval_, Evaluate(::testing::_, ::testing::_, ::testing::_, ::testing::_)) + .Times(1).WillOnce(::testing::Return(true)); + EXPECT_CALL(*val_, Validate(::testing::_, ::testing::_, ::testing::_)) + .Times(1).WillOnce(::testing::Return(true)); + EXPECT_CALL(*opt_, Optimize(::testing::An(), + ::testing::_, ::testing::_)) + .Times(1).WillOnce(::testing::Return(true)); + + auto trajectory_future = planner_->PlanPath(goal_constraints_, initial_state_, 0.1, {"ignored"}); + EXPECT_EQ(trajectory_future.wait_for(std::chrono::seconds(1)), std::future_status::ready); +} + +// Failed in the empty Constraints +TEST_F(RobotLocalPlannerCompositionTest, EmptyConstraints) { + EXPECT_CALL(*gen_, Generate(::testing::_, ::testing::_, ::testing::_, ::testing::_, ::testing::_, ::testing::_)) + .Times(0); + EXPECT_CALL(*eval_, Evaluate(::testing::_, ::testing::_, ::testing::_, ::testing::_)).Times(0); + EXPECT_CALL(*val_, Validate(::testing::_, ::testing::_, ::testing::_)).Times(0); + EXPECT_CALL(*opt_, Optimize(::testing::An(), + ::testing::_, ::testing::_)).Times(0); + + auto trajectory_future = planner_->PlanPath(Constraints(), initial_state_, 0.1, {"ignored"}); + EXPECT_EQ(trajectory_future.wait_for(std::chrono::seconds(1)), std::future_status::ready); + EXPECT_EQ(RobotLocalPlannerErrorCode::kConstraintsEmpty, planner_->last_error_code()); + EXPECT_TRUE(trajectory_future.get().joint_trajectory.joint_names.empty()); +} + +// Fail in Generator +TEST_F(RobotLocalPlannerCompositionTest, AbortedByGenerator) { + EXPECT_CALL(*gen_, Generate(::testing::_, ::testing::_, ::testing::_, ::testing::_, ::testing::_, ::testing::_)) + .Times(1).WillOnce(::testing::Return(false)); + EXPECT_CALL(*eval_, Evaluate(::testing::_, ::testing::_, ::testing::_, ::testing::_)).Times(0); + EXPECT_CALL(*val_, Validate(::testing::_, ::testing::_, ::testing::_)).Times(0); + + tmc_manipulation_types::TimedRobotTrajectory opt_output_trajectory; + opt_output_trajectory.joint_trajectory.joint_names.resize(5); + EXPECT_CALL(*opt_, Optimize(::testing::An(), + ::testing::_, ::testing::_)) + .WillRepeatedly(::testing::DoAll(::testing::SetArgReferee<2>(opt_output_trajectory), + ::testing::Return(false))); + + auto trajectory_future = planner_->PlanPath(goal_constraints_, initial_state_, 0.1, {"ignored"}); + EXPECT_EQ(trajectory_future.wait_for(std::chrono::seconds(1)), std::future_status::ready); + EXPECT_EQ(RobotLocalPlannerErrorCode::kGenerationFailure, planner_->last_error_code()); + EXPECT_TRUE(trajectory_future.get().joint_trajectory.joint_names.empty()); +} + +// Failed in EVALUATOR +TEST_F(RobotLocalPlannerCompositionTest, AbortedByEvaluator) { + EXPECT_CALL(*gen_, Generate(::testing::_, ::testing::_, ::testing::_, ::testing::_, ::testing::_, ::testing::_)) + .Times(1).WillOnce(::testing::Return(true)); + EXPECT_CALL(*eval_, Evaluate(::testing::_, ::testing::_, ::testing::_, ::testing::_)) + .Times(1).WillOnce(::testing::Return(false)); + EXPECT_CALL(*val_, Validate(::testing::_, ::testing::_, ::testing::_)).Times(0); + + tmc_manipulation_types::TimedRobotTrajectory opt_output_trajectory; + opt_output_trajectory.joint_trajectory.joint_names.resize(5); + EXPECT_CALL(*opt_, Optimize(::testing::An(), + ::testing::_, ::testing::_)) + .WillRepeatedly(::testing::DoAll(::testing::SetArgReferee<2>(opt_output_trajectory), + ::testing::Return(false))); + + auto trajectory_future = planner_->PlanPath(goal_constraints_, initial_state_, 0.1, {"ignored"}); + EXPECT_EQ(trajectory_future.wait_for(std::chrono::seconds(1)), std::future_status::ready); + EXPECT_EQ(RobotLocalPlannerErrorCode::kEvaluationFailure, planner_->last_error_code()); + EXPECT_TRUE(trajectory_future.get().joint_trajectory.joint_names.empty()); +} + +// Fail in Validator +TEST_F(RobotLocalPlannerCompositionTest, AbortedByValidator) { + EXPECT_CALL(*gen_, Generate(::testing::_, ::testing::_, ::testing::_, ::testing::_, ::testing::_, ::testing::_)) + .Times(1).WillOnce(::testing::Return(true)); + EXPECT_CALL(*eval_, Evaluate(::testing::_, ::testing::_, ::testing::_, ::testing::_)) + .Times(1).WillOnce(::testing::Return(true)); + EXPECT_CALL(*val_, Validate(::testing::_, ::testing::_, ::testing::_)) + .Times(1).WillOnce(::testing::Return(false)); + + tmc_manipulation_types::TimedRobotTrajectory opt_output_trajectory; + opt_output_trajectory.joint_trajectory.joint_names.resize(5); + EXPECT_CALL(*opt_, Optimize(::testing::An(), + ::testing::_, ::testing::_)) + .WillRepeatedly(::testing::DoAll(::testing::SetArgReferee<2>(opt_output_trajectory), + ::testing::Return(true))); + + auto trajectory_future = planner_->PlanPath(goal_constraints_, initial_state_, 0.1, {"ignored"}); + EXPECT_EQ(trajectory_future.wait_for(std::chrono::seconds(1)), std::future_status::ready); + EXPECT_EQ(RobotLocalPlannerErrorCode::kValidationFailure, planner_->last_error_code()); + EXPECT_TRUE(trajectory_future.get().joint_trajectory.joint_names.empty()); +} + +// Failed with Optimizer +TEST_F(RobotLocalPlannerCompositionTest, AbortedByOptimizer) { + EXPECT_CALL(*gen_, Generate(::testing::_, ::testing::_, ::testing::_, ::testing::_, ::testing::_, ::testing::_)) + .Times(1).WillOnce(::testing::Return(true)); + EXPECT_CALL(*eval_, Evaluate(::testing::_, ::testing::_, ::testing::_, ::testing::_)) + .Times(1).WillOnce(::testing::Return(true)); + EXPECT_CALL(*val_, Validate(::testing::_, ::testing::_, ::testing::_)) + .Times(1).WillOnce(::testing::Return(true)); + + tmc_manipulation_types::TimedRobotTrajectory opt_output_trajectory; + opt_output_trajectory.joint_trajectory.joint_names.resize(5); + EXPECT_CALL(*opt_, Optimize(::testing::An(), + ::testing::_, ::testing::_)) + .WillRepeatedly(::testing::DoAll(::testing::SetArgReferee<2>(opt_output_trajectory), + ::testing::Return(false))); + + auto trajectory_future = planner_->PlanPath(goal_constraints_, initial_state_, 0.1, {"ignored"}); + EXPECT_EQ(trajectory_future.wait_for(std::chrono::seconds(1)), std::future_status::ready); + EXPECT_EQ(RobotLocalPlannerErrorCode::kOptimizationFailure, planner_->last_error_code()); + EXPECT_TRUE(trajectory_future.get().joint_trajectory.joint_names.empty()); +} + +// Generate interrupt while executing +TEST_F(RobotLocalPlannerCompositionTest, InterruptGenerator) { + EXPECT_CALL(*gen_, Generate(::testing::_, ::testing::_, ::testing::_, ::testing::_, ::testing::_, ::testing::_)) + .Times(1).WillOnce(::testing::Invoke(Sleep6)); + EXPECT_CALL(*eval_, Evaluate(::testing::_, ::testing::_, ::testing::_, ::testing::_)).Times(0); + EXPECT_CALL(*val_, Validate(::testing::_, ::testing::_, ::testing::_)).Times(0); + + tmc_manipulation_types::TimedRobotTrajectory opt_output_trajectory; + opt_output_trajectory.joint_trajectory.joint_names.resize(5); + EXPECT_CALL(*opt_, Optimize(::testing::An(), + ::testing::_, ::testing::_)) + .WillRepeatedly(::testing::DoAll(::testing::SetArgReferee<2>(opt_output_trajectory), + ::testing::Return(false))); + + auto trajectory_future = planner_->PlanPath(goal_constraints_, initial_state_, 0.1, {"ignored"}); + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + planner_->InterruptPlanPath(); + + EXPECT_EQ(trajectory_future.wait_for(std::chrono::seconds(1)), std::future_status::ready); + EXPECT_EQ(RobotLocalPlannerErrorCode::kGenerationFailure, planner_->last_error_code()); + EXPECT_TRUE(trajectory_future.get().joint_trajectory.joint_names.empty()); +} + +// EVALUATE interrupt +TEST_F(RobotLocalPlannerCompositionTest, InterruptEvaluator) { + EXPECT_CALL(*gen_, Generate(::testing::_, ::testing::_, ::testing::_, ::testing::_, ::testing::_, ::testing::_)) + .Times(1).WillOnce(::testing::Return(true)); + EXPECT_CALL(*eval_, Evaluate(::testing::_, ::testing::_, ::testing::_, ::testing::_)) + .Times(1).WillOnce(::testing::Invoke(Sleep4)); + EXPECT_CALL(*val_, Validate(::testing::_, ::testing::_, ::testing::_)).Times(0); + + tmc_manipulation_types::TimedRobotTrajectory opt_output_trajectory; + opt_output_trajectory.joint_trajectory.joint_names.resize(5); + EXPECT_CALL(*opt_, Optimize(::testing::An(), + ::testing::_, ::testing::_)) + .WillRepeatedly(::testing::DoAll(::testing::SetArgReferee<2>(opt_output_trajectory), + ::testing::Return(false))); + + auto trajectory_future = planner_->PlanPath(goal_constraints_, initial_state_, 0.1, {"ignored"}); + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + planner_->InterruptPlanPath(); + + EXPECT_EQ(trajectory_future.wait_for(std::chrono::seconds(1)), std::future_status::ready); + EXPECT_EQ(RobotLocalPlannerErrorCode::kEvaluationFailure, planner_->last_error_code()); + EXPECT_TRUE(trajectory_future.get().joint_trajectory.joint_names.empty()); +} + +// Validate interrupt +TEST_F(RobotLocalPlannerCompositionTest, InterruptValidator) { + EXPECT_CALL(*gen_, Generate(::testing::_, ::testing::_, ::testing::_, ::testing::_, ::testing::_, ::testing::_)) + .Times(1).WillOnce(::testing::Return(true)); + EXPECT_CALL(*eval_, Evaluate(::testing::_, ::testing::_, ::testing::_, ::testing::_)) + .Times(1).WillOnce(::testing::Return(true)); + EXPECT_CALL(*val_, Validate(::testing::_, ::testing::_, ::testing::_)) + .Times(1).WillOnce(::testing::Invoke(Sleep3)); + + tmc_manipulation_types::TimedRobotTrajectory opt_output_trajectory; + opt_output_trajectory.joint_trajectory.joint_names.resize(5); + EXPECT_CALL(*opt_, Optimize(::testing::An(), + ::testing::_, ::testing::_)) + .WillRepeatedly(::testing::DoAll(::testing::SetArgReferee<2>(opt_output_trajectory), + ::testing::Return(true))); + + auto trajectory_future = planner_->PlanPath(goal_constraints_, initial_state_, 0.1, {"ignored"}); + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + planner_->InterruptPlanPath(); + + EXPECT_EQ(trajectory_future.wait_for(std::chrono::seconds(1)), std::future_status::ready); + EXPECT_EQ(RobotLocalPlannerErrorCode::kValidationFailure, planner_->last_error_code()); + EXPECT_TRUE(trajectory_future.get().joint_trajectory.joint_names.empty()); +} + +// Optimize interrupt during execution +TEST_F(RobotLocalPlannerCompositionTest, InterruptOptimizer) { + EXPECT_CALL(*gen_, Generate(::testing::_, ::testing::_, ::testing::_, ::testing::_, ::testing::_, ::testing::_)) + .Times(1).WillOnce(::testing::Return(true)); + EXPECT_CALL(*eval_, Evaluate(::testing::_, ::testing::_, ::testing::_, ::testing::_)) + .Times(1).WillOnce(::testing::Return(true)); + EXPECT_CALL(*val_, Validate(::testing::_, ::testing::_, ::testing::_)) + .Times(1).WillOnce(::testing::Return(true)); + + tmc_manipulation_types::TimedRobotTrajectory opt_output_trajectory; + opt_output_trajectory.joint_trajectory.joint_names.resize(5); + EXPECT_CALL(*opt_, Optimize(::testing::An(), + ::testing::_, ::testing::_)) + .WillRepeatedly(::testing::DoAll(::testing::SetArgReferee<2>(opt_output_trajectory), + ::testing::Invoke(Sleep3))); + + auto trajectory_future = planner_->PlanPath(goal_constraints_, initial_state_, 0.1, {"ignored"}); + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + planner_->InterruptPlanPath(); + + EXPECT_EQ(trajectory_future.wait_for(std::chrono::seconds(1)), std::future_status::ready); + EXPECT_EQ(RobotLocalPlannerErrorCode::kOptimizationFailure, planner_->last_error_code()); + EXPECT_TRUE(trajectory_future.get().joint_trajectory.joint_names.empty()); +} + +} // namespace tmc_robot_local_planner + +int main(int argc, char* argv[]) { + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/tmc_robot_local_planner/test/trajectory_merger-test.cpp b/tmc_robot_local_planner/test/trajectory_merger-test.cpp new file mode 100644 index 0000000..2f98c7b --- /dev/null +++ b/tmc_robot_local_planner/test/trajectory_merger-test.cpp @@ -0,0 +1,151 @@ +/* +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. +*/ + +#include +#include +#include +#include +#include +#include +#include + +using tmc_manipulation_types::TimedRobotTrajectory; + +namespace tmc_robot_local_planner { + +class TrajectoryMergerTest : public ::testing::Test { + protected: + TrajectoryMergerTest() { + trajectory_merger_.reset(new TrajectoryMerger()); + } + virtual void SetUp(); + virtual void TearDown(); + + TrajectoryMerger::Ptr trajectory_merger_; + TimedRobotTrajectory trajectory_; + std::vector trajectories_; +}; + +void TrajectoryMergerTest::SetUp() { + trajectory_.joint_trajectory.joint_names.push_back("joint1"); + trajectory_.joint_trajectory.points.resize(1); + trajectory_.joint_trajectory.points[0].positions.resize(1); + trajectory_.joint_trajectory.points[0].positions << 0.1; + trajectory_.multi_dof_joint_trajectory.joint_names.push_back("world_joint"); + trajectory_.multi_dof_joint_trajectory.points.resize(1); + trajectory_.multi_dof_joint_trajectory.points[0].transforms.push_back( + Eigen::Translation3d(1.0, 2.0, 0.0) * Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ())); + trajectories_.push_back(trajectory_); +} + +void TrajectoryMergerTest::TearDown() { + trajectory_merger_->ClearTrajectory(); +} + +// Confirm that the set trajectory is added +TEST_F(TrajectoryMergerTest, MergeTrajectory) { + // setup + std::string joint_name = "joint0"; + TimedRobotTrajectory trajectory; + trajectory.joint_trajectory.joint_names = {joint_name}; + trajectory.joint_trajectory.points.resize(1); + trajectory.joint_trajectory.points[0].positions.resize(1); + trajectory.joint_trajectory.points[0].positions << 0.0; + trajectory.multi_dof_joint_trajectory.joint_names = {"world_joint"}; + trajectory.multi_dof_joint_trajectory.points.resize(1); + trajectory.multi_dof_joint_trajectory.points[0].transforms.push_back( + Eigen::Translation3d(0.0, 0.0, 0.0) * Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ())); + + // Set the orbit + ASSERT_TRUE(trajectory_merger_->SetTrajectory(trajectory)); + + // Add orbit + trajectory_merger_->MergeTrajectory(trajectories_); + + EXPECT_EQ(trajectories_.size(), 2); + + // There should be a trajectory including the joint name of JOINT0 + auto pred = [joint_name](const TimedRobotTrajectory& trajectory) { + return trajectory.joint_trajectory.joint_names[0] == joint_name; + }; + auto it = std::find_if(std::begin(trajectories_), std::end(trajectories_), pred); + ASSERT_TRUE(it != trajectories_.end()); +} + +// Confirm that the output is the same as input when the orbit is not set +TEST_F(TrajectoryMergerTest, UnsetMergeTrajectory) { + // Add orbit + trajectory_merger_->MergeTrajectory(trajectories_); + + EXPECT_EQ(trajectories_.size(), 1); + + // Confirm that the same joint name is included + ASSERT_EQ(trajectories_[0].joint_trajectory.joint_names[0], "joint1"); +} + +// Confirm that it becomes fraudulent when entering the sky trajectory +TEST_F(TrajectoryMergerTest, EmptyTrajectory) { + TimedRobotTrajectory trajectory; + + // Set the orbit + ASSERT_FALSE(trajectory_merger_->SetTrajectory(trajectory)); +} + +// Confirm that the orbit has been cleared +TEST_F(TrajectoryMergerTest, ClearTrajectory) { + // setup + std::string joint_name = "joint0"; + TimedRobotTrajectory trajectory; + trajectory.joint_trajectory.joint_names = {joint_name}; + trajectory.joint_trajectory.points.resize(1); + trajectory.joint_trajectory.points[0].positions.resize(1); + trajectory.joint_trajectory.points[0].positions << 0.0; + trajectory.multi_dof_joint_trajectory.joint_names = {"world_joint"}; + trajectory.multi_dof_joint_trajectory.points.resize(1); + trajectory.multi_dof_joint_trajectory.points[0].transforms.push_back( + Eigen::Translation3d(0.0, 0.0, 0.0) * Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ())); + + // Set the orbit + ASSERT_TRUE(trajectory_merger_->SetTrajectory(trajectory)); + + // Clear the orbit + trajectory_merger_->ClearTrajectory(); + + // Add orbit + trajectory_merger_->MergeTrajectory(trajectories_); + + // If the set trajectory is cleared, it should not be added in MERGETRAJECTORY. + EXPECT_EQ(trajectories_.size(), 1); + ASSERT_EQ(trajectories_[0].joint_trajectory.joint_names[0], "joint1"); +} +} // namespace tmc_robot_local_planner + +int main(int argc, char** argv) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/tmc_robot_local_planner/test/tsr_link_constraint-test.cpp b/tmc_robot_local_planner/test/tsr_link_constraint-test.cpp new file mode 100644 index 0000000..58587ae --- /dev/null +++ b/tmc_robot_local_planner/test/tsr_link_constraint-test.cpp @@ -0,0 +1,360 @@ +/* +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. +*/ +#include +#include +#include +#include + +using Eigen::Affine3d; +using Eigen::Vector3d; +using tmc_manipulation_types::RegionValues; +using tmc_manipulation_types::TaskSpaceRegion; + +namespace { +constexpr uint32_t kSampleTestNumberOfTime = 100; +constexpr double kEpsilon = 0.00001; +} // anonymous namespace + +namespace tmc_robot_local_planner { + +// class for tsr link constraint test +// set temp values for tsr link constraint +class TsrLinkConstraintTest : public ::testing::Test { + protected: + virtual void SetUp() { + RegionValues min; + RegionValues max; + min << -1.0, 0.0, 0.0, 0.0, 0.0, 0.0; + max << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0; + TaskSpaceRegion tsr(Affine3d::Identity(), + Affine3d::Identity(), + min, + max, + "origin", + "hand"); + tsr_const_ = std::make_shared( + tsr, 10, static_cast(time(NULL))); + } + ILinkConstraint::Ptr tsr_const_; +}; + +// check end_frame_id in tsr link constraint +TEST_F(TsrLinkConstraintTest, GetLinkNameTest) { + EXPECT_EQ("hand", tsr_const_->GetLinkName()); +} + +// check origin_frame_id in tsr link constraint +TEST_F(TsrLinkConstraintTest, GetOriginNameTest) { + EXPECT_EQ("origin", tsr_const_->GetOriginName()); +} + +// check priority in tsr link constraint +TEST_F(TsrLinkConstraintTest, GetPriorityTest) { + EXPECT_EQ(10, tsr_const_->GetPriority()); +} + +// check tsr samples pointed if they are within region pointed +TEST_F(TsrLinkConstraintTest, SampleTest) { + for (int i = 0; i < kSampleTestNumberOfTime; i++) { + Affine3d result_tsr = tsr_const_->Sample(); + EXPECT_LE(result_tsr.translation()[0], 1.0); + EXPECT_GE(result_tsr.translation()[0], -1.0); + } +} + +// check tsr constraints made if they are in constraint +TEST_F(TsrLinkConstraintTest, IsInConstraint) { + Affine3d right_sample = Affine3d::Identity(); + Vector3d rand = Vector3d::Random(); + Affine3d wrong_sample = Eigen::Translation3d::Identity() + * Eigen::AngleAxisd(rand[0], Vector3d::UnitX()) + * Eigen::AngleAxisd(rand[1], Vector3d::UnitY()) + * Eigen::AngleAxisd(rand[2], Vector3d::UnitZ()); + EXPECT_TRUE(tsr_const_->IsInConstraint(right_sample)); + EXPECT_FALSE(tsr_const_->IsInConstraint(wrong_sample)); +} + +// check the calucuration result of displacement to constraint +TEST_F(TsrLinkConstraintTest, DisplacementTest) { + Affine3d zero_sample = Affine3d::Identity(); + EXPECT_NEAR(0.0, tsr_const_->CalcDisplacement(zero_sample), kEpsilon); + { + const auto disp = tsr_const_->CalcSeparateDisplacements(zero_sample); + ASSERT_EQ(disp.size(), 6); + EXPECT_NEAR(0.0, disp[0], kEpsilon); + EXPECT_NEAR(0.0, disp[1], kEpsilon); + EXPECT_NEAR(0.0, disp[2], kEpsilon); + EXPECT_NEAR(0.0, disp[3], kEpsilon); + EXPECT_NEAR(0.0, disp[4], kEpsilon); + EXPECT_NEAR(0.0, disp[5], kEpsilon); + } + + Vector3d pos(3.0, 0.0, 0.0); + Affine3d test_sample = Eigen::Translation3d(pos) * Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitY()); + EXPECT_NEAR(2.00249, tsr_const_->CalcDisplacement(test_sample), kEpsilon); + { + const auto disp = tsr_const_->CalcSeparateDisplacements(test_sample); + ASSERT_EQ(disp.size(), 6); + EXPECT_NEAR(2.0, disp[0], kEpsilon); + EXPECT_NEAR(0.0, disp[1], kEpsilon); + EXPECT_NEAR(0.0, disp[2], kEpsilon); + EXPECT_NEAR(0.0, disp[3], kEpsilon); + EXPECT_NEAR(0.1, disp[4], kEpsilon); + EXPECT_NEAR(0.0, disp[5], kEpsilon); + } +} + +TEST_F(TsrLinkConstraintTest, RotationDisplacement) { + RegionValues min = RegionValues::Zero(); + RegionValues max = RegionValues::Zero(); + min << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + max << 0.0, 0.0, 0.0, 0.5, 1.5, 3.0; + TaskSpaceRegion tsr(Affine3d::Identity(), Affine3d::Identity(), min, max, "origin", "hand"); + auto tsr_const = std::make_shared(tsr, 10, static_cast(time(NULL))); + + const std::vector value_candidates = {-3.0, -1.5, -0.5, 0.0, 0.5, 1.5, 3.0}; + std::vector> rot_values_seq; + for (auto x : value_candidates) { + for (auto y : value_candidates) { + for (auto z : value_candidates) { + rot_values_seq.emplace_back(std::array({x, y, z})); + } + } + } + + for (const auto& rot_values : rot_values_seq) { + const Affine3d sample = Eigen::Translation3d::Identity() + * Eigen::AngleAxisd(rot_values[0], Eigen::Vector3d::UnitX()) + * Eigen::AngleAxisd(rot_values[1], Eigen::Vector3d::UnitY()) + * Eigen::AngleAxisd(rot_values[2], Eigen::Vector3d::UnitZ()); + + double expected = 0.0; + if (rot_values[0] < 0.0) { + expected += rot_values[0] * rot_values[0]; + } else if (rot_values[0] > 0.5) { + expected += (rot_values[0] - 0.5) * (rot_values[0] - 0.5); + } + if (rot_values[1] < 0.0) { + expected += rot_values[1] * rot_values[1]; + } else if (rot_values[1] > 1.5) { + expected += (rot_values[1] - 1.5) * (rot_values[1] - 1.5); + } + if (rot_values[2] < 0.0) { + expected += rot_values[2] * rot_values[2]; + } else if (rot_values[2] > 3.0) { + expected += (rot_values[2] - 3.0) * (rot_values[2] - 3.0); + } + expected = std::sqrt(expected); + + SCOPED_TRACE(std::string("values: [") + std::to_string(rot_values[0]) + ", " + std::to_string(rot_values[1]) + + ", " + std::to_string(rot_values[2]) + "]"); + + // Since it is mechanically turned, another small RPY may exist, so compare it with LT instead of EQ. + ASSERT_LT(tsr_const->CalcDisplacement(sample), expected + kEpsilon); + } +} + +// check the calucuration result of closest pose to constraint +TEST_F(TsrLinkConstraintTest, CalcClosestTest) { + RegionValues min; + min << -1.0, 0.0, 0.0, 0.0, 0.0, -0.5; + RegionValues max; + max << 1.0, 0.0, 0.0, 0.0, 0.0, 0.5; + + TaskSpaceRegion tsr(Affine3d::Identity(), Affine3d::Identity(), min, max, "origin", "hand"); + ILinkConstraint::Ptr constraint = std::make_shared(tsr, 0); + { + // As it is in TSR, just as it is + const Affine3d input = Eigen::Translation3d(0.1, 0.0, 0.0) * Eigen::AngleAxisd(0.2, Vector3d::UnitZ()); + const auto result = constraint->CalcClosest(input); + EXPECT_NEAR(0.1, result.translation().x(), kEpsilon); + EXPECT_NEAR(0.0, result.translation().y(), kEpsilon); + EXPECT_NEAR(0.0, result.translation().z(), kEpsilon); + EXPECT_NEAR(0.0, result.linear().eulerAngles(0, 1, 2)[0], kEpsilon); + EXPECT_NEAR(0.0, result.linear().eulerAngles(0, 1, 2)[1], kEpsilon); + EXPECT_NEAR(0.2, result.linear().eulerAngles(0, 1, 2)[2], kEpsilon); + } + { + // Translation is outside MAX + const Affine3d input = Eigen::Translation3d(1.1, 0.0, 0.0) * Eigen::AngleAxisd(0.2, Vector3d::UnitZ()); + const auto result = constraint->CalcClosest(input); + EXPECT_NEAR(1.0, result.translation().x(), kEpsilon); + EXPECT_NEAR(0.0, result.translation().y(), kEpsilon); + EXPECT_NEAR(0.0, result.translation().z(), kEpsilon); + EXPECT_NEAR(0.0, result.linear().eulerAngles(0, 1, 2)[0], kEpsilon); + EXPECT_NEAR(0.0, result.linear().eulerAngles(0, 1, 2)[1], kEpsilon); + EXPECT_NEAR(0.2, result.linear().eulerAngles(0, 1, 2)[2], kEpsilon); + } + { + // Translation is outside the min + const Affine3d input = Eigen::Translation3d(-1.1, 0.0, 0.0) * Eigen::AngleAxisd(0.2, Vector3d::UnitZ()); + const auto result = constraint->CalcClosest(input); + EXPECT_NEAR(-1.0, result.translation().x(), kEpsilon); + EXPECT_NEAR(0.0, result.translation().y(), kEpsilon); + EXPECT_NEAR(0.0, result.translation().z(), kEpsilon); + EXPECT_NEAR(0.0, result.linear().eulerAngles(0, 1, 2)[0], kEpsilon); + EXPECT_NEAR(0.0, result.linear().eulerAngles(0, 1, 2)[1], kEpsilon); + EXPECT_NEAR(0.2, result.linear().eulerAngles(0, 1, 2)[2], kEpsilon); + } + { + // Rotation is outside MAX + const Affine3d input = Eigen::Translation3d(0.1, 0.0, 0.0) * Eigen::AngleAxisd(0.6, Vector3d::UnitZ()); + const auto result = constraint->CalcClosest(input); + EXPECT_NEAR(0.1, result.translation().x(), kEpsilon); + EXPECT_NEAR(0.0, result.translation().y(), kEpsilon); + EXPECT_NEAR(0.0, result.translation().z(), kEpsilon); + EXPECT_NEAR(0.0, result.linear().eulerAngles(0, 1, 2)[0], kEpsilon); + EXPECT_NEAR(0.0, result.linear().eulerAngles(0, 1, 2)[1], kEpsilon); + EXPECT_NEAR(0.5, result.linear().eulerAngles(0, 1, 2)[2], kEpsilon); + } + { + // Rotation is outside the min + const Affine3d input = Eigen::Translation3d(0.1, 0.0, 0.0) * Eigen::AngleAxisd(-0.6, Vector3d::UnitZ()); + const auto result = constraint->CalcClosest(input); + EXPECT_NEAR(0.1, result.translation().x(), kEpsilon); + EXPECT_NEAR(0.0, result.translation().y(), kEpsilon); + EXPECT_NEAR(0.0, result.translation().z(), kEpsilon); + EXPECT_NEAR(0.0, result.linear().eulerAngles(0, 1, 2)[0], kEpsilon); + EXPECT_NEAR(0.0, result.linear().eulerAngles(0, 1, 2)[1], kEpsilon); + EXPECT_NEAR(-0.5, result.linear().eulerAngles(0, 1, 2)[2], kEpsilon); + } +} + +class RotationFirstTsrLinkConstraintTest : public ::testing::Test { + protected: + virtual void SetUp() { + RegionValues min; + RegionValues max; + min << 0.9, 0.0, 0.0, 0.0, 0.0, -1.0; + max << 1.0, 0.0, 0.0, 0.0, 0.0, 1.0; + TaskSpaceRegion tsr(Affine3d::Identity(), + Affine3d::Identity(), + min, + max, + "origin", + "hand"); + tsr_const_ = std::make_shared(tsr, 10, static_cast(time(NULL))); + } + ILinkConstraint::Ptr tsr_const_; +}; + +// check tsr samples pointed if they are within region pointed +TEST_F(RotationFirstTsrLinkConstraintTest, SampleTest) { + std::vector r_values; + for (auto i = 0; i < kSampleTestNumberOfTime; i++) { + const Affine3d result_tsr = tsr_const_->Sample(); + const double r = std::sqrt( + std::pow(result_tsr.translation()[0], 2.0) + std::pow(result_tsr.translation()[1], 2.0)); + r_values.push_back(r); + EXPECT_GE(r, 0.9); + EXPECT_LE(r, 1.0); + + const double yaw_from_pos = std::atan2(result_tsr.translation()[1], result_tsr.translation()[0]); + EXPECT_GE(yaw_from_pos, -1.0); + EXPECT_LE(yaw_from_pos, 1.0); + + const double yaw_from_rot = result_tsr.linear().eulerAngles(0, 1, 2)[2]; + EXPECT_GE(yaw_from_rot, -1.0); + EXPECT_LE(yaw_from_rot, 1.0); + + EXPECT_NEAR(result_tsr.translation()[2], 0.0, kEpsilon); + } + + std::sort(r_values.begin(), r_values.end()); + EXPECT_GE(r_values.back() - r_values.front(), kEpsilon); +} + +// check the calucuration result of displacement to constraint +TEST_F(RotationFirstTsrLinkConstraintTest, DisplacementTest) { + const Affine3d in_tsr_sample = + Eigen::Translation3d(std::cos(0.5), std::sin(0.5), 0.0) * Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitZ()); + EXPECT_NEAR(0.0, tsr_const_->CalcDisplacement(in_tsr_sample), kEpsilon); + + const Affine3d tranration_sample = Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond::Identity(); + EXPECT_NEAR(2.0, tsr_const_->CalcDisplacement(tranration_sample), kEpsilon); + + const Affine3d rotation_sample = + Eigen::Translation3d(0.0, 0.0, 0.0) * Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitZ()); + EXPECT_NEAR(0.9, tsr_const_->CalcDisplacement(rotation_sample), kEpsilon); +} + +// check the calucuration result of closest pose to constraint +TEST_F(RotationFirstTsrLinkConstraintTest, CalcClosestTest) { + { + // As it is in TSR, just as it is + const Affine3d in_tsr_sample = + Eigen::Translation3d(std::cos(0.5), std::sin(0.5), 0.0) * Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitZ()); + const auto result = tsr_const_->CalcClosest(in_tsr_sample); + EXPECT_NEAR(std::cos(0.5), result.translation().x(), kEpsilon); + EXPECT_NEAR(std::sin(0.5), result.translation().y(), kEpsilon); + EXPECT_NEAR(0.0, result.translation().z(), kEpsilon); + EXPECT_NEAR(0.0, result.linear().eulerAngles(0, 1, 2)[0], kEpsilon); + EXPECT_NEAR(0.0, result.linear().eulerAngles(0, 1, 2)[1], kEpsilon); + EXPECT_NEAR(0.5, result.linear().eulerAngles(0, 1, 2)[2], kEpsilon); + } + { + const Affine3d tranration_sample = Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond::Identity(); + const auto result = tsr_const_->CalcClosest(tranration_sample); + EXPECT_NEAR(1.0, result.translation().x(), kEpsilon); + EXPECT_NEAR(0.0, result.translation().y(), kEpsilon); + EXPECT_NEAR(0.0, result.translation().z(), kEpsilon); + EXPECT_NEAR(0.0, result.linear().eulerAngles(0, 1, 2)[0], kEpsilon); + EXPECT_NEAR(0.0, result.linear().eulerAngles(0, 1, 2)[1], kEpsilon); + EXPECT_NEAR(0.0, result.linear().eulerAngles(0, 1, 2)[2], kEpsilon); + } + { + const Affine3d rotation_sample = + Eigen::Translation3d(0.0, 0.0, 0.0) * Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitZ()); + const auto result = tsr_const_->CalcClosest(rotation_sample); + EXPECT_NEAR(0.9 * cos(0.5), result.translation().x(), kEpsilon); + EXPECT_NEAR(0.9 * sin(0.5), result.translation().y(), kEpsilon); + EXPECT_NEAR(0.0, result.translation().z(), kEpsilon); + EXPECT_NEAR(0.0, result.linear().eulerAngles(0, 1, 2)[0], kEpsilon); + EXPECT_NEAR(0.0, result.linear().eulerAngles(0, 1, 2)[1], kEpsilon); + EXPECT_NEAR(0.5, result.linear().eulerAngles(0, 1, 2)[2], kEpsilon); + } + { + const Affine3d rotation_out_sample = + Eigen::Translation3d(0.0, 0.0, 0.0) * Eigen::AngleAxisd(1.5, Eigen::Vector3d::UnitZ()); + const auto result = tsr_const_->CalcClosest(rotation_out_sample); + EXPECT_NEAR(0.9 * cos(1.0), result.translation().x(), kEpsilon); + EXPECT_NEAR(0.9 * sin(1.0), result.translation().y(), kEpsilon); + EXPECT_NEAR(0.0, result.translation().z(), kEpsilon); + EXPECT_NEAR(0.0, result.linear().eulerAngles(0, 1, 2)[0], kEpsilon); + EXPECT_NEAR(0.0, result.linear().eulerAngles(0, 1, 2)[1], kEpsilon); + EXPECT_NEAR(1.0, result.linear().eulerAngles(0, 1, 2)[2], kEpsilon); + } +} + + +} // namespace tmc_robot_local_planner + +int main(int argc, char **argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/tmc_robot_local_planner_utils/CHANGELOG.rst b/tmc_robot_local_planner_utils/CHANGELOG.rst new file mode 100644 index 0000000..7acd879 --- /dev/null +++ b/tmc_robot_local_planner_utils/CHANGELOG.rst @@ -0,0 +1,9 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package tmc_robot_local_planner_utils +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.0.0 (2024-10-15) +------------------- +* Initial release +* Contributors: Keisuke Takeshita, Satoru Onoda + diff --git a/tmc_robot_local_planner_utils/CMakeLists.txt b/tmc_robot_local_planner_utils/CMakeLists.txt new file mode 100644 index 0000000..ddd6e2d --- /dev/null +++ b/tmc_robot_local_planner_utils/CMakeLists.txt @@ -0,0 +1,53 @@ +cmake_minimum_required(VERSION 3.5) +project(tmc_robot_local_planner_utils) + +find_package(ament_cmake REQUIRED) +find_package(Boost REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(tf2_geometry_msgs 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_local_planner REQUIRED) + +add_library(${PROJECT_NAME} SHARED + src/${PROJECT_NAME}/arrival_rate_calculator.cpp + src/${PROJECT_NAME}/common.cpp + src/${PROJECT_NAME}/converter.cpp + src/${PROJECT_NAME}/extractor.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC include ${EIGEN3_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) +ament_target_dependencies(${PROJECT_NAME} tf2_eigen tf2_geometry_msgs tf2_ros tmc_manipulation_types tmc_manipulation_types_bridge tmc_planning_msgs tmc_robot_local_planner) + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) +install(DIRECTORY include/ + DESTINATION include +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest(arrival_rate_calculator_test test/arrival_rate_calculator-test.cpp) + target_link_libraries(arrival_rate_calculator_test ${PROJECT_NAME}) + + ament_add_gtest(common_test test/common-test.cpp) + target_link_libraries(common_test ${PROJECT_NAME}) + + ament_add_gtest(converter_test test/converter-test.cpp) + target_link_libraries(converter_test ${PROJECT_NAME}) + + ament_add_gtest(extractor_test test/extractor-test.cpp) + target_link_libraries(extractor_test ${PROJECT_NAME}) +endif() + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies(tf2_eigen tf2_geometry_msgs tf2_ros tmc_manipulation_types tmc_manipulation_types_bridge tmc_planning_msgs tmc_robot_local_planner) + +ament_package() diff --git a/tmc_robot_local_planner_utils/include/tmc_robot_local_planner_utils/arrival_rate_calculator.hpp b/tmc_robot_local_planner_utils/include/tmc_robot_local_planner_utils/arrival_rate_calculator.hpp new file mode 100644 index 0000000..df1b307 --- /dev/null +++ b/tmc_robot_local_planner_utils/include/tmc_robot_local_planner_utils/arrival_rate_calculator.hpp @@ -0,0 +1,69 @@ +/* +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. +*/ +/// @brief Arrival Rate Calculator Class +#ifndef TMC_ROBOT_LOCAL_PLANNER_UTILS_ARRIVAL_RATE_CALCULATOR_HPP_ +#define TMC_ROBOT_LOCAL_PLANNER_UTILS_ARRIVAL_RATE_CALCULATOR_HPP_ + +#include + +#include +#include + +#include +#include + +namespace tmc_robot_local_planner_utils { + +class ArrivalRateCalculator { + public: + /// @brief constructor + ArrivalRateCalculator(); + /// @brief Destructor + ~ArrivalRateCalculator() {} + /// @brief Calculate the reach to the goal + boost::optional CalculateArrivalRate(const tmc_manipulation_types::RobotState& state_in); + + /// @brief Set Constraint + void SetConstraints(const tmc_robot_local_planner::Constraints& constraints) { + constraints_ = constraints; + is_new_constraints_ = true; + } + + using Ptr = std::shared_ptr; + + private: + /// @brief Normalization within the range of 0-100 + double Normalize(double value); + + tmc_robot_local_planner::Constraints constraints_; + bool is_new_constraints_; + double max_value_; +}; +} // namespace tmc_robot_local_planner_utils + +#endif // TMC_ROBOT_LOCAL_PLANNER_UTILS_ARRIVAL_RATE_CALCULATOR_HPP_ diff --git a/tmc_robot_local_planner_utils/include/tmc_robot_local_planner_utils/common.hpp b/tmc_robot_local_planner_utils/include/tmc_robot_local_planner_utils/common.hpp new file mode 100644 index 0000000..a3df588 --- /dev/null +++ b/tmc_robot_local_planner_utils/include/tmc_robot_local_planner_utils/common.hpp @@ -0,0 +1,73 @@ +/* +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. +*/ +/// @brief Common functions +#ifndef TMC_ROBOT_LOCAL_PLANNER_UTILS_COMMON_HPP_ +#define TMC_ROBOT_LOCAL_PLANNER_UTILS_COMMON_HPP_ + +#include +#include +#include + +#include + +namespace tmc_robot_local_planner_utils { + +// /// @brief Get the MAP origin +// /// @param MAP map +// /// @return Origin of the map +// Eigen::Affine2d GetMapOrigin(const tmc_manipulation_types::OccupancyGrid& map); + +/// @brief Get the tracking point at the end +bool GetLastPoint(const tmc_manipulation_types::TimedRobotTrajectory& trajectory, + tmc_manipulation_types::RobotState& state_out); + +/// @brief Erase orbit points that exceed the specified time +void DeleteTrajectoryPointsAtTime(double time, + tmc_manipulation_types::TimedRobotTrajectory& trajectory); + +/// @brief Erase orbital point in the specified score in the order of time stamps +bool DeleteTrajectoryPointsAtPoint(uint32_t num, trajectory_msgs::msg::JointTrajectory& trajectory); + +/// @brief Joint orbital point and BASE orbital point determine whether they have the same time +bool CheckTimeFromStarts(const tmc_manipulation_types::TimedRobotTrajectory& trajectory); + +/// @brief From the input orbit to the track point with the minimum time in it, larger than the current time+cycle +bool SearchConnectablePoint( + const tf2::Stamped& trajectory, + const rclcpp::Time& target_time, + rclcpp::Time& connectable_time, + tf2::Stamped& state_out); + +/// @brief Shift the orbit time +void ShiftTrajectoryTimes( + const tmc_manipulation_types::TimedRobotTrajectory& trajectory, + double shift_time, + tmc_manipulation_types::TimedRobotTrajectory& trajectory_out); + +} // namespace tmc_robot_local_planner_utils +#endif // TMC_ROBOT_LOCAL_PLANNER_UTILS_COMMON_HPP_ diff --git a/tmc_robot_local_planner_utils/include/tmc_robot_local_planner_utils/converter.hpp b/tmc_robot_local_planner_utils/include/tmc_robot_local_planner_utils/converter.hpp new file mode 100644 index 0000000..a7b0895 --- /dev/null +++ b/tmc_robot_local_planner_utils/include/tmc_robot_local_planner_utils/converter.hpp @@ -0,0 +1,98 @@ +/* +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. +*/ +/// @brief Converter functions +#ifndef TMC_ROBOT_LOCAL_PLANNER_UTILS_CONVERTER_HPP_ +#define TMC_ROBOT_LOCAL_PLANNER_UTILS_CONVERTER_HPP_ + +#include + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +namespace tmc_robot_local_planner_utils { + +/// @brief Return 3D (x, y, theta) +Eigen::Vector3d Get2DPose(const Eigen::Affine3d& transform); + +/// @brief Return the conversion line +Eigen::Affine3d GetTransform(const Eigen::Vector3d& pose2d); + +/// @brief Return Linear.x Linear.y Angular.z +Eigen::Vector3d Get2DTwist(const tmc_manipulation_types::Twist& twist); + +/// @brief Return twist +tmc_manipulation_types::Twist GetTwist(const Eigen::Vector3d& twist2d); + +/// @brief Change each Constraint +bool TransformConstraints(const std::string& origin_frame, + const double timeout, + const tf2_ros::Buffer& buffer, + tmc_planning_msgs::msg::Constraints& msg); + +/// @brief Convert the ROS message type Constraint into a non -ROS type +void ConvertConstraints(const tmc_planning_msgs::msg::Constraints& msg, + tmc_robot_local_planner::Constraints& constraints); + +/// @brief Convert Link Constraint coordinates +bool TransformTSRConstraint(const std::string& origin_frame, + const double timeout, + const tf2_ros::Buffer& buffer, + tmc_planning_msgs::msg::TsrLinkConstraint& msg); + +/// @brief Convert the ROS message type Link Constraint into a non -ROS type +void ConvertLinkConstraintMsgToLinkConstraint( + const tmc_planning_msgs::msg::TsrLinkConstraint& msg, + tmc_robot_local_planner::ILinkConstraint::Ptr& constraint_out); + +/// @brief Coordinates Joint Constraint +bool TransformJointConstraint(const std::string& origin_frame, + const double timeout, + const tf2_ros::Buffer& buffer, + tmc_planning_msgs::msg::RangeJointConstraint& msg); + +/// @brief Convert the ROS message type Joint Constraint into a non -ROS type +void ConvertJointConstraintMsgToJointConstraint( + const tmc_planning_msgs::msg::RangeJointConstraint& msg, + tmc_robot_local_planner::IJointConstraint::Ptr& constraint_out); + +/// @brief Convert the ROS message type Linear Constraint into a non -ROS type +void ConvertLinearConstraintMsgToLinearConstraint( + const tmc_planning_msgs::msg::LinearConstraint& msg, + tmc_robot_local_planner::LinearConstraint& constraint_out); + +} // namespace tmc_robot_local_planner_utils +#endif // TMC_ROBOT_LOCAL_PLANNER_UTILS_CONVERTER_HPP_ diff --git a/tmc_robot_local_planner_utils/include/tmc_robot_local_planner_utils/extractor.hpp b/tmc_robot_local_planner_utils/include/tmc_robot_local_planner_utils/extractor.hpp new file mode 100644 index 0000000..c149e68 --- /dev/null +++ b/tmc_robot_local_planner_utils/include/tmc_robot_local_planner_utils/extractor.hpp @@ -0,0 +1,53 @@ +/* +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. +*/ +/// @brief Extractor functions +#ifndef TMC_ROBOT_LOCAL_PLANNER_UTILS_EXTRACTOR_HPP_ +#define TMC_ROBOT_LOCAL_PLANNER_UTILS_EXTRACTOR_HPP_ + +#include + +#include +#include + +#include + +namespace tmc_robot_local_planner_utils { + +/// @brief Extract partial timed_joint_trajectory from Trajectory for the entire robot +tmc_manipulation_types::TimedJointTrajectory ExtractMultiDOFJointTrajectory( + const tmc_manipulation_types::TimedMultiDOFJointTrajectory& whole_trajectory, + const tmc_manipulation_types::NameSeq& coordinates); + +/// @brief Extract the orbit after the specified time +bool ExtractTrajectoryAtTime( + const tf2::Stamped& trajectory, + const rclcpp::Time& target_time, + tmc_manipulation_types::TimedRobotTrajectory& trajectory_out); + +} // namespace tmc_robot_local_planner_utils +#endif // TMC_ROBOT_LOCAL_PLANNER_UTILS_EXTRACTOR_HPP_ diff --git a/tmc_robot_local_planner_utils/package.xml b/tmc_robot_local_planner_utils/package.xml new file mode 100644 index 0000000..e4e2d03 --- /dev/null +++ b/tmc_robot_local_planner_utils/package.xml @@ -0,0 +1,31 @@ + + tmc_robot_local_planner_utils + 2.0.0 + The tmc_robot_local_planner_utils package + + HSR Support + + Keisuke Takeshita + Satoru Onoda + + BSD 3-clause Clear License + + ament_cmake + + eigen + + boost + tf2_eigen + tf2_geometry_msgs + tf2_ros + tmc_manipulation_types + tmc_manipulation_types_bridge + tmc_planning_msgs + tmc_robot_local_planner + + ament_cmake_gtest + + + ament_cmake + + \ No newline at end of file diff --git a/tmc_robot_local_planner_utils/src/tmc_robot_local_planner_utils/arrival_rate_calculator.cpp b/tmc_robot_local_planner_utils/src/tmc_robot_local_planner_utils/arrival_rate_calculator.cpp new file mode 100644 index 0000000..fa182f2 --- /dev/null +++ b/tmc_robot_local_planner_utils/src/tmc_robot_local_planner_utils/arrival_rate_calculator.cpp @@ -0,0 +1,81 @@ +/* +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. +*/ +/// @brief Arrival Rate Calculator Class + +#include +#include +#include "tmc_robot_local_planner_utils/arrival_rate_calculator.hpp" + +namespace tmc_robot_local_planner_utils { + +ArrivalRateCalculator::ArrivalRateCalculator() : is_new_constraints_(false) { + // TODO(Takeshita) Use TF +} + +boost::optional ArrivalRateCalculator::CalculateArrivalRate( + const tmc_manipulation_types::RobotState& state_in) { + // for (const auto& link_const : constraints_.hard_link_constraints) { + // auto link_name = link_const->GetLinkName(); + // auto origin_to_link = tmc_robot_local_planner_utils::CalcFK(link_name, state_in, robot_); + // if (origin_to_link) { + // satisfaction += link_const->CalcDisplacement(origin_to_link.get()); + // } else { + // return boost::none; + // } + // } + if (!constraints_.hard_link_constraints.empty() && constraints_.hard_joint_constraints.empty()) { + return 0.0; + } else if (constraints_.hard_link_constraints.empty() && constraints_.hard_joint_constraints.empty()) { + return 100.0; + } + + double displacement = 0.0; + for (const auto& joint_const : constraints_.hard_joint_constraints) { + displacement = std::max(displacement, joint_const->CalcDisplacement(state_in)); + } + return Normalize(displacement); +} + +double ArrivalRateCalculator::Normalize(double value) { + if (is_new_constraints_) { + is_new_constraints_ = false; + if (value > std::numeric_limits::min()) { + max_value_ = value; + } else { + return 100.0; + } + } + if (value <= 0.0) { + value = 0.0; + } + if (value > max_value_) { + value = max_value_; + } + return (1.0 - value / max_value_) * 100.0; +} +} // namespace tmc_robot_local_planner_utils diff --git a/tmc_robot_local_planner_utils/src/tmc_robot_local_planner_utils/common.cpp b/tmc_robot_local_planner_utils/src/tmc_robot_local_planner_utils/common.cpp new file mode 100644 index 0000000..a53327c --- /dev/null +++ b/tmc_robot_local_planner_utils/src/tmc_robot_local_planner_utils/common.cpp @@ -0,0 +1,185 @@ +/* +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. +*/ +/// @brief Common functions + +#include + +namespace tmc_robot_local_planner_utils { + +// // Get the MAP origin +// Eigen::Affine2d GetMapOrigin(const tmc_manipulation_types::OccupancyGrid& map) { +// return Eigen::Translation2d(map.info.origin.translation().topRows<2>()) * +// map.info.origin.linear().topLeftCorner<2, 2>(); +// } + +// Acquired the trajectory point at the end +bool GetLastPoint( + const tmc_manipulation_types::TimedRobotTrajectory& trajectory, + tmc_manipulation_types::RobotState& state_out) { + if (trajectory.joint_trajectory.points.empty()) { + return false; + } + if (!CheckTimeFromStarts(trajectory)) { + return false; + } + + tmc_manipulation_types::TimedJointTrajectoryPoint joint_point = + trajectory.joint_trajectory.points.back(); + state_out.joint_state.name = trajectory.joint_trajectory.joint_names; + state_out.joint_state.position = joint_point.positions; + state_out.joint_state.velocity = joint_point.velocities; + tmc_manipulation_types::TimedMultiDOFJointTrajectoryPoint base_point = + trajectory.multi_dof_joint_trajectory.points.back(); + state_out.multi_dof_joint_state.names = + trajectory.multi_dof_joint_trajectory.joint_names; + state_out.multi_dof_joint_state.poses = base_point.transforms; + state_out.multi_dof_joint_state.twist = base_point.velocities; + return true; +} + +// Erase orbit points that exceed the specified time +void DeleteTrajectoryPointsAtTime( + double time, + tmc_manipulation_types::TimedRobotTrajectory& trajectory) { + for (uint32_t i = 0; i < trajectory.joint_trajectory.points.size(); ++i) { + if (trajectory.joint_trajectory.points[i].time_from_start > time) { + trajectory.joint_trajectory.points.erase( + trajectory.joint_trajectory.points.begin() + i, + trajectory.joint_trajectory.points.end()); + trajectory.multi_dof_joint_trajectory.points.erase( + trajectory.multi_dof_joint_trajectory.points.begin() + i, + trajectory.multi_dof_joint_trajectory.points.end()); + return; + } + } +} + +// Erase orbital point in the specified score in the order of time stamps +bool DeleteTrajectoryPointsAtPoint(uint32_t num, + trajectory_msgs::msg::JointTrajectory& trajectory) { + if (num > 0) { + if (trajectory.points.size() > num) { + trajectory.points.erase(trajectory.points.begin(), trajectory.points.begin() + num); + } else { + trajectory.points.erase(trajectory.points.begin(), + trajectory.points.begin() + trajectory.points.size() - 1); + } + return true; + } + + return false; +} + +// Joint orbit and BASE orbital point will have the same time +bool CheckTimeFromStarts( + const tmc_manipulation_types::TimedRobotTrajectory& trajectory) { + if (trajectory.joint_trajectory.points.size() != + trajectory.multi_dof_joint_trajectory.points.size()) { + return false; + } + double previous_time_from_start = -1.0; + for (uint32_t i = 0; i < trajectory.joint_trajectory.points.size(); ++i) { + if (trajectory.joint_trajectory.points[i].time_from_start <= previous_time_from_start) { + return false; + } + if (trajectory.joint_trajectory.points[i].time_from_start != + trajectory.multi_dof_joint_trajectory.points[i].time_from_start) { + return false; + } + previous_time_from_start = trajectory.joint_trajectory.points[i].time_from_start; + } + return true; +} + +// From the input orbit to the track point with the minimum time in it, larger than the current time+cycle +bool SearchConnectablePoint( + const tf2::Stamped& trajectory, + const rclcpp::Time& target_time, + rclcpp::Time& connectable_time, + tf2::Stamped& state_out) { + if (!CheckTimeFromStarts(trajectory)) { + return false; + } + + // The first orbit point larger than the current time+cycle + // Search using JointTrajectory + auto joint_point_it = std::find_if( + trajectory.joint_trajectory.points.begin(), + trajectory.joint_trajectory.points.end(), + [trajectory, target_time](const tmc_manipulation_types::TimedJointTrajectoryPoint& p) { + return tf2::timeToSec(trajectory.stamp_) + p.time_from_start > target_time.seconds(); + }); + + tmc_manipulation_types::TimedJointTrajectoryPoint joint_point; + tmc_manipulation_types::TimedMultiDOFJointTrajectoryPoint base_point; + if (joint_point_it != trajectory.joint_trajectory.points.end()) { + // If you find a connected orbital point + const auto nanoseconds = + std::chrono::duration_cast(trajectory.stamp_.time_since_epoch()).count(); + connectable_time = rclcpp::Time(nanoseconds, target_time.get_clock_type()) + + rclcpp::Duration::from_seconds(joint_point_it->time_from_start); + joint_point = *joint_point_it; + base_point = trajectory.multi_dof_joint_trajectory.points[ + std::distance(trajectory.joint_trajectory.points.begin(), joint_point_it)]; + } else { + return false; + } + + state_out.joint_state.name = trajectory.joint_trajectory.joint_names; + state_out.joint_state.position = joint_point.positions; + state_out.joint_state.velocity = joint_point.velocities; + state_out.multi_dof_joint_state.names = trajectory.multi_dof_joint_trajectory.joint_names; + state_out.multi_dof_joint_state.poses = base_point.transforms; + state_out.multi_dof_joint_state.twist = base_point.velocities; + return true; +} + +// Shift the orbit time +void ShiftTrajectoryTimes( + const tmc_manipulation_types::TimedRobotTrajectory& trajectory, + double shift_time, + tmc_manipulation_types::TimedRobotTrajectory& trajectory_out) { + if (trajectory.joint_trajectory.points[0].time_from_start < shift_time) { + shift_time = trajectory.joint_trajectory.points[0].time_from_start; + } + trajectory_out.joint_trajectory.joint_names = trajectory.joint_trajectory.joint_names; + trajectory_out.multi_dof_joint_trajectory.joint_names = trajectory.multi_dof_joint_trajectory.joint_names; + + for (uint32_t i = 0; i < trajectory.joint_trajectory.points.size(); ++i) { + tmc_manipulation_types::TimedJointTrajectoryPoint joint_point = + trajectory.joint_trajectory.points[i]; + tmc_manipulation_types::TimedMultiDOFJointTrajectoryPoint multi_dof_joint_point = + trajectory.multi_dof_joint_trajectory.points[i]; + joint_point.time_from_start -= shift_time; + multi_dof_joint_point.time_from_start -= shift_time; + trajectory_out.joint_trajectory.points.push_back(joint_point); + trajectory_out.multi_dof_joint_trajectory.points.push_back(multi_dof_joint_point); + } +} + +} // namespace tmc_robot_local_planner_utils diff --git a/tmc_robot_local_planner_utils/src/tmc_robot_local_planner_utils/converter.cpp b/tmc_robot_local_planner_utils/src/tmc_robot_local_planner_utils/converter.cpp new file mode 100644 index 0000000..835081d --- /dev/null +++ b/tmc_robot_local_planner_utils/src/tmc_robot_local_planner_utils/converter.cpp @@ -0,0 +1,254 @@ +/* +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. +*/ +/// @brief Converter functions + +#include + +#include +#include +#include +#include + +#include + +using Eigen::Affine3d; +using Eigen::Vector3d; + +namespace { + +template +bool TryTransform(const T& in, + const std::string& origin_frame, + const double& timeout, + const tf2_ros::Buffer& buffer, + T& out) { + try { + out = buffer.transform(in, origin_frame, tf2::durationFromSec(timeout)); + } catch (tf2::TransformException& ex) { + return false; + } + return true; +} + +bool TransformWithLoop(const std_msgs::msg::Header& header, + const std::string& origin_frame, + const double& timeout, + const tf2_ros::Buffer& buffer, + std::vector& in) { + for (auto& transform : in) { + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped.header = header; + transform_stamped.transform = transform; + geometry_msgs::msg::TransformStamped transformed; + if (TryTransform(transform_stamped, origin_frame, timeout, buffer, transformed)) { + transform = transformed.transform; + } else { + return false; + } + } + return true; +} + +template +bool BatchTransformWithLoop(const Fn& fn, + const std::string& origin_frame, + const double& timeout, + const tf2_ros::Buffer& buffer, + T& constraints) { + for (auto& constraint : constraints) { + if (!fn(origin_frame, timeout, buffer, constraint)) { + return false; + } + } + return true; +} + +template +void BatchConvertConstMsgConstToWithLoop(const Fn& fn, + const std::vector& constraints, + std::vector& out) { + for (auto& constraint : constraints) { + U converted_constraint; + fn(constraint, converted_constraint); + out.push_back(converted_constraint); + } +} + +} // namespace + +namespace tmc_robot_local_planner_utils { + +// Extract x, y, yaw from the conversion matrix +Vector3d Get2DPose(const Affine3d& transform) { + return Eigen::Vector3d(transform.translation().x(), + transform.translation().y(), + transform.rotation().eulerAngles(0, 1, 2).z()); +} + +// The input outputs a conversion line as x, y, yaw +Affine3d GetTransform(const Vector3d& pose2d) { + return Eigen::Translation3d(pose2d.x(), pose2d.y(), 0.0) * + Eigen::AngleAxisd(pose2d.z(), Eigen::Vector3d::UnitZ()); +} + +// Return Linear.x Linear.y Angular.z from Twist +Vector3d Get2DTwist(const tmc_manipulation_types::Twist& twist) { + return Vector3d(twist[0], twist[1], twist[5]); +} + +// Outputs twist as x, y, yaw +tmc_manipulation_types::Twist GetTwist(const Vector3d& twist2d) { + tmc_manipulation_types::Twist twist; + twist << twist2d[0], twist2d[1], 0.0, 0.0, 0.0, twist2d[2]; + return twist; +} + +bool TransformConstraints( + const std::string& origin_frame, + const double timeout, + const tf2_ros::Buffer& buffer, + tmc_planning_msgs::msg::Constraints& msg) { + if (!BatchTransformWithLoop(TransformTSRConstraint, origin_frame, + timeout, buffer, msg.soft_link_constraints)) { + return false; + } + if (!BatchTransformWithLoop(TransformTSRConstraint, origin_frame, + timeout, buffer, msg.hard_link_constraints)) { + return false; + } + if (!BatchTransformWithLoop(TransformJointConstraint, origin_frame, + timeout, buffer, msg.soft_joint_constraints)) { + return false; + } + if (!BatchTransformWithLoop(TransformJointConstraint, origin_frame, + timeout, buffer, msg.hard_joint_constraints)) { + return false; + } + if (!BatchTransformWithLoop(TransformTSRConstraint, origin_frame, + timeout, buffer, msg.hard_path_link_constraints)) { + return false; + } + if (!BatchTransformWithLoop(TransformJointConstraint, origin_frame, + timeout, buffer, msg.soft_path_joint_constraints)) { + return false; + } + return true; +} + +void ConvertConstraints(const tmc_planning_msgs::msg::Constraints& msg, + tmc_robot_local_planner::Constraints& constraints) { + BatchConvertConstMsgConstToWithLoop(ConvertLinkConstraintMsgToLinkConstraint, + msg.soft_link_constraints, + constraints.soft_link_constraints); + BatchConvertConstMsgConstToWithLoop(ConvertLinkConstraintMsgToLinkConstraint, + msg.hard_link_constraints, + constraints.hard_link_constraints); + BatchConvertConstMsgConstToWithLoop(ConvertJointConstraintMsgToJointConstraint, + msg.soft_joint_constraints, + constraints.soft_joint_constraints); + BatchConvertConstMsgConstToWithLoop(ConvertJointConstraintMsgToJointConstraint, + msg.hard_joint_constraints, + constraints.hard_joint_constraints); + BatchConvertConstMsgConstToWithLoop(ConvertLinkConstraintMsgToLinkConstraint, + msg.hard_path_link_constraints, + constraints.hard_path_link_constraints); + ConvertLinearConstraintMsgToLinearConstraint(msg.goal_relative_linear_constraint, + constraints.goal_relative_linear_constraint); + BatchConvertConstMsgConstToWithLoop(ConvertJointConstraintMsgToJointConstraint, + msg.soft_path_joint_constraints, + constraints.soft_path_joint_constraints); +} + +bool TransformTSRConstraint( + const std::string& origin_frame, + const double timeout, + const tf2_ros::Buffer& buffer, + tmc_planning_msgs::msg::TsrLinkConstraint& constraint) { + geometry_msgs::msg::PoseStamped origin_to_tsr; + origin_to_tsr.header = constraint.header; + origin_to_tsr.pose = constraint.tsr.origin_to_tsr; + geometry_msgs::msg::PoseStamped transformed; + constraint.tsr.origin_to_tsr = transformed.pose; + if (TryTransform(origin_to_tsr, origin_frame, timeout, buffer, transformed)) { + constraint.tsr.origin_to_tsr = transformed.pose; + } else { + return false; + } + return true; +} + +void ConvertLinkConstraintMsgToLinkConstraint( + const tmc_planning_msgs::msg::TsrLinkConstraint& msg, + tmc_robot_local_planner::ILinkConstraint::Ptr& constraint_out) { + tmc_manipulation_types::TaskSpaceRegion tsr; + tmc_manipulation_types_bridge::TaskSpaceRegionMsgToTaskSpaceRegion(msg.tsr, tsr); + if (msg.tsr.rotation_first) { + constraint_out = std::make_shared( + tsr, 0, std::chrono::system_clock::now().time_since_epoch().count()); + } else { + constraint_out = std::make_shared( + tsr, 0, std::chrono::system_clock::now().time_since_epoch().count()); + } +} + +bool TransformJointConstraint( + const std::string& origin_frame, + const double timeout, + const tf2_ros::Buffer& buffer, + tmc_planning_msgs::msg::RangeJointConstraint& constraint) { + if (!TransformWithLoop(constraint.header, origin_frame, timeout, buffer, + constraint.min.multi_dof_joint_state.transforms)) { + return false; + } + if (!TransformWithLoop(constraint.header, origin_frame, timeout, buffer, + constraint.max.multi_dof_joint_state.transforms)) { + return false; + } + return true; +} + +void ConvertJointConstraintMsgToJointConstraint( + const tmc_planning_msgs::msg::RangeJointConstraint& msg, + tmc_robot_local_planner::IJointConstraint::Ptr& constraint_out) { + tmc_manipulation_types::RobotState min; + tmc_manipulation_types::RobotState max; + tmc_manipulation_types_bridge::RobotStateMsgToRobotState(msg.min, min); + tmc_manipulation_types_bridge::RobotStateMsgToRobotState(msg.max, max); + constraint_out = std::make_shared(min, max, 0, 0); +} + +void ConvertLinearConstraintMsgToLinearConstraint( + const tmc_planning_msgs::msg::LinearConstraint& msg, + tmc_robot_local_planner::LinearConstraint& constraint_out) { + constraint_out.end_frame_id = msg.end_frame_id; + // You can make a zero judgment here, but the conversion function is determined by the policy to focus on conversion, and the use side. + tf2::fromMsg(msg.axis, constraint_out.axis); + constraint_out.distance = msg.distance; +} + +} // namespace tmc_robot_local_planner_utils diff --git a/tmc_robot_local_planner_utils/src/tmc_robot_local_planner_utils/extractor.cpp b/tmc_robot_local_planner_utils/src/tmc_robot_local_planner_utils/extractor.cpp new file mode 100644 index 0000000..4c3410b --- /dev/null +++ b/tmc_robot_local_planner_utils/src/tmc_robot_local_planner_utils/extractor.cpp @@ -0,0 +1,100 @@ +/* +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. +*/ +/// @brief Extractor functions + +#include + +#include +#include + +#include +#include + + +using Eigen::VectorXd; + +namespace tmc_robot_local_planner_utils { + +// Extract partial timed_joint_trajectory from Trajectory for the entire robot +tmc_manipulation_types::TimedJointTrajectory ExtractMultiDOFJointTrajectory( + const tmc_manipulation_types::TimedMultiDOFJointTrajectory& whole_trajectory, + const tmc_manipulation_types::NameSeq& coordinates) { + tmc_manipulation_types::TimedJointTrajectory trajectory; + trajectory.joint_names = coordinates; + for (const auto& whole_point : whole_trajectory.points) { + tmc_manipulation_types::TimedJointTrajectoryPoint partial_point; + if (!whole_point.transforms.empty()) { + partial_point.positions = Get2DPose(whole_point.transforms[0]); + } + if (!whole_point.velocities.empty()) { + partial_point.velocities = Get2DTwist(whole_point.velocities[0]); + } + if (!whole_point.accelerations.empty()) { + partial_point.accelerations = Get2DTwist(whole_point.accelerations[0]); + } + partial_point.time_from_start = whole_point.time_from_start; + trajectory.points.push_back(partial_point); + } + return trajectory; +} + +// Extract the orbit after the specified time +bool ExtractTrajectoryAtTime( + const tf2::Stamped& trajectory, + const rclcpp::Time& target_time, + tmc_manipulation_types::TimedRobotTrajectory& trajectory_out) { + if (!CheckTimeFromStarts(trajectory)) { + return false; + } + trajectory_out.joint_trajectory.joint_names = trajectory.joint_trajectory.joint_names; + trajectory_out.multi_dof_joint_trajectory.joint_names = trajectory.multi_dof_joint_trajectory.joint_names; + + auto joint_point_it = std::find_if( + trajectory.joint_trajectory.points.begin(), + trajectory.joint_trajectory.points.end(), + [trajectory, target_time](const tmc_manipulation_types::TimedJointTrajectoryPoint& p) { + return tf2::timeToSec(trajectory.stamp_) + p.time_from_start > target_time.seconds();}); + + if (joint_point_it != trajectory.joint_trajectory.points.end()) { + uint32_t current_index = std::distance(trajectory.joint_trajectory.points.begin(), + joint_point_it); + for (uint32_t i = current_index; i < trajectory.joint_trajectory.points.size(); ++i) { + tmc_manipulation_types::TimedJointTrajectoryPoint joint_point = + trajectory.joint_trajectory.points[i]; + tmc_manipulation_types::TimedMultiDOFJointTrajectoryPoint multi_dof_joint_point = + trajectory.multi_dof_joint_trajectory.points[i]; + trajectory_out.joint_trajectory.points.push_back(joint_point); + trajectory_out.multi_dof_joint_trajectory.points.push_back(multi_dof_joint_point); + } + } else { + return false; + } + return true; +} + +} // namespace tmc_robot_local_planner_utils diff --git a/tmc_robot_local_planner_utils/test/arrival_rate_calculator-test.cpp b/tmc_robot_local_planner_utils/test/arrival_rate_calculator-test.cpp new file mode 100644 index 0000000..3922727 --- /dev/null +++ b/tmc_robot_local_planner_utils/test/arrival_rate_calculator-test.cpp @@ -0,0 +1,265 @@ +/* +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. +*/ +/// @brief ARRIVALRATECALCULATOR test + +#include + +#include +#include +// #include +#include + +namespace tmc_robot_local_planner_utils { + +class ArrivalRateCalculatorTest : public ::testing::Test { + protected: + ArrivalRateCalculatorTest() {} + + void SetUp() override; + void TearDown() override; + + ArrivalRateCalculator::Ptr arrival_rate_calculator_; + std::vector joint_names_ = {"joint1", "joint2", "joint3", + "joint4", "joint5", "joint6"}; + tmc_robot_local_planner::Constraints constraints_; + tmc_manipulation_types::RobotState min_; + tmc_manipulation_types::RobotState max_; + // tmc_manipulation_types::TaskSpaceRegion tsr_; + tmc_manipulation_types::RobotState robot_state_; +}; + +void ArrivalRateCalculatorTest::SetUp() { + // Instance generation + arrival_rate_calculator_ = std::make_shared(); + + // RangeJointConstraint settings + min_.joint_state.name = joint_names_; + min_.joint_state.position.resize(joint_names_.size()); + min_.joint_state.position << 1.1, 1.2, 1.3, 1.4, 1.5, 1.6; + max_ = min_; + + // // TSRLINKCONSTRAINT settings + // tsr_.end_frame_id = "link7"; + // Eigen::Translation3d translation(0.0, 0.0, 0.0); + // Eigen::Quaterniond quaternion(1.0, 0.0, 0.0, 0.0); + // tsr_.tsr_to_end = translation * quaternion; + // Eigen::VectorXd bounds; + // bounds.resize(6); + // bounds << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + // tsr_.min_bounds = bounds; + // tsr_.max_bounds = tsr_.min_bounds; + + robot_state_.joint_state.name = joint_names_; + robot_state_.joint_state.position.resize(joint_names_.size()); + robot_state_.joint_state.position << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6; + robot_state_.multi_dof_joint_state.names.push_back("world_joint"); + robot_state_.multi_dof_joint_state.poses.push_back(Eigen::Affine3d::Identity()); +} + +void ArrivalRateCalculatorTest::TearDown() { + constraints_.hard_joint_constraints.clear(); + constraints_.hard_link_constraints.clear(); +} + +// Arrivalrate can be calculated at Hard_Joint_constraints +TEST_F(ArrivalRateCalculatorTest, JointConstraintCalculate) { + // setup + constraints_.hard_joint_constraints.push_back( + std::make_shared(min_, max_, 0, 0)); + + // Arrivalrate when receiving Constraint + arrival_rate_calculator_->SetConstraints(constraints_); + auto arrival_rate = arrival_rate_calculator_->CalculateArrivalRate(robot_state_); + ASSERT_TRUE(static_cast(arrival_rate)); + // The first arrival rate of constraint should be 0% + EXPECT_NEAR(arrival_rate.get(), 0.0, 0.01); + + // Arrivalrate when not receiving Constraint + robot_state_.joint_state.position << 0.6, 0.7, 0.8, 0.9, 1.0, 1.1; + arrival_rate = arrival_rate_calculator_->CalculateArrivalRate(robot_state_); + ASSERT_TRUE(static_cast(arrival_rate)); + // If it moves halfway, the reach should be 50% + EXPECT_NEAR(arrival_rate.get(), 50.0, 0.01); + + robot_state_.joint_state.position << 1.1, 1.2, 1.3, 1.4, 1.5, 1.6; + arrival_rate = arrival_rate_calculator_->CalculateArrivalRate(robot_state_); + ASSERT_TRUE(static_cast(arrival_rate)); + // If you reach the goal, it should be 100% + EXPECT_NEAR(arrival_rate.get(), 100.0, 0.01); +} + +// // Arrivalrate can be calculated at hard_link_constraints +// TEST_F(ArrivalRateCalculatorTest, LinkConstraintCalculate) { +// // setup +// // Link7 position when each Joint is 0.0 +// Eigen::Translation3d translation(0.0, -0.25, 0.95); +// Eigen::Quaterniond quaternion(1.0, 0.0, 0.0, 0.0); +// tsr_.origin_to_tsr = translation * quaternion; +// constraints_.hard_link_constraints.push_back( +// std::make_shared(tsr_, 0, 0)); + +// // Arrivalrate when receiving Constraint +// arrival_rate_calculator_->SetConstraints(constraints_); +// // Move only the direct motion axis JOINR3 +// robot_state_.joint_state.position << 0.0, 0.0, 0.2, 0.0, 0.0, 0.0; +// auto arrival_rate = arrival_rate_calculator_->CalculateArrivalRate(robot_state_); +// ASSERT_TRUE(static_cast(arrival_rate)); +// // The first arrival rate of constraint should be 0% +// EXPECT_NEAR(arrival_rate.get(), 0.0, 0.01); + +// // Arrivalrate when not receiving Constraint +// robot_state_.joint_state.position << 0.0, 0.0, 0.1, 0.0, 0.0, 0.0; +// arrival_rate = arrival_rate_calculator_->CalculateArrivalRate(robot_state_); +// ASSERT_TRUE(static_cast(arrival_rate)); +// // JOINT3 should be 50%with the initial value+half the target value. +// EXPECT_NEAR(arrival_rate.get(), 50, 0.01); + +// robot_state_.joint_state.position << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; +// arrival_rate = arrival_rate_calculator_->CalculateArrivalRate(robot_state_); +// ASSERT_TRUE(static_cast(arrival_rate)); +// // Each Joint should be 0.0 and the reach is 100% +// EXPECT_NEAR(arrival_rate.get(), 100, 0.01); +// } + +// // Hard_link_constraints when there is no robot model +// TEST_F(ArrivalRateCalculatorTest, LinkConstraintsCannotCalculatedWithoutKinematicsModel) { +// // setup +// Eigen::Translation3d translation(1.0, 1.0, 1.0); +// Eigen::Quaterniond quaternion(1.0, 0.0, 0.0, 0.0); +// tsr_.origin_to_tsr = translation * quaternion; +// constraints_.hard_link_constraints.push_back( +// std::make_shared(tsr_, 0, 0)); + +// // Hold the robot model parameter and delete it +// std::string keep_description; +// nh_.getParam("robot_description", keep_description); +// nh_.deleteParam("robot_description"); +// // Confirm that there is no +// ASSERT_FALSE(nh_.hasParam("robot_description")); + +// // Instance generation +// arrival_rate_calculator_ = ArrivalRateCalculator::Ptr(new ArrivalRateCalculator()); + +// // You can't calculate without a robot model +// arrival_rate_calculator_->SetConstraints(constraints_); +// auto arrival_rate = arrival_rate_calculator_->CalculateArrivalRate(robot_state_); +// ASSERT_FALSE(static_cast(arrival_rate)); + +// // teardown +// nh_.setParam("robot_description", keep_description); +// } + +// // Hard_joint_constraints when there is no robot model +// TEST_F(ArrivalRateCalculatorTest, JointConstraintsCanCalculatedWithoutKinematicsModel) { +// // setup +// constraints_.hard_joint_constraints.push_back( +// std::make_shared(min_, max_, 0, 0)); + +// // Hold the robot model parameter and delete it +// std::string keep_description; +// nh_.getParam("robot_description", keep_description); +// nh_.deleteParam("robot_description"); +// // Confirm that there is no +// ASSERT_FALSE(nh_.hasParam("robot_description")); + +// // Instance generation +// arrival_rate_calculator_ = ArrivalRateCalculator::Ptr(new ArrivalRateCalculator()); + +// // Joint_constraint is calculated by displacement, so you can calculate without a robot model. +// arrival_rate_calculator_->SetConstraints(constraints_); +// auto arrival_rate = arrival_rate_calculator_->CalculateArrivalRate(robot_state_); +// ASSERT_TRUE(static_cast(arrival_rate)); +// EXPECT_NEAR(arrival_rate.get(), 0.0, 0.01); + +// robot_state_.joint_state.position << 0.6, 0.7, 0.8, 0.9, 1.0, 1.1; +// arrival_rate = arrival_rate_calculator_->CalculateArrivalRate(robot_state_); +// ASSERT_TRUE(static_cast(arrival_rate)); +// EXPECT_NEAR(arrival_rate.get(), 50.0, 0.01); + +// // teardown +// nh_.setParam("robot_description", keep_description); +// } + +// // When both Hard_link_constraint and Hard_joint_constraint +// TEST_F(ArrivalRateCalculatorTest, JointAndLinkConstraints) { +// // setup +// // Hard_Joint_constraint settings +// tmc_manipulation_types::RobotState min; +// min.joint_state.name.push_back("joint6"); +// min.joint_state.position.resize(1); +// min.joint_state.position << 0.0; +// tmc_manipulation_types::RobotState max = min; +// constraints_.hard_joint_constraints.push_back( +// std::make_shared(min, max, 0, 0)); + +// // Link7 position when each Joint is 0.0 +// Eigen::Translation3d translation(0.0, -0.25, 0.95); +// Eigen::Quaterniond quaternion(1.0, 0.0, 0.0, 0.0); +// tsr_.origin_to_tsr = translation * quaternion; +// constraints_.hard_link_constraints.push_back( +// std::make_shared(tsr_, 0, 0)); + +// // Arrivalrate when receiving Constraint +// arrival_rate_calculator_->SetConstraints(constraints_); +// robot_state_.joint_state.position << 0.0, 0.0, 0.2, 0.0, 0.0, 0.2; +// auto arrival_rate = arrival_rate_calculator_->CalculateArrivalRate(robot_state_); +// ASSERT_TRUE(static_cast(arrival_rate)); +// // The first arrival rate of constraint should be 0% +// EXPECT_NEAR(arrival_rate.get(), 0.0, 0.01); + +// // Arrivalrate when not receiving Constraint +// robot_state_.joint_state.position << 0.0, 0.0, 0.1, 0.0, 0.0, 0.1; +// arrival_rate = arrival_rate_calculator_->CalculateArrivalRate(robot_state_); +// ASSERT_TRUE(static_cast(arrival_rate)); +// // If it moves halfway, the reach should be 50% +// EXPECT_NEAR(arrival_rate.get(), 50.0, 0.01); + +// robot_state_.joint_state.position << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; +// arrival_rate = arrival_rate_calculator_->CalculateArrivalRate(robot_state_); +// ASSERT_TRUE(static_cast(arrival_rate)); +// // If you reach the goal, it should be 100% +// EXPECT_NEAR(arrival_rate.get(), 100.0, 0.01); +// } + +// When constraint is empty +TEST_F(ArrivalRateCalculatorTest, EmptyConstraints) { + // Set the empty Constraint + arrival_rate_calculator_->SetConstraints(constraints_); + + // If constraint is empty, the reach will be 100% + auto arrival_rate = arrival_rate_calculator_->CalculateArrivalRate(robot_state_); + ASSERT_TRUE(static_cast(arrival_rate)); + EXPECT_NEAR(arrival_rate.get(), 100.0, 0.01); +} + +} // namespace tmc_robot_local_planner_utils + +int main(int argc, char **argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/tmc_robot_local_planner_utils/test/common-test.cpp b/tmc_robot_local_planner_utils/test/common-test.cpp new file mode 100644 index 0000000..9bdcd6a --- /dev/null +++ b/tmc_robot_local_planner_utils/test/common-test.cpp @@ -0,0 +1,224 @@ +/* +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. +*/ +/// @brief Common test + +#include + +#include + +#include + +#include "test_base.hpp" + +namespace tmc_robot_local_planner_utils { + +class CommonTest : public TestBase { + protected: + void SetUp(); +}; + +void CommonTest::SetUp() { + TestBase::Initialize(); +} + +// TEST_F(CommonTest, GetMapOrigin) { +// // setup +// tmc_manipulation_types::Pose origin; +// origin = Eigen::Translation3d(0.5, 0.5, 0.0) * +// Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()) * +// Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * +// Eigen::AngleAxisd(1.57, Eigen::Vector3d::UnitZ()); +// tmc_manipulation_types::OccupancyGrid map; +// map.info.resolution = 0.05; +// map.info.width = 1; +// map.info.height = 1; +// map.info.origin = origin; +// int32_t max_data = static_cast((static_cast(map.info.width) / map.info.resolution) +// * (static_cast(map.info.height) / map.info.resolution)); +// map.data.resize(max_data); +// std::fill(map.data.begin(), map.data.end(), 0); +// Eigen::Affine2d result = GetMapOrigin(map); +// EXPECT_DOUBLE_EQ(result.translation().x(), 0.5); +// EXPECT_DOUBLE_EQ(result.translation().y(), 0.5); +// EXPECT_DOUBLE_EQ(result(0, 0), std::cos(1.57)); +// EXPECT_DOUBLE_EQ(result(0, 1), -std::sin(1.57)); +// EXPECT_DOUBLE_EQ(result(1, 0), std::sin(1.57)); +// EXPECT_DOUBLE_EQ(result(1, 1), std::cos(1.57)); +// } + +TEST_F(CommonTest, GetLastPointTest) { + // Normal test + tmc_manipulation_types::RobotState state; + ASSERT_TRUE(GetLastPoint(trajectory_, state)); + for (int32_t i = 0; i < joint_names_.size(); ++i) { + EXPECT_EQ(state.joint_state.name[i], joint_names_[i]); + EXPECT_DOUBLE_EQ(state.joint_state.position[i], 0.1 * (1 + i) * 3); + EXPECT_DOUBLE_EQ(state.joint_state.velocity[i], 0.1 * (1 + i) * 4); + } + for (int32_t i = 0; i < base_names_.size(); ++i) { + EXPECT_EQ(state.multi_dof_joint_state.names[i], base_names_[i]); + } + EXPECT_DOUBLE_EQ(state.multi_dof_joint_state.poses[0].translation().x(), 0.2); + EXPECT_DOUBLE_EQ(state.multi_dof_joint_state.twist[0][0], 0.4); + // When time reverses + trajectory_.joint_trajectory.points[0].time_from_start = 6.0; + ASSERT_FALSE(GetLastPoint(trajectory_, state)); + trajectory_.joint_trajectory.points[0].time_from_start = 1.0; + // When the track of the sky + trajectory_.joint_trajectory.points.clear(); + ASSERT_FALSE(GetLastPoint(trajectory_, state)); +} + +TEST_F(CommonTest, DeleteTrajectoryPointsAtTime) { + tf2::Stamped trajectory = trajectory_; + // Normal test + DeleteTrajectoryPointsAtTime(2.0, trajectory); + EXPECT_EQ(trajectory.joint_trajectory.points.size(), 2); + for (int32_t i = 0; i < trajectory.joint_trajectory.points.size(); ++i) { + EXPECT_DOUBLE_EQ(trajectory.joint_trajectory.points[i].time_from_start, (i + 1) * 1.0); + EXPECT_DOUBLE_EQ(trajectory.multi_dof_joint_trajectory.points[i].time_from_start, (i + 1) * 1.0); + } +} + +TEST_F(CommonTest, DeleteTrajectoryPointsAtPoint) { + trajectory_msgs::msg::JointTrajectory trajectory; + int32_t point_size = 3; + trajectory.joint_names = {base_names_[0], base_names_[1], base_names_[2]}; + trajectory.points.resize(point_size); + for (int32_t i = 0; i < point_size; ++i) { + for (int32_t j = 0; j < trajectory.joint_names.size(); ++j) { + trajectory.points[i].positions.push_back(static_cast(j + i + 1)); + trajectory.points[i].velocities.push_back(static_cast(j + i + 2)); + trajectory.points[i].accelerations.push_back(static_cast(j + i + 3)); + trajectory.points[i].effort.push_back(static_cast(j + i + 4)); + } + } + int num = 1; + // Normal test + ASSERT_TRUE(DeleteTrajectoryPointsAtPoint(num, trajectory)); + EXPECT_EQ(trajectory.points.size(), point_size - num); + for (int32_t i = 0; i < point_size - num; ++i) { + for (int32_t j = 0; j < trajectory.joint_names.size(); ++j) { + EXPECT_DOUBLE_EQ(trajectory.points[i].positions[j], static_cast(j + i + 1 + num)); + EXPECT_DOUBLE_EQ(trajectory.points[i].velocities[j], static_cast(j + i + 2 + num)); + EXPECT_DOUBLE_EQ(trajectory.points[i].accelerations[j], static_cast(j + i + 3 + num)); + EXPECT_DOUBLE_EQ(trajectory.points[i].effort[j], static_cast(j + i + 4 + num)); + } + } + // TRAJECTORY score <= NUM ​​will return 1 point + num = 3; + ASSERT_TRUE(DeleteTrajectoryPointsAtPoint(num, trajectory)); + EXPECT_EQ(trajectory.points.size(), 1); + for (int32_t i = 0; i < trajectory.joint_names.size(); ++i) { + EXPECT_DOUBLE_EQ(trajectory.points[0].positions[i], static_cast(i+3)); + EXPECT_DOUBLE_EQ(trajectory.points[0].velocities[i], static_cast(i+4)); + EXPECT_DOUBLE_EQ(trajectory.points[0].accelerations[i], static_cast(i+5)); + EXPECT_DOUBLE_EQ(trajectory.points[0].effort[i], static_cast(i+6)); + } + // When the input value is not larger than 0 + num = 0; + ASSERT_FALSE(DeleteTrajectoryPointsAtPoint(num, trajectory)); +} + +TEST_F(CommonTest, CheckTimeFromStartsTest) { + // Normal test + ASSERT_TRUE(CheckTimeFromStarts(trajectory_)); + // When Joint_trajectory and Multi_dof_Trajectory Time_from_start do not match + trajectory_.joint_trajectory.points[0].time_from_start = 1.1; + ASSERT_FALSE(CheckTimeFromStarts(trajectory_)); + trajectory_.joint_trajectory.points[0].time_from_start = 1.0; + // When the 0th and the first Time_from_start were the same + trajectory_.joint_trajectory.points[1].time_from_start = 1.0; + trajectory_.multi_dof_joint_trajectory.points[1].time_from_start = 1.0; + ASSERT_FALSE(CheckTimeFromStarts(trajectory_)); + trajectory_.joint_trajectory.points[1].time_from_start = 2.0; + trajectory_.multi_dof_joint_trajectory.points[1].time_from_start = 2.0; + // When Joint_trajectory Points and Multi_dof_Joint_trajectory Points are not available + trajectory_.joint_trajectory.points.resize(6); + trajectory_.joint_trajectory.points[5].time_from_start = 6.0; + trajectory_.joint_trajectory.points[5].positions.resize(1); + trajectory_.joint_trajectory.points[5].positions << 0.5; + trajectory_.joint_trajectory.points[5].velocities.resize(1); + trajectory_.joint_trajectory.points[5].velocities << 0.5; + ASSERT_FALSE(CheckTimeFromStarts(trajectory_)); +} + +TEST_F(CommonTest, SearchConnectablePointTest) { + // Normal test + rclcpp::Time target_time(102, 500000000); + rclcpp::Time connectable_time; + tf2::Stamped state; + ASSERT_TRUE(SearchConnectablePoint(trajectory_, target_time, connectable_time, state)); + EXPECT_NEAR(103.0, connectable_time.seconds(), 1e-5); + EXPECT_EQ(connectable_time.get_clock_type(), target_time.get_clock_type()); + EXPECT_DOUBLE_EQ(state.joint_state.position[0], 0.3); + EXPECT_DOUBLE_EQ(state.joint_state.velocity[0], 0.4); + EXPECT_DOUBLE_EQ(state.multi_dof_joint_state.poses[0].translation().x(), 0.2); + EXPECT_DOUBLE_EQ(state.multi_dof_joint_state.twist[0][0], 0.4); + // When CheckTimeFromstarts is False + trajectory_.joint_trajectory.points[0].time_from_start = 6.0; + ASSERT_FALSE(SearchConnectablePoint(trajectory_, target_time, connectable_time, state)); + trajectory_.joint_trajectory.points[0].time_from_start = 1.0; + // When the current time + Perido is not connected on the orbit + target_time = rclcpp::Time(104, 0); + ASSERT_FALSE(SearchConnectablePoint(trajectory_, target_time, connectable_time, state)); + // When the past time is entered + target_time = rclcpp::Time(99, 0); + ASSERT_TRUE(SearchConnectablePoint(trajectory_, target_time, connectable_time, state)); + EXPECT_NEAR(101.0, connectable_time.seconds(), 1e-5); + EXPECT_DOUBLE_EQ(state.joint_state.position[0], 0.1); + EXPECT_DOUBLE_EQ(state.joint_state.velocity[0], 0.2); + EXPECT_DOUBLE_EQ(state.multi_dof_joint_state.poses[0].translation().x(), 0.0); + EXPECT_DOUBLE_EQ(state.multi_dof_joint_state.twist[0][0], 0.0); +} + +TEST_F(CommonTest, ShiftTrajectoryTimesTest) { + // Normal test + double shift_time = 0.5; + tmc_manipulation_types::TimedRobotTrajectory trajectory_out; + ShiftTrajectoryTimes(trajectory_, shift_time, trajectory_out); + for (uint32_t i = 0; i < trajectory_out.joint_trajectory.points.size(); ++i) { + EXPECT_DOUBLE_EQ(trajectory_out.joint_trajectory.points[i].time_from_start, + trajectory_.joint_trajectory.points[i].time_from_start - shift_time); + } + // When Shift_Time is larger than the first orbital point + shift_time = 1.1; + trajectory_out = tmc_manipulation_types::TimedRobotTrajectory(); + ShiftTrajectoryTimes(trajectory_, shift_time, trajectory_out); + shift_time = trajectory_.joint_trajectory.points[0].time_from_start; + for (uint32_t i = 0; i < trajectory_out.joint_trajectory.points.size(); ++i) { + EXPECT_DOUBLE_EQ(trajectory_out.joint_trajectory.points[i].time_from_start, + trajectory_.joint_trajectory.points[i].time_from_start - shift_time); + } +} +} // namespace tmc_robot_local_planner_utils + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/tmc_robot_local_planner_utils/test/converter-test.cpp b/tmc_robot_local_planner_utils/test/converter-test.cpp new file mode 100644 index 0000000..142364d --- /dev/null +++ b/tmc_robot_local_planner_utils/test/converter-test.cpp @@ -0,0 +1,319 @@ +/* +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. +*/ +/// @brief Converter test + +#include + +#include + +#include +#include + +#include + +#include "test_base.hpp" + +namespace tmc_robot_local_planner_utils { + +class ConverterTest : public TestBase { + protected: + void SetUp() override; + tmc_planning_msgs::msg::TaskSpaceRegion tsr_; + tmc_planning_msgs::msg::TsrLinkConstraint link_const_; + tmc_planning_msgs::msg::RangeJointConstraint rjc_; + tmc_planning_msgs::msg::LinearConstraint linear_const_; + geometry_msgs::msg::Transform base_to_target_pos_; +}; + +void ConverterTest::SetUp() { + TestBase::Initialize(); + + tsr_.origin_to_tsr.position.x = 2.0; + tsr_.origin_to_tsr.position.y = 1.0; + tsr_.origin_to_tsr.orientation.z = std::sin(1.5707945 / 2); + tsr_.origin_to_tsr.orientation.w = std::cos(1.5707945 / 2); + tsr_.min_bounds = std::array({0.0, 0.0, 0.0, 0.0, 0.0, 0.0}); + tsr_.max_bounds = std::array({0.0, 0.0, 0.0, 0.0, 0.0, 0.0}); + tsr_.tsr_to_end.orientation.w = 1.0; + tsr_.end_frame_id = "hand"; + link_const_.tsr = tsr_; + link_const_.header.frame_id = "map"; + + base_to_target_pos_.translation.x = 2.0; + base_to_target_pos_.translation.y = 1.0; + rjc_.header.frame_id = "map"; + rjc_.min.joint_state.name = {"arm"}; + rjc_.min.joint_state.position = {1.0}; + rjc_.min.multi_dof_joint_state.joint_names = {"world_joint"}; + rjc_.min.multi_dof_joint_state.transforms = {base_to_target_pos_}; + rjc_.max = rjc_.min; + + linear_const_.end_frame_id = "hand"; + linear_const_.axis.x = 0.1; + linear_const_.axis.y = 0.2; + linear_const_.axis.z = 0.3; + linear_const_.distance = 0.4; +} + +TEST_F(ConverterTest, Get2DPose) { + Eigen::Vector3d result = Get2DPose(trajectory_.multi_dof_joint_trajectory.points[1].transforms[0]); + EXPECT_DOUBLE_EQ(result[0], 0.1); + EXPECT_DOUBLE_EQ(result[1], 0.0); + EXPECT_DOUBLE_EQ(result[2], 0.0); +} + +TEST_F(ConverterTest, GetTransform) { + Eigen::Vector3d pose2d(1.0, 2.0, 1.57); + Eigen::Affine3d result = GetTransform(pose2d); + + double cos = std::cos(1.57); + double sin = std::sin(1.57); + + // | cos -sin 0.0 1.0 | + // | sin cos 0.0 2.0 | + // | 0.0 0.0 1.0 0.0 | + EXPECT_DOUBLE_EQ(result(0, 0), cos); + EXPECT_DOUBLE_EQ(result(0, 1), -sin); + EXPECT_DOUBLE_EQ(result(0, 2), 0.0); + EXPECT_DOUBLE_EQ(result(0, 3), 1.0); + EXPECT_DOUBLE_EQ(result(1, 0), sin); + EXPECT_DOUBLE_EQ(result(1, 1), cos); + EXPECT_DOUBLE_EQ(result(1, 2), 0.0); + EXPECT_DOUBLE_EQ(result(1, 3), 2.0); + EXPECT_DOUBLE_EQ(result(2, 0), 0.0); + EXPECT_DOUBLE_EQ(result(2, 1), 0.0); + EXPECT_DOUBLE_EQ(result(2, 2), 1.0); + EXPECT_DOUBLE_EQ(result(2, 3), 0.0); +} + +TEST_F(ConverterTest, Get2DTwist) { + Eigen::Vector3d result = Get2DTwist(trajectory_.multi_dof_joint_trajectory.points[1].velocities[0]); + EXPECT_DOUBLE_EQ(result[0], 0.2); + EXPECT_DOUBLE_EQ(result[1], 0.0); + EXPECT_DOUBLE_EQ(result[2], 0.0); +} + +TEST_F(ConverterTest, GetTwist) { + Eigen::Vector3d twist2d(1.0, 2.0, 1.57); + tmc_manipulation_types::Twist result = GetTwist(twist2d); + EXPECT_DOUBLE_EQ(result[0], 1.0); + EXPECT_DOUBLE_EQ(result[1], 2.0); + EXPECT_DOUBLE_EQ(result[2], 0.0); + EXPECT_DOUBLE_EQ(result[3], 0.0); + EXPECT_DOUBLE_EQ(result[4], 0.0); + EXPECT_DOUBLE_EQ(result[5], 1.57); +} + +TEST_F(ConverterTest, TransformConstraint) { + // setup + tmc_planning_msgs::msg::Constraints msg; + msg.soft_link_constraints.push_back(link_const_); + msg.soft_joint_constraints.push_back(rjc_); + msg.hard_link_constraints.push_back(link_const_); + msg.hard_joint_constraints.push_back(rjc_); + msg.hard_path_link_constraints.push_back(link_const_); + msg.soft_path_joint_constraints.push_back(rjc_); + std::string origin_frame = "odom"; + double tf_timeout = 1.0; + + auto node = rclcpp::Node::make_shared("test_node"); + tf2_ros::Buffer tf_buffer(node->get_clock()); + tf_buffer.setUsingDedicatedThread(true); + + geometry_msgs::msg::TransformStamped static_tf; + static_tf.header.frame_id = "map"; + static_tf.child_frame_id = origin_frame; + static_tf.transform.translation.x = 1.0; + static_tf.transform.translation.y = 2.0; + static_tf.transform.rotation.w = 1.0; + tf_buffer.setTransform(static_tf, "test", true); + + // Normal test + ASSERT_TRUE(TransformConstraints(origin_frame, tf_timeout, tf_buffer, msg)); + EXPECT_DOUBLE_EQ(msg.soft_link_constraints[0].tsr.origin_to_tsr.position.x, 1.0); + EXPECT_DOUBLE_EQ(msg.soft_link_constraints[0].tsr.origin_to_tsr.position.y, -1.0); + EXPECT_DOUBLE_EQ(msg.soft_link_constraints[0].tsr.origin_to_tsr.orientation.z, std::sin(1.5707945 / 2)); + EXPECT_DOUBLE_EQ(msg.soft_link_constraints[0].tsr.origin_to_tsr.orientation.w, std::cos(1.5707945 / 2)); + EXPECT_DOUBLE_EQ(msg.soft_joint_constraints[0].min.multi_dof_joint_state.transforms[0].translation.x, 1.0); + EXPECT_DOUBLE_EQ(msg.soft_joint_constraints[0].min.multi_dof_joint_state.transforms[0].translation.y, -1.0); + // Other constraints check only the representative value + EXPECT_DOUBLE_EQ(msg.hard_link_constraints[0].tsr.origin_to_tsr.position.x, 1.0); + EXPECT_DOUBLE_EQ(msg.hard_joint_constraints[0].min.multi_dof_joint_state.transforms[0].translation.x, 1.0); + EXPECT_DOUBLE_EQ(msg.hard_path_link_constraints[0].tsr.origin_to_tsr.position.x, 1.0); + EXPECT_DOUBLE_EQ(msg.soft_path_joint_constraints[0].min.multi_dof_joint_state.transforms[0].translation.x, 1.0); + + // When coordinates cannot be converted + link_const_.header.frame_id = "test"; + msg.soft_link_constraints.push_back(link_const_); + ASSERT_FALSE(TransformConstraints(origin_frame, tf_timeout, tf_buffer, msg)); +} + +TEST_F(ConverterTest, ConvertConstraints) { + // setup + tmc_planning_msgs::msg::Constraints msg; + msg.soft_link_constraints.push_back(link_const_); + msg.soft_joint_constraints.push_back(rjc_); + msg.hard_link_constraints.push_back(link_const_); + msg.hard_joint_constraints.push_back(rjc_); + msg.goal_relative_linear_constraint = linear_const_; + msg.hard_path_link_constraints.push_back(link_const_); + msg.soft_path_joint_constraints.push_back(rjc_); + tmc_robot_local_planner::Constraints constraints; + // Normal test + ConvertConstraints(msg, constraints); + Eigen::Affine3d link_trans = constraints.soft_link_constraints[0]->Sample(); + Eigen::Vector3d rot = link_trans.rotation().eulerAngles(0, 1, 2); + EXPECT_DOUBLE_EQ(link_trans.translation().x(), 2.0); + EXPECT_DOUBLE_EQ(link_trans.translation().y(), 1.0); + EXPECT_NEAR(rot[2], 1.57, 0.001); + tmc_manipulation_types::RobotState joint_trans = constraints.soft_joint_constraints[0]->Sample(); + EXPECT_DOUBLE_EQ(joint_trans.multi_dof_joint_state.poses[0].translation().x(), 2.0); + EXPECT_DOUBLE_EQ(joint_trans.multi_dof_joint_state.poses[0].translation().y(), 1.0); + // Other constraints check only the representative value + EXPECT_DOUBLE_EQ(constraints.hard_link_constraints[0]->Sample().translation().x(), 2.0); + EXPECT_DOUBLE_EQ(constraints.hard_joint_constraints[0]->Sample().multi_dof_joint_state.poses[0].translation().x(), + 2.0); + EXPECT_DOUBLE_EQ(constraints.goal_relative_linear_constraint.distance, 0.4); + EXPECT_DOUBLE_EQ(constraints.hard_path_link_constraints[0]->Sample().translation().x(), 2.0); + EXPECT_DOUBLE_EQ( + constraints.soft_path_joint_constraints[0]->Sample().multi_dof_joint_state.poses[0].translation().x(), 2.0); +} + +TEST_F(ConverterTest, TransformTSRConstraint) { + // setup + std::string origin_frame = "odom"; + double tf_timeout = 1.0; + + auto node = rclcpp::Node::make_shared("test_node"); + tf2_ros::Buffer tf_buffer(node->get_clock()); + tf_buffer.setUsingDedicatedThread(true); + + geometry_msgs::msg::TransformStamped static_tf; + static_tf.header.frame_id = "map"; + static_tf.child_frame_id = "odom"; + static_tf.transform.translation.x = 1.0; + static_tf.transform.translation.y = 2.0; + static_tf.transform.rotation.w = 1.0; + tf_buffer.setTransform(static_tf, "test", true); + + // Normal test + ASSERT_TRUE(TransformTSRConstraint(origin_frame, tf_timeout, tf_buffer, link_const_)); + EXPECT_DOUBLE_EQ(link_const_.tsr.origin_to_tsr.position.x, 1.0); + EXPECT_DOUBLE_EQ(link_const_.tsr.origin_to_tsr.position.y, -1.0); + EXPECT_DOUBLE_EQ(link_const_.tsr.origin_to_tsr.orientation.z, std::sin(1.5707945 / 2)); + EXPECT_DOUBLE_EQ(link_const_.tsr.origin_to_tsr.orientation.w, std::cos(1.5707945 / 2)); + // When coordinates cannot be converted + const char* const test_frame = "test"; + ASSERT_FALSE(TransformTSRConstraint(test_frame, tf_timeout, tf_buffer, link_const_)); +} + +TEST_F(ConverterTest, ConvertLinkConstraintMsgToLinkConstraint) { + // setup + tmc_robot_local_planner::ILinkConstraint::Ptr link_const; + // Normal test + ConvertLinkConstraintMsgToLinkConstraint(link_const_, link_const); + Eigen::Affine3d trans = link_const->Sample(); + Eigen::Vector3d rot = trans.rotation().eulerAngles(0, 1, 2); + EXPECT_DOUBLE_EQ(trans.translation()[0], 2.0); + EXPECT_DOUBLE_EQ(trans.translation()[1], 1.0); + EXPECT_NEAR(rot[2], 1.57, 0.001); +} + +TEST_F(ConverterTest, ConvertLinkConstraintMsgToRotationFirstLinkConstraint) { + link_const_.tsr.min_bounds[0] = 1.0; + link_const_.tsr.max_bounds[0] = 1.0; + link_const_.tsr.min_bounds[5] = -M_PI / 2.0; + link_const_.tsr.max_bounds[5] = -M_PI / 2.0; + link_const_.tsr.rotation_first = true; + + tmc_robot_local_planner::ILinkConstraint::Ptr link_const; + ConvertLinkConstraintMsgToLinkConstraint(link_const_, link_const); + + Eigen::Affine3d trans = link_const->Sample(); + EXPECT_NEAR(trans.translation()[0], 3.0, 1.0e-5); + EXPECT_NEAR(trans.translation()[1], 1.0, 1.0e-5); + + Eigen::Vector3d rot = trans.rotation().eulerAngles(0, 1, 2); + EXPECT_NEAR(rot[2], 0.0, 1.0e-5); +} + +TEST_F(ConverterTest, TransformJointConstraint) { + // setup + std::string origin_frame = "odom"; + double tf_timeout = 1.0; + + auto node = rclcpp::Node::make_shared("test_node"); + tf2_ros::Buffer tf_buffer(node->get_clock()); + tf_buffer.setUsingDedicatedThread(true); + + geometry_msgs::msg::TransformStamped static_tf; + static_tf.header.frame_id = "map"; + static_tf.child_frame_id = "odom"; + static_tf.transform.translation.x = 1.0; + static_tf.transform.translation.y = 2.0; + static_tf.transform.rotation.w = 1.0; + tf_buffer.setTransform(static_tf, "test", true); + + // Normal test + ASSERT_TRUE(TransformJointConstraint(origin_frame, tf_timeout, tf_buffer, rjc_)); + EXPECT_DOUBLE_EQ(rjc_.min.multi_dof_joint_state.transforms[0].translation.x, 1.0); + EXPECT_DOUBLE_EQ(rjc_.min.multi_dof_joint_state.transforms[0].translation.y, -1.0); + // When coordinates cannot be converted + const char* const test_frame = "test"; + ASSERT_FALSE(TransformJointConstraint(test_frame, tf_timeout, tf_buffer, rjc_)); +} + +TEST_F(ConverterTest, ConvertJointConstraintMsgToJointConstraint) { + // setup + tmc_robot_local_planner::IJointConstraint::Ptr joint_const; + // Normal test + ConvertJointConstraintMsgToJointConstraint(rjc_, joint_const); + tmc_manipulation_types::RobotState state = joint_const->Sample(); + EXPECT_DOUBLE_EQ(state.multi_dof_joint_state.poses[0].translation()[0], 2.0); + EXPECT_DOUBLE_EQ(state.multi_dof_joint_state.poses[0].translation()[1], 1.0); +} + +TEST_F(ConverterTest, ConvertLinearConstraintMsgToLinearConstraint) { + // setup + tmc_robot_local_planner::LinearConstraint linear_const; + // Normal test + ConvertLinearConstraintMsgToLinearConstraint(linear_const_, linear_const); + EXPECT_EQ(linear_const.end_frame_id, "hand"); + EXPECT_DOUBLE_EQ(linear_const.axis[0], 0.1); + EXPECT_DOUBLE_EQ(linear_const.axis[1], 0.2); + EXPECT_DOUBLE_EQ(linear_const.axis[2], 0.3); + EXPECT_DOUBLE_EQ(linear_const.distance, 0.4); +} + +} // namespace tmc_robot_local_planner_utils + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/tmc_robot_local_planner_utils/test/extractor-test.cpp b/tmc_robot_local_planner_utils/test/extractor-test.cpp new file mode 100644 index 0000000..fd82070 --- /dev/null +++ b/tmc_robot_local_planner_utils/test/extractor-test.cpp @@ -0,0 +1,171 @@ +/* +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. +*/ +/// @brief Extractor test + +#include + +#include + +#include + +#include "test_base.hpp" + +namespace tmc_robot_local_planner_utils { + +class ExtractorTest : public TestBase { + protected: + virtual void SetUp(); + tmc_manipulation_types::JointState joint_state_; +}; + +void ExtractorTest::SetUp() { + TestBase::Initialize(); + + joint_state_.name = joint_names_; + joint_state_.position = init_vector_; + joint_state_.velocity = init_vector_; + joint_state_.effort = init_vector_; +} + +TEST_F(ExtractorTest, ExtractMultiDOFJointTrajectory) { + // setup + auto result_checker = [](int32_t i, uint32_t j) { + if (j == 0) { + return i * 0.1; + } else { + return 0.0; + } + }; + tmc_manipulation_types::TimedJointTrajectory result = ExtractMultiDOFJointTrajectory( + trajectory_.multi_dof_joint_trajectory, base_names_); + for (int32_t i = 0; i < base_names_.size(); ++i) { + EXPECT_EQ(result.joint_names[i], base_names_[i]); + } + for (int32_t i = 0; i < trajectory_.multi_dof_joint_trajectory.points.size(); ++i) { + for (int32_t j = 0; j < base_names_.size(); ++j) { + EXPECT_DOUBLE_EQ(result.points[i].positions[j], result_checker(i, j)); + EXPECT_DOUBLE_EQ(result.points[i].velocities[j], result_checker(i, j) * 2); + EXPECT_DOUBLE_EQ(result.points[i].accelerations[j], result_checker(i, j) * 3); + } + EXPECT_DOUBLE_EQ(result.points[i].time_from_start, (i + 1) * 1.0); + } +} + +TEST_F(ExtractorTest, ExtractMultiDOFJointTrajectoryWithoutPositions) { + for (auto& point : trajectory_.multi_dof_joint_trajectory.points) { + point.transforms.clear(); + } + auto result = ExtractMultiDOFJointTrajectory( + trajectory_.multi_dof_joint_trajectory, base_names_); + // I omit the detailed numbers because I do it in the above test + ASSERT_EQ(trajectory_.multi_dof_joint_trajectory.points.size(), + result.points.size()); + for (const auto& point : result.points) { + EXPECT_EQ(0, point.positions.size()); + EXPECT_EQ(3, point.velocities.size()); + EXPECT_EQ(3, point.accelerations.size()); + } +} + +TEST_F(ExtractorTest, ExtractMultiDOFJointTrajectoryWithoutVelocities) { + for (auto& point : trajectory_.multi_dof_joint_trajectory.points) { + point.velocities.clear(); + } + auto result = ExtractMultiDOFJointTrajectory( + trajectory_.multi_dof_joint_trajectory, base_names_); + + ASSERT_EQ(trajectory_.multi_dof_joint_trajectory.points.size(), + result.points.size()); + for (const auto& point : result.points) { + EXPECT_EQ(3, point.positions.size()); + EXPECT_EQ(0, point.velocities.size()); + EXPECT_EQ(3, point.accelerations.size()); + } +} + +TEST_F(ExtractorTest, ExtractMultiDOFJointTrajectoryWithoutAccelerations) { + for (auto& point : trajectory_.multi_dof_joint_trajectory.points) { + point.accelerations.clear(); + } + auto result = ExtractMultiDOFJointTrajectory( + trajectory_.multi_dof_joint_trajectory, base_names_); + + ASSERT_EQ(trajectory_.multi_dof_joint_trajectory.points.size(), + result.points.size()); + for (const auto& point : result.points) { + EXPECT_EQ(3, point.positions.size()); + EXPECT_EQ(3, point.velocities.size()); + EXPECT_EQ(0, point.accelerations.size()); + } +} + +TEST_F(ExtractorTest, ExtractTrajectoryAtTime) { + auto result_checker = [](int32_t i, uint32_t j) { + if (j == 0) { + return i * 0.1; + } else { + return 0.0; + } + }; + rclcpp::Time target_time(102, 500000000); + tmc_manipulation_types::TimedRobotTrajectory trajectory_out; + // Normal test + ASSERT_TRUE(ExtractTrajectoryAtTime(trajectory_, target_time, trajectory_out)); + int32_t diff_points = trajectory_.joint_trajectory.points.size() - trajectory_out.joint_trajectory.points.size(); + EXPECT_EQ(diff_points, 2); + for (int32_t i = 0; i < trajectory_out.joint_trajectory.points.size(); ++i) { + EXPECT_DOUBLE_EQ(trajectory_out.joint_trajectory.points[i].time_from_start, + trajectory_.joint_trajectory.points[i].time_from_start + (i + diff_points)); + EXPECT_DOUBLE_EQ(trajectory_out.multi_dof_joint_trajectory.points[i].time_from_start, + trajectory_.multi_dof_joint_trajectory.points[i].time_from_start + (i + diff_points)); + EXPECT_DOUBLE_EQ(trajectory_out.multi_dof_joint_trajectory.points[i].transforms[0].translation().x(), + 0.1 * (i + diff_points)); + for (int32_t j = 0; j < 6; ++j) { + EXPECT_DOUBLE_EQ(trajectory_out.multi_dof_joint_trajectory.points[i].velocities[0](j, 0), + result_checker(i + diff_points, j) * 2); + EXPECT_DOUBLE_EQ(trajectory_out.multi_dof_joint_trajectory.points[i].accelerations[0](j, 0), + result_checker(i + diff_points, j) * 3); + } + for (int32_t j = 0; j < joint_names_.size(); ++j) { + EXPECT_DOUBLE_EQ(trajectory_out.joint_trajectory.points[i].positions[j], + init_vector_[j] * (i + 1 + diff_points)); + EXPECT_DOUBLE_EQ(trajectory_out.joint_trajectory.points[i].velocities[j], + init_vector_[j] * (i + 2 + diff_points)); + EXPECT_DOUBLE_EQ(trajectory_out.joint_trajectory.points[i].accelerations[j], + init_vector_[j] * (i + 3 + diff_points)); + EXPECT_DOUBLE_EQ(trajectory_out.joint_trajectory.points[i].effort[j], + init_vector_[j] * (i + 4 + diff_points)); + } + } +} +} // namespace tmc_robot_local_planner_utils + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/tmc_robot_local_planner_utils/test/test_base.hpp b/tmc_robot_local_planner_utils/test/test_base.hpp new file mode 100644 index 0000000..c1b35af --- /dev/null +++ b/tmc_robot_local_planner_utils/test/test_base.hpp @@ -0,0 +1,76 @@ +/* +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. +*/ +/// @brief Test between tests +#ifndef TMC_ROBOT_LOCAL_PLANNER_UTILS_TESTBASE_HPP_ +#define TMC_ROBOT_LOCAL_PLANNER_UTILS_TESTBASE_HPP_ + +#include +#include +#include + +class TestBase : public ::testing::Test { + protected: + void Initialize(); + + tf2::Stamped trajectory_; + tmc_manipulation_types::NameSeq joint_names_{"arm", "wrist", "shoulder"}; + tmc_manipulation_types::NameSeq base_names_{"odom_x", "odom_y", "odom_t"}; + Eigen::VectorXd init_vector_ = Eigen::VectorXd::LinSpaced(joint_names_.size(), + 0.1, + 0.1 * joint_names_.size()); +}; + +void TestBase::Initialize() { + trajectory_.stamp_ = tf2::TimePoint(std::chrono::seconds(100)); + trajectory_.joint_trajectory.points.resize(joint_names_.size()); + trajectory_.multi_dof_joint_trajectory.points.resize(joint_names_.size()); + for (int32_t i = 0; i < 3; ++i) { + trajectory_.joint_trajectory.joint_names.push_back(joint_names_[i]); + trajectory_.joint_trajectory.points[i].time_from_start = (i + 1) * 1.0; + trajectory_.joint_trajectory.points[i].positions.resize(joint_names_.size()); + trajectory_.joint_trajectory.points[i].positions = init_vector_ * (i + 1); + trajectory_.joint_trajectory.points[i].velocities.resize(joint_names_.size()); + trajectory_.joint_trajectory.points[i].velocities = init_vector_ * (i + 2); + trajectory_.joint_trajectory.points[i].accelerations.resize(joint_names_.size()); + trajectory_.joint_trajectory.points[i].accelerations = init_vector_ * (i + 3); + trajectory_.joint_trajectory.points[i].effort.resize(joint_names_.size()); + trajectory_.joint_trajectory.points[i].effort = init_vector_ * (i + 4); + trajectory_.multi_dof_joint_trajectory.points[i].time_from_start = (i + 1) * 1.0; + trajectory_.multi_dof_joint_trajectory.points[i].transforms.resize(1); + trajectory_.multi_dof_joint_trajectory.points[i].transforms[0] = Eigen::Translation3d(0.1 * i, 0.0, 0.0); + trajectory_.multi_dof_joint_trajectory.points[i].velocities.resize(1); + trajectory_.multi_dof_joint_trajectory.points[i].velocities[0] << 0.2 * i, 0.0, 0.0, 0.0, 0.0, 0.0; + trajectory_.multi_dof_joint_trajectory.points[i].accelerations.resize(1); + trajectory_.multi_dof_joint_trajectory.points[i].accelerations[0] << 0.3 * i, 0.0, 0.0, 0.0, 0.0, 0.0; + } + for (int32_t i = 0; i < base_names_.size(); ++i) { + trajectory_.multi_dof_joint_trajectory.joint_names.push_back(base_names_[i]); + } +} +#endif // TMC_ROBOT_LOCAL_PLANNER_UTILS_TESTBASE_HPP_ + diff --git a/tmc_robot_local_planner_visualization/CHANGELOG.rst b/tmc_robot_local_planner_visualization/CHANGELOG.rst new file mode 100644 index 0000000..9e0f399 --- /dev/null +++ b/tmc_robot_local_planner_visualization/CHANGELOG.rst @@ -0,0 +1,9 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package tmc_robot_local_planner_visualization +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.0.0 (2024-10-15) +------------------- +* Initial release +* Contributors: Keisuke Takeshita + diff --git a/tmc_robot_local_planner_visualization/CMakeLists.txt b/tmc_robot_local_planner_visualization/CMakeLists.txt new file mode 100644 index 0000000..e966fc2 --- /dev/null +++ b/tmc_robot_local_planner_visualization/CMakeLists.txt @@ -0,0 +1,97 @@ +cmake_minimum_required(VERSION 3.5) +project(tmc_robot_local_planner_visualization) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(moveit_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(rviz_common REQUIRED) +find_package(rviz_default_plugins REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(tmc_eigen_utils REQUIRED) +find_package(tmc_manipulation_types_bridge REQUIRED) +find_package(tmc_planning_msgs REQUIRED) +find_package(tmc_robot_kinematics_model REQUIRED) +find_package(tmc_utils REQUIRED) +find_package(visualization_msgs REQUIRED) + +set(CMAKE_AUTOMOC ON) + +add_library(${PROJECT_NAME} SHARED src/generated_trajectories_display.cpp) +target_include_directories(${PROJECT_NAME} PUBLIC ${EIGEN3_INCLUDE_DIRS}) +ament_target_dependencies(${PROJECT_NAME} + moveit_msgs + pluginlib + rclcpp + rviz_common + rviz_default_plugins + tf2_eigen + tmc_robot_kinematics_model + tmc_utils +) +pluginlib_export_plugin_description_file(rviz_common plugins_description.xml) + +add_library(link_constraints_visualization_lib SHARED src/link_constraints_visualization.cpp) +target_include_directories(link_constraints_visualization_lib PUBLIC ${EIGEN3_INCLUDE_DIRS}) +ament_target_dependencies(link_constraints_visualization_lib + rclcpp + rclcpp_components + tf2_eigen + tmc_eigen_utils + tmc_planning_msgs + tmc_utils + visualization_msgs +) +rclcpp_components_register_node(link_constraints_visualization_lib + PLUGIN "tmc_robot_local_planner_visualization::LinkConstraintsVisualization" + EXECUTABLE link_constraints_visualization +) + +add_library(planned_trajectory_visualization_lib SHARED src/planned_trajectory_visualization.cpp) +target_include_directories(planned_trajectory_visualization_lib PUBLIC ${EIGEN3_INCLUDE_DIRS}) +ament_target_dependencies(planned_trajectory_visualization_lib + moveit_msgs + rclcpp + rclcpp_components + tf2_eigen + tmc_manipulation_types_bridge + tmc_robot_kinematics_model + tmc_utils + visualization_msgs +) +rclcpp_components_register_node(planned_trajectory_visualization_lib + PLUGIN "tmc_robot_local_planner_visualization::PlannedTrajectoryVisualization" + EXECUTABLE planned_trajectory_visualization +) + +install(TARGETS ${PROJECT_NAME} link_constraints_visualization_lib planned_trajectory_visualization_lib + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) +install(PROGRAMS scripts/rosbag_player.py DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(tmc_manipulation_tests REQUIRED) + + ament_add_gtest(test_link_constraints_visualization test/test_link_constraints_visualization.cpp) + target_link_libraries(test_link_constraints_visualization link_constraints_visualization_lib) + + ament_add_gtest(test_planned_trajectory_visualization test/test_planned_trajectory_visualization.cpp) + target_link_libraries(test_planned_trajectory_visualization planned_trajectory_visualization_lib) + ament_target_dependencies(test_planned_trajectory_visualization tmc_manipulation_tests) +endif() + + +ament_package() diff --git a/tmc_robot_local_planner_visualization/launch/rosbag_display.launch.py b/tmc_robot_local_planner_visualization/launch/rosbag_display.launch.py new file mode 100644 index 0000000..fa8d40d --- /dev/null +++ b/tmc_robot_local_planner_visualization/launch/rosbag_display.launch.py @@ -0,0 +1,73 @@ +#!/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='hsre3p_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]) + link_constraints_visualizations = Node(package='tmc_robot_local_planner_visualization', + executable='link_constraints_visualization') + + rviz_config_file = PathJoinSubstitution( + [FindPackageShare('tmc_robot_local_planner_visualization'), 'launch', 'rosbag_display.rviz']) + rviz_node = Node(package='rviz2', + executable='rviz2', + parameters=[robot_description], + arguments=['-d', rviz_config_file]) + + nodes = [joint_state_publisher, robot_state_publisher, link_constraints_visualizations, rviz_node] + return LaunchDescription(declare_arguments() + nodes) diff --git a/tmc_robot_local_planner_visualization/launch/rosbag_display.rviz b/tmc_robot_local_planner_visualization/launch/rosbag_display.rviz new file mode 100644 index 0000000..9fa242f --- /dev/null +++ b/tmc_robot_local_planner_visualization/launch/rosbag_display.rviz @@ -0,0 +1,674 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 87 + Name: Displays + Property Tree Widget: + Expanded: + - /GeneratedTrajectoriesDisplay1/Topic1 + Splitter Ratio: 0.5 + Tree Height: 655 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_accurate_imu_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + base_b_bumper_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_f_bumper_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + base_imu_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + base_l_drive_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_l_passive_wheel_x_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + base_l_passive_wheel_y_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + base_l_passive_wheel_z_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_r_drive_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_r_passive_wheel_x_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + base_r_passive_wheel_y_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + base_r_passive_wheel_z_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_range_sensor_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_rear_range_sensor_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + d435_gazebo_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + d435_gazebo_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + d435_gazebo_left_ir_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + d435_gazebo_right_ir_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + d435_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + elbow_pitch_link: + Alpha: 1 + Show Axes: false + Show Trail: false + elbow_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_virtual_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + hand_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + head_l_stereo_camera_gazebo_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_l_stereo_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_r_stereo_camera_gazebo_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_r_stereo_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_rgbd_sensor_gazebo_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_rgbd_sensor_gazebo_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_rgbd_sensor_gazebo_left_ir_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_rgbd_sensor_gazebo_right_ir_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_rgbd_sensor_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rh_p12_rn_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rh_p12_rn_l1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rh_p12_rn_l2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rh_p12_rn_r1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rh_p12_rn_r2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulder_pitch_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulder_root_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulder_yaw_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tlibar_root_link: + Alpha: 1 + Show Axes: false + Show Trail: false + torso_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_pitch_link: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_yaw_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_yaw_translation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 1 + Name: TF + Show Arrows: false + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + cube_0: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /attached_object_publisher/marker_array + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + bottle_1: true + shelf: true + table: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /collision_environment_server/marker_array + Value: true + - Alpha: 0.30000001192092896 + Class: tmc_robot_local_planner_visualization/GeneratedTrajectoriesDisplay + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_accurate_imu_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + base_b_bumper_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_f_bumper_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + base_imu_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + base_l_drive_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_l_passive_wheel_x_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + base_l_passive_wheel_y_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + base_l_passive_wheel_z_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_r_drive_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_r_passive_wheel_x_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + base_r_passive_wheel_y_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + base_r_passive_wheel_z_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_range_sensor_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_rear_range_sensor_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + d435_gazebo_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + d435_gazebo_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + d435_gazebo_left_ir_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + d435_gazebo_right_ir_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + d435_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + elbow_pitch_link: + Alpha: 1 + Show Axes: false + Show Trail: false + elbow_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_virtual_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + hand_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + head_l_stereo_camera_gazebo_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_l_stereo_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_r_stereo_camera_gazebo_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_r_stereo_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_rgbd_sensor_gazebo_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_rgbd_sensor_gazebo_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_rgbd_sensor_gazebo_left_ir_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_rgbd_sensor_gazebo_right_ir_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_rgbd_sensor_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rh_p12_rn_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rh_p12_rn_l1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rh_p12_rn_l2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rh_p12_rn_r1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rh_p12_rn_r2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulder_pitch_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulder_root_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulder_yaw_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tlibar_root_link: + Alpha: 1 + Show Axes: false + Show Trail: false + torso_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_pitch_link: + Alpha: 1 + Show Axes: false + Show Trail: false + wrist_yaw_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_yaw_translation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: GeneratedTrajectoriesDisplay + Speed: 20 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: generated_trajectories + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + hard_link_constraints: true + hard_path_link_constraints: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /link_constraints_visualization/link_constraints_marker_array + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: odom + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 2.88112211227417 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.296915203332901 + Y: 0.4020375609397888 + Z: 0.5328660607337952 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.3253994286060333 + Target Frame: + Value: Orbit (rviz) + Yaw: 1.592233657836914 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 893 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000323fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000323000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000011000000323fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000323000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003e50000032300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1643 + X: 412 + Y: 139 diff --git a/tmc_robot_local_planner_visualization/package.xml b/tmc_robot_local_planner_visualization/package.xml new file mode 100644 index 0000000..e59de65 --- /dev/null +++ b/tmc_robot_local_planner_visualization/package.xml @@ -0,0 +1,34 @@ + + tmc_robot_local_planner_visualization + 2.0.0 + Visualization tools for robot local planner + HSR Support + BSD 3-clause Clear License + + Keisuke Takeshita + + ament_cmake + + eigen + + pluginlib + moveit_msgs + rclcpp + rclcpp_components + rviz_common + rviz_default_plugins + tf2_eigen + tmc_eigen_utils + tmc_planning_msgs + tmc_manipulation_types_bridge + tmc_robot_kinematics_model + tmc_utils + visualization_msgs + + ament_cmake_gtest + tmc_manipulation_tests + + + ament_cmake + + \ No newline at end of file diff --git a/tmc_robot_local_planner_visualization/plugins_description.xml b/tmc_robot_local_planner_visualization/plugins_description.xml new file mode 100644 index 0000000..9e2a9cd --- /dev/null +++ b/tmc_robot_local_planner_visualization/plugins_description.xml @@ -0,0 +1,7 @@ + + + Displays for trajectories by generator + + diff --git a/tmc_robot_local_planner_visualization/scripts/rosbag_player.py b/tmc_robot_local_planner_visualization/scripts/rosbag_player.py new file mode 100755 index 0000000..7063157 --- /dev/null +++ b/tmc_robot_local_planner_visualization/scripts/rosbag_player.py @@ -0,0 +1,365 @@ +#!/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. +''' +import argparse +from collections.abc import Callable +import fcntl +import math +import os +from pathlib import Path +import sys +import termios +import time +from typing import Any +from typing import TypeAlias + +from geometry_msgs.msg import ( + Pose, + Transform, + TransformStamped, +) +from moveit_msgs.msg import ( + DisplayTrajectory, + RobotTrajectory, +) +import rclpy +import rclpy.node +import rclpy.publisher + +from rosbags.rosbag2 import Reader +from rosbags.typesys import ( + get_types_from_msg, + get_typestore, + Stores, +) +import rosbags.typesys.store + +from sensor_msgs.msg import JointState +import tf2_ros +from tmc_planning_msgs.msg import ( + Constraints, + TsrLinkConstraint, +) +from trajectory_msgs.msg import ( + JointTrajectoryPoint, + MultiDOFJointTrajectoryPoint, +) +from visualization_msgs.msg import Marker, MarkerArray + + +def get_key(): + # https://qiita.com/pukin/items/3b791b8b759dd704f765 + fno = sys.stdin.fileno() + + attr_old = termios.tcgetattr(fno) + + attr = termios.tcgetattr(fno) + attr[3] = attr[3] & ~termios.ECHO & ~termios.ICANON + termios.tcsetattr(fno, termios.TCSADRAIN, attr) + + fcntl_old = fcntl.fcntl(fno, fcntl.F_GETFL) + fcntl.fcntl(fno, fcntl.F_SETFL, fcntl_old | os.O_NONBLOCK) + + chr = 0 + + try: + c = sys.stdin.read(1) + if len(c): + while len(c): + chr = (chr << 8) + ord(c) + c = sys.stdin.read(1) + finally: + fcntl.fcntl(fno, fcntl.F_SETFL, fcntl_old) + termios.tcsetattr(fno, termios.TCSANOW, attr_old) + + return chr + + +# Is this okay? +DisplayTrajectoryRosBag: TypeAlias = 'rosbags.usertypes.moveit_msgs__msg__DisplayTrajectory' +RobotLocalGoalRosBag: TypeAlias = 'rosbags.usertypes.tmc_planning_msgs__msg__RobotLocalGoal' +JointStateRosBag: TypeAlias = rosbags.typesys.stores.ros2_dashing.sensor_msgs__msg__JointState +JointTrajectoryControllerStateRosBag: TypeAlias = 'rosbags.usertypes.control_msgs__msg__JointTrajectoryControllerState' +MarkerArrayRosBag: TypeAlias = rosbags.typesys.stores.ros2_dashing.visualization_msgs__msg__MarkerArray +PoseRosBag: TypeAlias = rosbags.typesys.stores.ros2_dashing.geometry_msgs__msg__Pose +TsrLinkConstraintRosBag: TypeAlias = 'rosbags.usertypes.tmc_planning_msgs__msg__TsrLinkConstraint' + + +def publish_joint_state(pub: rclpy.publisher.Publisher, joint_state_rosbag: JointStateRosBag) -> None: + if joint_state_rosbag is None: + return + msg = JointState() + msg.name = joint_state_rosbag.name + msg.position = joint_state_rosbag.position.tolist() + pub.publish(msg) + + +def broadcast_base_state(broadcaster: tf2_ros.StaticTransformBroadcaster, + node: rclpy.node.Node, + controller_state: JointTrajectoryControllerStateRosBag) -> None: + if controller_state is None: + return + transform = TransformStamped() + transform.header.stamp = node.get_clock().now().to_msg() + transform.header.frame_id = 'odom' + transform.child_frame_id = 'base_link' + transform.transform.translation.x = controller_state.actual.positions[0] + transform.transform.translation.y = controller_state.actual.positions[1] + transform.transform.rotation.z = math.sin(controller_state.actual.positions[2] / 2.0) + transform.transform.rotation.w = math.cos(controller_state.actual.positions[2] / 2.0) + broadcaster.sendTransform(transform) + + +def convert_to_pose_msg(pose_rosbag: PoseRosBag) -> Pose: + msg = Pose() + msg.position.x = pose_rosbag.position.x + msg.position.y = pose_rosbag.position.y + msg.position.z = pose_rosbag.position.z + msg.orientation.x = pose_rosbag.orientation.x + msg.orientation.y = pose_rosbag.orientation.y + msg.orientation.z = pose_rosbag.orientation.z + msg.orientation.w = pose_rosbag.orientation.w + return msg + + +def publish_marker_array(pub: rclpy.publisher.Publisher, marker_array_rosbag: MarkerArrayRosBag) -> None: + if marker_array_rosbag is None: + return + msg = MarkerArray() + for marker_rosbag in marker_array_rosbag.markers: + marker_msg = Marker() + marker_msg.header.frame_id = marker_rosbag.header.frame_id + marker_msg.ns = marker_rosbag.ns + marker_msg.id = marker_rosbag.id + marker_msg.type = marker_rosbag.type + marker_msg.action = marker_rosbag.action + marker_msg.pose = convert_to_pose_msg(marker_rosbag.pose) + marker_msg.scale.x = marker_rosbag.scale.x + marker_msg.scale.y = marker_rosbag.scale.y + marker_msg.scale.z = marker_rosbag.scale.z + marker_msg.color.r = marker_rosbag.color.r + marker_msg.color.g = marker_rosbag.color.g + marker_msg.color.b = marker_rosbag.color.b + marker_msg.color.a = marker_rosbag.color.a + msg.markers.append(marker_msg) + pub.publish(msg) + + +def convert_to_tsr_link_constraint_msg(constraint_rosbag: TsrLinkConstraintRosBag) -> TsrLinkConstraint: + tsr_constraint = TsrLinkConstraint() + tsr_constraint.header.frame_id = constraint_rosbag.header.frame_id + tsr_constraint.tsr.end_frame_id = constraint_rosbag.tsr.end_frame_id + tsr_constraint.tsr.origin_to_tsr = convert_to_pose_msg(constraint_rosbag.tsr.origin_to_tsr) + tsr_constraint.tsr.tsr_to_end = convert_to_pose_msg(constraint_rosbag.tsr.tsr_to_end) + tsr_constraint.tsr.min_bounds = constraint_rosbag.tsr.min_bounds.tolist() + tsr_constraint.tsr.max_bounds = constraint_rosbag.tsr.max_bounds.tolist() + return tsr_constraint + + +def publish_constraints_from_goal(pub: rclpy.publisher.Publisher, goal_rosbag: RobotLocalGoalRosBag) -> None: + if goal_rosbag is None: + return + msg = Constraints() + msg.hard_link_constraints = [ + convert_to_tsr_link_constraint_msg(x) for x in goal_rosbag.constraints.hard_link_constraints] + msg.soft_link_constraints = [ + convert_to_tsr_link_constraint_msg(x) for x in goal_rosbag.constraints.soft_link_constraints] + msg.hard_path_link_constraints = [ + convert_to_tsr_link_constraint_msg(x) for x in goal_rosbag.constraints.hard_path_link_constraints] + pub.publish(msg) + + +def convert_generated_trajectories(trajectories_rosbag: DisplayTrajectoryRosBag) -> DisplayTrajectory: + msg = DisplayTrajectory() + for trajectory_rosbag in trajectories_rosbag.trajectory: + trajectory_msg = RobotTrajectory() + trajectory_msg.joint_trajectory.joint_names = trajectory_rosbag.joint_trajectory.joint_names + for point_rosbag in trajectory_rosbag.joint_trajectory.points: + point_msg = JointTrajectoryPoint() + point_msg.positions = point_rosbag.positions.tolist() + point_msg.time_from_start.sec = point_rosbag.time_from_start.sec + point_msg.time_from_start.nanosec = point_rosbag.time_from_start.nanosec + trajectory_msg.joint_trajectory.points.append(point_msg) + + trajectory_msg.multi_dof_joint_trajectory.joint_names =\ + trajectory_rosbag.multi_dof_joint_trajectory.joint_names + for point_rosbag in trajectory_rosbag.multi_dof_joint_trajectory.points: + point_msg = MultiDOFJointTrajectoryPoint() + transform = Transform() + transform.translation.x = point_rosbag.transforms[0].translation.x + transform.translation.y = point_rosbag.transforms[0].translation.y + transform.translation.z = point_rosbag.transforms[0].translation.z + transform.rotation.x = point_rosbag.transforms[0].rotation.x + transform.rotation.y = point_rosbag.transforms[0].rotation.y + transform.rotation.z = point_rosbag.transforms[0].rotation.z + transform.rotation.w = point_rosbag.transforms[0].rotation.w + point_msg.transforms.append(transform) + point_msg.time_from_start.sec = point_rosbag.time_from_start.sec + point_msg.time_from_start.nanosec = point_rosbag.time_from_start.nanosec + trajectory_msg.multi_dof_joint_trajectory.points.append(point_msg) + + msg.trajectory.append(trajectory_msg) + return msg + + +def publish_generated_trajectories(pub: rclpy.publisher.Publisher, trajectories: DisplayTrajectory) -> None: + if trajectories is None: + return + pub.publish(trajectories) + + +class TopicStorage: + + def __init__(self, reader: Reader, typestore: rosbags.typesys.store.Typestore, topic_name: str, + convert_func: Callable[[Any], Any] = None) -> None: + self._topics = [] + connections = [x for x in reader.connections if x.topic == topic_name] + for connection, timestamp, rawdata in reader.messages(connections=connections): + msg = typestore.deserialize_cdr(rawdata, connection.msgtype) + if convert_func is None: + self._topics.append((timestamp, msg)) + else: + self._topics.append((timestamp, convert_func(msg))) + if not self._topics: + raise + self._topics.sort() + self._value = None + + def extract(self, stamp_nsec: int, change_only: bool = False) -> Any: + if not self._topics: + if change_only: + return None + else: + return self._value + + new_value_found = False + while self._topics and self._topics[0][0] < stamp_nsec: + self._value = self._topics.pop(0)[1] + new_value_found = True + if change_only: + if new_value_found: + return self._value + else: + return None + else: + return self._value + + +def main(): + rclpy.init() + node = rclpy.create_node('rosbag_player') + + broadcaster = tf2_ros.StaticTransformBroadcaster(node) + joint_states_pub = node.create_publisher(JointState, 'debug_joint_state', 1) + environment_marker_pub = node.create_publisher(MarkerArray, 'collision_environment_server/marker_array', 1) + attached_marker_pub = node.create_publisher(MarkerArray, 'attached_object_publisher/marker_array', 1) + constraints_pub = node.create_publisher(Constraints, 'link_constraints_visualization/constraints', 1) + generated_trajectories_pub = node.create_publisher(DisplayTrajectory, 'generated_trajectories', 1) + + parser = argparse.ArgumentParser() + parser.add_argument('rosbag2_dir') + parser.add_argument('--start-offset', type=float, default=0.0) + parser.add_argument('--rate', type=float, default=1.0) + parser.add_argument('--tmc-planning-msgs-share-dir', + default=os.environ['HOME'] + '/hsre_ws/install/tmc_planning_msgs/share') + parser.add_argument('--publish-generated-trajectories', action='store_true') + parser.add_argument('--start-paused', action='store_true') + args = parser.parse_args() + + if args.rate < 0.0: + raise + + add_types = {} + for name in ['RobotLocalGoal', 'Constraints', 'TsrLinkConstraint', 'TaskSpaceRegion', 'RangeJointConstraint']: + add_types.update(get_types_from_msg( + Path(args.tmc_planning_msgs_share_dir + '/tmc_planning_msgs/msg/' + name + '.msg').read_text(), + 'tmc_planning_msgs/msg/' + name)) + for name in ['RobotState', 'AttachedCollisionObject', 'CollisionObject', 'DisplayTrajectory', 'RobotTrajectory']: + add_types.update(get_types_from_msg( + Path('/opt/ros/humble/share/moveit_msgs/msg/' + name + '.msg').read_text(), + 'moveit_msgs/msg/' + name)) + add_types.update(get_types_from_msg( + Path('/opt/ros/humble/share/control_msgs/msg/JointTrajectoryControllerState.msg').read_text(), + 'control_msgs/msg/JointTrajectoryControllerState')) + add_types.update(get_types_from_msg( + Path('/opt/ros/humble/share/object_recognition_msgs/msg/ObjectType.msg').read_text(), + 'object_recognition_msgs/msg/ObjectType')) + + typestore = get_typestore(Stores.ROS2_HUMBLE) + typestore.register(add_types) + + with Reader(args.rosbag2_dir) as reader: + start_stamp_nsec = reader.start_time + end_stamp_nsec = reader.end_time + + joint_states_topics = TopicStorage(reader, typestore, '/joint_states') + base_states_topics = TopicStorage(reader, typestore, '/omni_base_controller/state') + environment_markers_topics = TopicStorage(reader, typestore, '/collision_environment_server/marker_array') + attached_markers_topics = TopicStorage(reader, typestore, '/attached_object_publisher/marker_array') + robot_local_goals_topics = TopicStorage(reader, typestore, '/hsrb_robot_local_planner/constraints') + if args.publish_generated_trajectories: + generated_trajectories_topics = TopicStorage( + reader, typestore, '/hsrb_robot_local_planner/generated_trajectories', convert_generated_trajectories) + + # I won't do my best to manage the time + time_from_start_nsec = int(args.start_offset * 1.0e9) + is_running = not args.start_paused + node.get_logger().info("Loaded rosbag") + if not is_running: + node.get_logger().info('Start with r.') + while start_stamp_nsec + time_from_start_nsec < end_stamp_nsec: + time.sleep(0.02) + key = get_key() + if key == 115: # s + is_running = False + node.get_logger().info('Stopped. Restart with r.') + elif key == 114: # r + is_running = True + node.get_logger().info('Restarted.') + elif key == 113: # q + return + + if is_running: + time_from_start_nsec += int(2e7 * args.rate) + + current_stamp = start_stamp_nsec + time_from_start_nsec + publish_joint_state(joint_states_pub, joint_states_topics.extract(current_stamp)) + broadcast_base_state(broadcaster, node, base_states_topics.extract(current_stamp)) + publish_marker_array(environment_marker_pub, environment_markers_topics.extract(current_stamp)) + publish_marker_array(attached_marker_pub, attached_markers_topics.extract(current_stamp)) + publish_constraints_from_goal(constraints_pub, robot_local_goals_topics.extract(current_stamp, True)) + if args.publish_generated_trajectories: + publish_generated_trajectories(generated_trajectories_pub, + generated_trajectories_topics.extract(current_stamp, True)) + if key == 103 and not is_running: # g + publish_generated_trajectories(generated_trajectories_pub, + generated_trajectories_topics.extract(current_stamp)) + + +if __name__ == '__main__': + main() diff --git a/tmc_robot_local_planner_visualization/src/generated_trajectories_display.cpp b/tmc_robot_local_planner_visualization/src/generated_trajectories_display.cpp new file mode 100644 index 0000000..1b990b8 --- /dev/null +++ b/tmc_robot_local_planner_visualization/src/generated_trajectories_display.cpp @@ -0,0 +1,256 @@ +/* +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. +*/ +#include "generated_trajectories_display.hpp" + +#include +#include +#include + +#include + +#include + +namespace tmc_robot_local_planner_visualization { + +RobotTrajectoryLinkUpdater::RobotTrajectoryLinkUpdater(const std::string& robot_description) { + model_ = std::make_shared(robot_description); +} + +bool RobotTrajectoryLinkUpdater::getLinkTransforms( + const std::string& link_name, + Ogre::Vector3& visual_position, + Ogre::Quaternion& visual_orientation, + Ogre::Vector3& collision_position, + Ogre::Quaternion& collision_orientation) const { + try { + // TODO(Takeshita) Transform + const auto pose = model_->GetObjectTransform(link_name); + + visual_position.x = pose.translation().x(); + visual_position.y = pose.translation().y(); + visual_position.z = pose.translation().z(); + collision_position = visual_position; + + const auto quaternion = Eigen::Quaterniond(pose.linear()); + visual_orientation.w = quaternion.w(); + visual_orientation.x = quaternion.x(); + visual_orientation.y = quaternion.y(); + visual_orientation.z = quaternion.z(); + collision_orientation = visual_orientation; + + return true; + } catch (const std::exception& ex) { + // TODO(Takeshita) Logger + std::cerr << ex.what() << std::endl; + return false; + } +} + +bool RobotTrajectoryLinkUpdater::update(const rclcpp::Duration& time_from_start) { + if (trajectory_.joint_trajectory.points.empty()) { + return false; + } + + auto target_index = 0u; + for (; target_index < trajectory_.multi_dof_joint_trajectory.points.size(); ++target_index) { + const auto current_time_from_start = + rclcpp::Duration(trajectory_.multi_dof_joint_trajectory.points[target_index].time_from_start); + if (current_time_from_start > time_from_start) { + const Eigen::Affine3d robot_pose = tf2::transformToEigen( + trajectory_.multi_dof_joint_trajectory.points[target_index].transforms[0]); + model_->SetRobotTransform(robot_pose); + + tmc_manipulation_types::JointState joint_state; + joint_state.name = trajectory_.joint_trajectory.joint_names; + joint_state.position = Eigen::Map( + &(trajectory_.joint_trajectory.points[target_index].positions[0]), + trajectory_.joint_trajectory.points[target_index].positions.size()); + model_->SetNamedAngle(joint_state); + + return true; + } + } + return false; +} + +void RobotTrajectoryLinkUpdater::resetTrajectory() { + resetTrajectory(moveit_msgs::msg::RobotTrajectory()); +} + +void RobotTrajectoryLinkUpdater::resetTrajectory(const moveit_msgs::msg::RobotTrajectory& msg) { + trajectory_ = msg; +} + + +GeneratedTrajectoriesDisplay::GeneratedTrajectoriesDisplay() + : process_messsage_stamp_(), max_trajetory_duration_(0, 0), do_update_(true) { + alpha_property_ = new rviz_common::properties::FloatProperty( + "Alpha", 0.3, "Amount of transparency to apply to the links.", this, SLOT(updateAlpha())); + alpha_property_->setMin(0.0); + alpha_property_->setMax(1.0); + + speed_property_ = new rviz_common::properties::FloatProperty( + "Speed", 10.0, "Rate of playback speed", this, SLOT(updateSpeed())); + speed_property_->setMin(1.0); + + update_immidiately_property_ = new rviz_common::properties::BoolProperty( + "Update immidiately", false, "Update immidiately", this, SLOT(updateUpdateImmidiately())); +} + +GeneratedTrajectoriesDisplay::~GeneratedTrajectoriesDisplay() { +} + +void GeneratedTrajectoriesDisplay::onInitialize() { + RTDClass::onInitialize(); + + // TODO(Takeshita) Parameter + constexpr uint32_t kModelNum = 10; + for (auto i = 0u; i < kModelNum; ++i) { + robots_.emplace_back(std::make_unique( + scene_node_, context_, "Robot: " + getName().toStdString() + " " + std::to_string(i), this)); + } + + // TODO(Takeshita) Use robot_description topic + auto rviz_ros_node_ = context_->getRosNodeAbstraction().lock(); + const auto robot_description = tmc_utils::GetParameter( + rviz_ros_node_->get_raw_node(), "robot_description", ""); + + urdf::Model urdf_model; + if (!urdf_model.initString(robot_description)) { + setStatus(rviz_common::properties::StatusProperty::Error, "URDF", "URDF failed Model parse"); + return; + } + setStatus(rviz_common::properties::StatusProperty::Ok, "URDF", "URDF parsed OK"); + for (const auto& robot : robots_) { + robot->load(urdf_model); + link_updaters_.emplace_back(RobotTrajectoryLinkUpdater(robot_description)); + robot->setVisible(false); + } + + clock_ = rviz_ros_node_->get_raw_node()->get_clock(); + process_messsage_stamp_ = clock_->now(); + + updateAlpha(); +} + +void GeneratedTrajectoriesDisplay::update(float wall_dt, float ros_dt) { + std::lock_guard lock(mutex_); + + const auto time_from_reset = (clock_->now() - process_messsage_stamp_) * speed_property_->getFloat(); + bool do_queue_render = false; + + if (time_from_reset > max_trajetory_duration_) { + for (auto& robot : robots_) { + if (robot->isVisible()) { + do_queue_render = true; + robot->setVisible(false); + } + } + do_update_ = true; + } else { + for (auto i = 0u; i < robots_.size(); ++i) { + if (link_updaters_[i].update(time_from_reset)) { + do_queue_render = true; + robots_[i]->setVisible(true); + robots_[i]->update(link_updaters_[i]); + } else { + if (robots_[i]->isVisible()) { + do_queue_render = true; + robots_[i]->setVisible(false); + } + } + } + } + + if (do_queue_render) { + context_->queueRender(); + } +} + +void GeneratedTrajectoriesDisplay::reset() { + RTDClass::reset(); +} + +void GeneratedTrajectoriesDisplay::processMessage(moveit_msgs::msg::DisplayTrajectory::ConstSharedPtr msg) { + std::lock_guard lock(mutex_); + + if (!update_immidiately_property_->getBool()) { + if (!do_update_) { + return; + } + do_update_ = false; + } + + max_trajetory_duration_ = rclcpp::Duration(0, 0); + if (msg->trajectory.size() <= robots_.size()) { + for (auto i = 0u; i < robots_.size(); ++i) { + if (i < msg->trajectory.size()) { + // TODO(Takeshita) Empty trajectory + link_updaters_[i].resetTrajectory(msg->trajectory[i]); + max_trajetory_duration_ = std::max( + max_trajetory_duration_, + rclcpp::Duration(msg->trajectory[i].joint_trajectory.points.back().time_from_start)); + } else { + link_updaters_[i].resetTrajectory(); + } + } + } else { + // TODO(Takeshita) Select rule + std::random_device seed_gen; + std::mt19937 mt(seed_gen()); + std::vector result; + std::sample(msg->trajectory.begin(), msg->trajectory.end(), std::back_inserter(result), robots_.size(), mt); + for (auto i = 0u; i < robots_.size(); ++i) { + link_updaters_[i].resetTrajectory(result[i]); + max_trajetory_duration_ = std::max( + max_trajetory_duration_, rclcpp::Duration(result[i].joint_trajectory.points.back().time_from_start)); + } + } + + process_messsage_stamp_ = clock_->now(); +} + +void GeneratedTrajectoriesDisplay::updateAlpha() { + for (const auto& robot : robots_) { + robot->setAlpha(alpha_property_->getFloat()); + + robot->setVisible(true); + robot->setVisualVisible(true); + robot->setCollisionVisible(false); + robot->setMassVisible(false); + robot->setInertiaVisible(false); + } + context_->queueRender(); +} + +} // namespace tmc_robot_local_planner_visualization + +#include // NOLINT + +PLUGINLIB_EXPORT_CLASS(tmc_robot_local_planner_visualization::GeneratedTrajectoriesDisplay, + rviz_common::Display) diff --git a/tmc_robot_local_planner_visualization/src/generated_trajectories_display.hpp b/tmc_robot_local_planner_visualization/src/generated_trajectories_display.hpp new file mode 100644 index 0000000..3b3d4c0 --- /dev/null +++ b/tmc_robot_local_planner_visualization/src/generated_trajectories_display.hpp @@ -0,0 +1,105 @@ +/* +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. +*/ +#ifndef TMC_ROBOT_LOCAL_PLANNER_VISUALIZATION_GENERATED_TRAJECTORY_VISUALIZATION_HPP_ +#define TMC_ROBOT_LOCAL_PLANNER_VISUALIZATION_GENERATED_TRAJECTORY_VISUALIZATION_HPP_ + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace tmc_robot_local_planner_visualization { + +// Function name matches the naming of RVIZ PLUGINS + +class RobotTrajectoryLinkUpdater : public rviz_default_plugins::robot::LinkUpdater { + public: + explicit RobotTrajectoryLinkUpdater(const std::string& robot_description); + virtual ~RobotTrajectoryLinkUpdater() = default; + + bool getLinkTransforms( + const std::string& link_name, + Ogre::Vector3& visual_position, + Ogre::Quaternion& visual_orientation, + Ogre::Vector3& collision_position, + Ogre::Quaternion& collision_orientation) const override; + + bool update(const rclcpp::Duration& time_from_start); + + void resetTrajectory(); + void resetTrajectory(const moveit_msgs::msg::RobotTrajectory& msg); + + private: + tmc_robot_kinematics_model::IRobotKinematicsModel::Ptr model_; + + moveit_msgs::msg::RobotTrajectory trajectory_; +}; + + +class GeneratedTrajectoriesDisplay : public rviz_common::RosTopicDisplay { + Q_OBJECT + + public: + GeneratedTrajectoriesDisplay(); + virtual ~GeneratedTrajectoriesDisplay(); + + void onInitialize() override; + void update(float wall_dt, float ros_dt) override; + void reset() override; + + private Q_SLOTS: // NOLINT + void updateAlpha(); + void updateSpeed() {} + void updateUpdateImmidiately() {} + + private: + void processMessage(moveit_msgs::msg::DisplayTrajectory::ConstSharedPtr msg) override; + + rviz_common::properties::FloatProperty* alpha_property_; + rviz_common::properties::FloatProperty* speed_property_; + rviz_common::properties::BoolProperty* update_immidiately_property_; + + std::vector> robots_; + std::vector link_updaters_; + + std::mutex mutex_; + + rclcpp::Clock::SharedPtr clock_; + rclcpp::Time process_messsage_stamp_; + rclcpp::Duration max_trajetory_duration_; + bool do_update_; +}; + +} // namespace tmc_robot_local_planner_visualization +#endif // TMC_ROBOT_LOCAL_PLANNER_VISUALIZATION_GENERATED_TRAJECTORY_VISUALIZATION_HPP_ diff --git a/tmc_robot_local_planner_visualization/src/link_constraints_visualization.cpp b/tmc_robot_local_planner_visualization/src/link_constraints_visualization.cpp new file mode 100644 index 0000000..ef8016c --- /dev/null +++ b/tmc_robot_local_planner_visualization/src/link_constraints_visualization.cpp @@ -0,0 +1,311 @@ +/* +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. +*/ +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "markers.hpp" + +namespace { +constexpr double kPositionBoundDistanceThreshold = 0.2; +constexpr double kPositionBoundMaxValueAbs = 1.0; + +constexpr double kRotationBoundDistanceThreshold = M_PI / 4.0; + +Eigen::Affine3d RegionValuesToPose(double x, double y, double z, double roll, double pitch, double yaw) { + return Eigen::Translation3d(x, y, z) * tmc_eigen_utils::RPYToQuaternion(Eigen::Vector3d(roll, pitch, yaw)); +} + +Eigen::Affine3d RotationFirstRegionValuesToPose(double x, double y, double z, double roll, double pitch, double yaw) { + const Eigen::Affine3d linear = Eigen::Translation3d::Identity() + * tmc_eigen_utils::RPYToQuaternion(Eigen::Vector3d(roll, pitch, yaw)); + const Eigen::Affine3d trans = Eigen::Translation3d(x, y, z) * Eigen::Quaterniond::Identity(); + return linear * trans; +} + +} // namespace + +namespace tmc_robot_local_planner_visualization { + +class TsrMarkerFactory { + public: + TsrMarkerFactory() {} + + std::vector GenerateMarkers( + const std::vector& constraints, + int32_t start_id, + const rclcpp::Time& stamp, + const MarkerParameters& params); + + protected: + virtual std::optional GenerateOriginMarker( + const Eigen::Affine3d& origin_to_end __attribute__((unused)), + const rclcpp::Time& stamp __attribute__((unused)), + const std::string& ref_frame_id __attribute__((unused)), + int32_t id __attribute__((unused)), + const MarkerParameters& params __attribute__((unused))) const { + return std::nullopt; + } +}; + +std::vector TsrMarkerFactory::GenerateMarkers( + const std::vector& constraints, + int32_t start_id, + const rclcpp::Time& stamp, + const MarkerParameters& params) { + // When converted to TMC_ROBOT_LOCAL_PLANNER type, min/max_bounds etc. is hidden in Private, so it is processed as MSG. + std::vector markers; + for (const auto& constraint : constraints) { + if (constraint.header.frame_id.empty()) { + continue; + } + Eigen::Affine3d origin_to_tsr; + tf2::fromMsg(constraint.tsr.origin_to_tsr, origin_to_tsr); + + Eigen::Affine3d tsr_to_end; + tf2::fromMsg(constraint.tsr.tsr_to_end, tsr_to_end); + + std::vector> candidates_xyz; + for (auto i = 0; i < 3; ++i) { + const auto distance = std::abs(constraint.tsr.max_bounds[i] - constraint.tsr.min_bounds[i]); + if (distance < std::numeric_limits::min()) { + candidates_xyz.emplace_back(std::vector({constraint.tsr.min_bounds[i]})); + } else if (distance < kPositionBoundDistanceThreshold) { + candidates_xyz.emplace_back(std::vector({constraint.tsr.min_bounds[i], constraint.tsr.max_bounds[i]})); + } else { + const auto min_value = std::max(-kPositionBoundMaxValueAbs, constraint.tsr.min_bounds[i]); + const auto max_value = std::min(kPositionBoundMaxValueAbs, constraint.tsr.max_bounds[i]); + candidates_xyz.emplace_back(std::vector({min_value, (min_value + max_value) / 2.0, max_value})); + } + } + + std::vector> candidates_rpy; + for (auto i = 3; i < 6; ++i) { + const auto distance = std::abs(constraint.tsr.max_bounds[i] - constraint.tsr.min_bounds[i]); + if (distance < std::numeric_limits::min()) { + candidates_rpy.emplace_back(std::vector({constraint.tsr.min_bounds[i]})); + } else if (distance < kRotationBoundDistanceThreshold) { + candidates_rpy.emplace_back(std::vector({constraint.tsr.min_bounds[i], constraint.tsr.max_bounds[i]})); + } else { + const auto min_value = std::max(-M_PI, constraint.tsr.min_bounds[i]); + const auto max_value = std::min(M_PI, constraint.tsr.max_bounds[i]); + // I just need to be divided into a lot, so I don't think about rounding. + const auto step = (max_value - min_value) / (std::round(distance / kRotationBoundDistanceThreshold) + 1.0); + std::vector candidates; + for (auto value = min_value; value < max_value; value += step) { + candidates.push_back(value); + } + if (std::abs(candidates.back() - max_value) > (step / 2.0)) { + candidates.push_back(max_value); + } + candidates_rpy.push_back(candidates); + } + } + + for (auto x : candidates_xyz[0]) { + for (auto y : candidates_xyz[1]) { + for (auto z : candidates_xyz[2]) { + for (auto roll : candidates_rpy[0]) { + for (auto pitch : candidates_rpy[1]) { + for (auto yaw : candidates_rpy[2]) { + Eigen::Affine3d origin_to_end; + if (constraint.tsr.rotation_first) { + origin_to_end = + origin_to_tsr * RotationFirstRegionValuesToPose(x, y, z, roll, pitch, yaw) * tsr_to_end; + } else { + origin_to_end = origin_to_tsr * RegionValuesToPose(x, y, z, roll, pitch, yaw) * tsr_to_end; + } + markers.emplace_back(GeneratePoseMarkers( + origin_to_end, stamp, constraint.header.frame_id, start_id + markers.size(), params)); + const auto origin_marker = GenerateOriginMarker( + origin_to_end, stamp, constraint.header.frame_id, start_id + markers.size(), params); + if (origin_marker) { + markers.push_back(origin_marker.value()); + } + } + } + } + } + } + } + } + return markers; +} + + +class GoalTsrMarkerFactory : public TsrMarkerFactory { + public: + GoalTsrMarkerFactory() {} + + protected: + std::optional GenerateOriginMarker( + const Eigen::Affine3d& origin_to_end, + const rclcpp::Time& stamp, + const std::string& ref_frame_id, + int32_t id, + const MarkerParameters& params) const override; +}; + +std::optional GoalTsrMarkerFactory::GenerateOriginMarker( + const Eigen::Affine3d& origin_to_end, + const rclcpp::Time& stamp, + const std::string& ref_frame_id, + int32_t id, + const MarkerParameters& params) const { + visualization_msgs::msg::Marker cube; + cube.header.stamp = stamp; + cube.header.frame_id = ref_frame_id; + cube.ns = params.ns; + cube.id = id; + cube.type = visualization_msgs::msg::Marker::CUBE; + cube.action = visualization_msgs::msg::Marker::ADD; + cube.pose = tf2::toMsg(origin_to_end); + cube.scale.x = params.line_width * 2.0; + cube.scale.y = params.line_width * 2.0; + cube.scale.z = params.line_width * 2.0; + cube.lifetime.sec = params.life_time; + cube.color.r = 1.0; + cube.color.g = 1.0; + cube.color.b = 1.0; + cube.color.a = params.alpha; + return cube; +} + + +class LinkConstraintsVisualization : public rclcpp::Node { + public: + LinkConstraintsVisualization() : LinkConstraintsVisualization(rclcpp::NodeOptions()) {} + explicit LinkConstraintsVisualization(const rclcpp::NodeOptions& options); + + private: + rclcpp::Publisher::SharedPtr marker_array_pub_; + + rclcpp::Subscription::SharedPtr constraints_sub_; + void ConstraintsCallback(const tmc_planning_msgs::msg::Constraints::SharedPtr msg); + + rclcpp::Subscription::SharedPtr robot_local_goal_sub_; + void RobotLocalGoalCallback(const tmc_planning_msgs::msg::RobotLocalGoal::SharedPtr msg); + + void Visualize(const tmc_planning_msgs::msg::Constraints& constraints_msg); + + GoalTsrMarkerFactory goal_tsr_marker_factory_; + TsrMarkerFactory path_tsr_marker_factory_; + + tmc_utils::DynamicParameter::Ptr line_length_hard_; + tmc_utils::DynamicParameter::Ptr line_width_hard_; + tmc_utils::DynamicParameter::Ptr alpha_hard_; + tmc_utils::DynamicParameter::Ptr line_length_soft_; + tmc_utils::DynamicParameter::Ptr line_width_soft_; + tmc_utils::DynamicParameter::Ptr alpha_soft_; + tmc_utils::DynamicParameter::Ptr life_time_; +}; + +LinkConstraintsVisualization::LinkConstraintsVisualization(const rclcpp::NodeOptions& options) + : Node("link_constraints_visualization", options) { + line_length_hard_ = std::make_shared>(this, "marker.hard.line_length", 0.2); + line_width_hard_ = std::make_shared>(this, "marker.hard.line_width", 0.02); + alpha_hard_ = std::make_shared>(this, "marker.hard.alpha", 1.0); + line_length_soft_ = std::make_shared>(this, "marker.soft.line_length", 0.2); + line_width_soft_ = std::make_shared>(this, "marker.soft.line_width", 0.04); + alpha_soft_ = std::make_shared>(this, "marker.soft.alpha", 0.5); + life_time_ = std::make_shared>(this, "marker.life_time", 0); + + marker_array_pub_ = this->create_publisher( + "~/link_constraints_marker_array", tmc_utils::BestEffortQoS(2)); + + constraints_sub_ = this->create_subscription( + "~/constraints", tmc_utils::BestEffortQoS(), + std::bind(&LinkConstraintsVisualization::ConstraintsCallback, this, std::placeholders::_1)); + + robot_local_goal_sub_ = this->create_subscription( + "~/robot_local_goal", tmc_utils::BestEffortQoS(), + std::bind(&LinkConstraintsVisualization::RobotLocalGoalCallback, this, std::placeholders::_1)); +} + +void LinkConstraintsVisualization::ConstraintsCallback(const tmc_planning_msgs::msg::Constraints::SharedPtr msg) { + Visualize(*msg); +} + +void LinkConstraintsVisualization::RobotLocalGoalCallback(const tmc_planning_msgs::msg::RobotLocalGoal::SharedPtr msg) { + Visualize(msg->constraints); +} + +void LinkConstraintsVisualization::Visualize(const tmc_planning_msgs::msg::Constraints& constraints_msg) { + { + visualization_msgs::msg::MarkerArray markers; + markers.markers.resize(1); + markers.markers[0].action = visualization_msgs::msg::Marker::DELETEALL; + marker_array_pub_->publish(markers); + } + + const auto stamp = this->now(); + visualization_msgs::msg::MarkerArray markers; + + MarkerParameters hard_goal_params; + hard_goal_params.ns = "hard_link_constraints"; + hard_goal_params.line_length = line_length_hard_->value(); + hard_goal_params.line_width = line_width_hard_->value(); + hard_goal_params.alpha = alpha_hard_->value(); + hard_goal_params.life_time = life_time_->value(); + const auto hard_goal_markers = goal_tsr_marker_factory_.GenerateMarkers( + constraints_msg.hard_link_constraints, markers.markers.size(), stamp, hard_goal_params); + markers.markers.insert(markers.markers.end(), hard_goal_markers.begin(), hard_goal_markers.end()); + + MarkerParameters soft_goal_params; + soft_goal_params.ns = "soft_link_constraints"; + soft_goal_params.line_length = line_length_soft_->value(); + soft_goal_params.line_width = line_width_soft_->value(); + soft_goal_params.alpha = alpha_soft_->value(); + soft_goal_params.life_time = life_time_->value(); + const auto soft_goal_markers = goal_tsr_marker_factory_.GenerateMarkers( + constraints_msg.soft_link_constraints, markers.markers.size(), stamp, soft_goal_params); + markers.markers.insert(markers.markers.end(), soft_goal_markers.begin(), soft_goal_markers.end()); + + MarkerParameters hard_path_params; + hard_path_params.ns = "hard_path_link_constraints"; + hard_path_params.line_length = line_length_hard_->value(); + hard_path_params.line_width = line_width_hard_->value(); + hard_path_params.alpha = alpha_hard_->value(); + hard_path_params.life_time = life_time_->value(); + const auto hard_path_markers = path_tsr_marker_factory_.GenerateMarkers( + constraints_msg.hard_path_link_constraints, markers.markers.size(), stamp, hard_path_params); + markers.markers.insert(markers.markers.end(), hard_path_markers.begin(), hard_path_markers.end()); + + marker_array_pub_->publish(markers); +} + +} // namespace tmc_robot_local_planner_visualization + +#include // NOLINT +RCLCPP_COMPONENTS_REGISTER_NODE(tmc_robot_local_planner_visualization::LinkConstraintsVisualization) diff --git a/tmc_robot_local_planner_visualization/src/markers.hpp b/tmc_robot_local_planner_visualization/src/markers.hpp new file mode 100644 index 0000000..aa36b82 --- /dev/null +++ b/tmc_robot_local_planner_visualization/src/markers.hpp @@ -0,0 +1,83 @@ +/* +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. +*/ + +#include + +#include + +#include +#include + +#include + +namespace tmc_robot_local_planner_visualization { + +struct MarkerParameters { + std::string ns; + double line_length; + double line_width; + double alpha; + int32_t life_time; +}; + +visualization_msgs::msg::Marker GeneratePoseMarkers(const Eigen::Affine3d& origin_to_end, + const rclcpp::Time& stamp, + const std::string& ref_frame_id, + int32_t id, + const MarkerParameters& params) { + visualization_msgs::msg::Marker line_list; + line_list.header.stamp = stamp; + line_list.header.frame_id = ref_frame_id; + line_list.ns = params.ns; + line_list.id = id; + line_list.type = visualization_msgs::msg::Marker::LINE_LIST; + line_list.action = visualization_msgs::msg::Marker::ADD; + line_list.pose = tf2::toMsg(origin_to_end); + line_list.scale.x = params.line_width; + line_list.lifetime.sec = params.life_time; + line_list.points.resize(6); + line_list.points[1].x = params.line_length; + line_list.points[3].y = params.line_length; + line_list.points[5].z = params.line_length; + line_list.colors.resize(6); + line_list.colors[0].a = params.alpha; + line_list.colors[0].r = 1.0; + line_list.colors[1].a = params.alpha; + line_list.colors[1].r = 1.0; + line_list.colors[2].a = params.alpha; + line_list.colors[2].g = 1.0; + line_list.colors[3].a = params.alpha; + line_list.colors[3].g = 1.0; + line_list.colors[4].a = params.alpha; + line_list.colors[4].b = 1.0; + line_list.colors[5].a = params.alpha; + line_list.colors[5].b = 1.0; + return line_list; +} + +} // namespace tmc_robot_local_planner_visualization diff --git a/tmc_robot_local_planner_visualization/src/planned_trajectory_visualization.cpp b/tmc_robot_local_planner_visualization/src/planned_trajectory_visualization.cpp new file mode 100644 index 0000000..a014550 --- /dev/null +++ b/tmc_robot_local_planner_visualization/src/planned_trajectory_visualization.cpp @@ -0,0 +1,119 @@ +/* +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. +*/ +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "markers.hpp" + +namespace tmc_robot_local_planner_visualization { + +class PlannedTrajectoryVisualization : public rclcpp::Node { + public: + PlannedTrajectoryVisualization() : PlannedTrajectoryVisualization(rclcpp::NodeOptions()) {} + explicit PlannedTrajectoryVisualization(const rclcpp::NodeOptions& options); + + private: + rclcpp::Publisher::SharedPtr marker_array_pub_; + rclcpp::Publisher::SharedPtr display_trajectory_pub_; + + rclcpp::Subscription::SharedPtr trajectory_sub_; + void TrajectoryCallback(const moveit_msgs::msg::RobotTrajectory::SharedPtr msg); + + std::string origin_frame_id_; + std::string end_effector_frame_id_; + + double line_length_; + double line_width_; + double alpha_; + + tmc_robot_kinematics_model::IRobotKinematicsModel::Ptr model_; +}; + +PlannedTrajectoryVisualization::PlannedTrajectoryVisualization(const rclcpp::NodeOptions& options) + : Node("planned_trajectory_visualization", options) { + origin_frame_id_ = tmc_utils::GetParameter(this, "origin_frame_id", "odom"); + end_effector_frame_id_ = tmc_utils::GetParameter(this, "end_effector_frame_id", "hand_palm_link"); + + line_length_ = tmc_utils::GetParameter(this, "line_length", 0.2); + line_width_ = tmc_utils::GetParameter(this, "line_width", 0.05); + alpha_ = tmc_utils::GetParameter(this, "alpha", 0.5); + + auto robot_description = tmc_utils::GetParameter(this, "robot_description_kinematics", ""); + if (robot_description.empty()) { + robot_description = tmc_utils::GetParameter(this, "robot_description", ""); + } + model_ = std::make_shared(robot_description); + + marker_array_pub_ = this->create_publisher( + "~/end_effector_marker", tmc_utils::BestEffortQoS()); + display_trajectory_pub_ = this->create_publisher( + "~/planned_trajectory_display", tmc_utils::BestEffortQoS()); + + trajectory_sub_ = this->create_subscription( + "~/planned_trajectory", tmc_utils::BestEffortQoS(), + std::bind(&PlannedTrajectoryVisualization::TrajectoryCallback, this, std::placeholders::_1)); +} + +void PlannedTrajectoryVisualization::TrajectoryCallback(const moveit_msgs::msg::RobotTrajectory::SharedPtr msg) { + if (msg->joint_trajectory.points.empty() && msg->multi_dof_joint_trajectory.points.empty()) { + return; + } + + // Visualization of orbit + moveit_msgs::msg::DisplayTrajectory display_trajectory; + display_trajectory.trajectory.push_back(*msg); + display_trajectory_pub_->publish(display_trajectory); + + // Visualization of terminal posture + tmc_manipulation_types::TimedRobotTrajectory robot_trajectory; + tmc_manipulation_types_bridge::RobotTrajectoryMsgToTimedRobotTrajectory(*msg, robot_trajectory); + + tmc_manipulation_types::JointState joint_state; + joint_state.name = robot_trajectory.joint_trajectory.joint_names; + joint_state.position = robot_trajectory.joint_trajectory.points.back().positions; + model_->SetNamedAngle(joint_state); + model_->SetRobotTransform(robot_trajectory.multi_dof_joint_trajectory.points.back().transforms[0]); + + visualization_msgs::msg::MarkerArray markers; + markers.markers.push_back(GeneratePoseMarkers( + model_->GetObjectTransform(end_effector_frame_id_), this->now(), origin_frame_id_, 0, + {end_effector_frame_id_, line_length_, line_width_, alpha_, 0})); + marker_array_pub_->publish(markers); +} + +} // namespace tmc_robot_local_planner_visualization + +#include // NOLINT +RCLCPP_COMPONENTS_REGISTER_NODE(tmc_robot_local_planner_visualization::PlannedTrajectoryVisualization) diff --git a/tmc_robot_local_planner_visualization/test/test_link_constraints_visualization.cpp b/tmc_robot_local_planner_visualization/test/test_link_constraints_visualization.cpp new file mode 100644 index 0000000..977fff4 --- /dev/null +++ b/tmc_robot_local_planner_visualization/test/test_link_constraints_visualization.cpp @@ -0,0 +1,445 @@ +/* +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. +*/ +#include + +#include + +#include +#include + +#include "../src/link_constraints_visualization.cpp" // NOLINT + +namespace { +constexpr double kEpsilon = 1.0e-6; + +std::vector ExtractMarkers(const visualization_msgs::msg::MarkerArray& marker_array, + const std::string& target_frame_id) { + std::vector markers; + for (const auto& marker : marker_array.markers) { + if (marker.header.frame_id == target_frame_id) { + markers.push_back(marker); + } + } + return markers; +} + +void ValidateDeleteAll(const visualization_msgs::msg::MarkerArray& marker_array) { + EXPECT_EQ(marker_array.markers.size(), 1); + EXPECT_EQ(marker_array.markers[0].action, visualization_msgs::msg::Marker::DELETEALL); +} + +void ValidateMarkerArrayId(const visualization_msgs::msg::MarkerArray& marker_array, + const std::string& target_ns) { + std::vector ids; + for (const auto& marker : marker_array.markers) { + if (marker.ns == target_ns) { + ids.push_back(marker.id); + } + } + std::sort(ids.begin(), ids.end()); + EXPECT_EQ(ids.size(), std::distance(ids.begin(), std::unique(ids.begin(), ids.end()))); +} + +void ValidatePose(const geometry_msgs::msg::Pose& pose_msg, const Eigen::Affine3d& pose_eigen) { + EXPECT_DOUBLE_EQ(pose_msg.position.x, pose_eigen.translation().x()); + EXPECT_DOUBLE_EQ(pose_msg.position.y, pose_eigen.translation().y()); + EXPECT_DOUBLE_EQ(pose_msg.position.z, pose_eigen.translation().z()); + EXPECT_DOUBLE_EQ(pose_msg.orientation.x, Eigen::Quaterniond(pose_eigen.linear()).x()); + EXPECT_DOUBLE_EQ(pose_msg.orientation.y, Eigen::Quaterniond(pose_eigen.linear()).y()); + EXPECT_DOUBLE_EQ(pose_msg.orientation.z, Eigen::Quaterniond(pose_eigen.linear()).z()); + EXPECT_DOUBLE_EQ(pose_msg.orientation.w, Eigen::Quaterniond(pose_eigen.linear()).w()); +} + +void ValidatePoint(const geometry_msgs::msg::Point& msg, + double x, double y, double z) { + EXPECT_DOUBLE_EQ(msg.x, x); + EXPECT_DOUBLE_EQ(msg.y, y); + EXPECT_DOUBLE_EQ(msg.z, z); +} + +void ValidateColor(const std_msgs::msg::ColorRGBA& msg, + double r, double g, double b, double a) { + EXPECT_DOUBLE_EQ(msg.r, r); + EXPECT_DOUBLE_EQ(msg.g, g); + EXPECT_DOUBLE_EQ(msg.b, b); + EXPECT_DOUBLE_EQ(msg.a, a); +} + +void ValidateAxisMarker(const visualization_msgs::msg::Marker& marker, + const std::string& ns, + double line_length, + double line_width, + double alpha, + int32_t lifetime) { + EXPECT_EQ(marker.ns, ns); + EXPECT_EQ(marker.type, visualization_msgs::msg::Marker::LINE_LIST); + EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::ADD); + EXPECT_DOUBLE_EQ(marker.scale.x, line_width); + EXPECT_DOUBLE_EQ(marker.lifetime.sec, lifetime); + // Really, the Points corresponding to R is x 0.0 and LINE_LENGTH, and it is ideal to test like that, but I will not do my best. + EXPECT_EQ(marker.points.size(), 6); + ValidatePoint(marker.points[0], 0.0, 0.0, 0.0); + ValidatePoint(marker.points[1], line_length, 0.0, 0.0); + ValidatePoint(marker.points[2], 0.0, 0.0, 0.0); + ValidatePoint(marker.points[3], 0.0, line_length, 0.0); + ValidatePoint(marker.points[4], 0.0, 0.0, 0.0); + ValidatePoint(marker.points[5], 0.0, 0.0, line_length); + EXPECT_EQ(marker.colors.size(), 6); + ValidateColor(marker.colors[0], 1.0, 0.0, 0.0, alpha); + ValidateColor(marker.colors[1], 1.0, 0.0, 0.0, alpha); + ValidateColor(marker.colors[2], 0.0, 1.0, 0.0, alpha); + ValidateColor(marker.colors[3], 0.0, 1.0, 0.0, alpha); + ValidateColor(marker.colors[4], 0.0, 0.0, 1.0, alpha); + ValidateColor(marker.colors[5], 0.0, 0.0, 1.0, alpha); +} + +void ValidateAxisMarker(const visualization_msgs::msg::Marker& marker, + const std::string& ns, + const Eigen::Affine3d& pose, + double line_length, + double line_width, + double alpha, + int32_t lifetime) { + ValidatePose(marker.pose, pose); + ValidateAxisMarker(marker, ns, line_length, line_width, alpha, lifetime); +} + +void ValidateCubeMarker(const visualization_msgs::msg::Marker& marker, + const std::string& ns, + const Eigen::Affine3d& pose, + double scale, + double alpha, + int32_t lifetime) { + EXPECT_EQ(marker.ns, ns); + EXPECT_EQ(marker.type, visualization_msgs::msg::Marker::CUBE); + EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::ADD); + ValidatePose(marker.pose, pose); + EXPECT_DOUBLE_EQ(marker.scale.x, scale); + EXPECT_DOUBLE_EQ(marker.scale.y, scale); + EXPECT_DOUBLE_EQ(marker.scale.z, scale); + EXPECT_DOUBLE_EQ(marker.color.r, 1.0); + EXPECT_DOUBLE_EQ(marker.color.g, 1.0); + EXPECT_DOUBLE_EQ(marker.color.b, 1.0); + EXPECT_DOUBLE_EQ(marker.color.a, alpha); + EXPECT_DOUBLE_EQ(marker.lifetime.sec, lifetime); +} + +void ValidateGoalMarker(const visualization_msgs::msg::Marker& marker_1, + const visualization_msgs::msg::Marker& marker_2, + const std::string& ns, + const Eigen::Affine3d& pose, + double line_length, + double line_width, + double alpha, + int32_t lifetime) { + if (marker_1.type == visualization_msgs::msg::Marker::LINE_LIST) { + ValidateAxisMarker(marker_1, ns, pose, line_length, line_width, alpha, lifetime); + ValidateCubeMarker(marker_2, ns, pose, 2.0 * line_width, alpha, lifetime); + } else { + ValidateCubeMarker(marker_1, ns, pose, 2.0 * line_width, alpha, lifetime); + ValidateAxisMarker(marker_2, ns, pose, line_length, line_width, alpha, lifetime); + } +} +} // namespace + +namespace tmc_robot_local_planner_visualization { + +class LinkConstraintsVisualizationTest : public ::testing::Test { + protected: + void SetUp() override; + + void SpinSome() { + rclcpp::spin_some(client_node_); + rclcpp::spin_some(server_node_); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + bool WaitForMarker() { + const auto end_time = std::chrono::system_clock::now() + std::chrono::duration(1.0); + while (msgs_.size() < 2) { + SpinSome(); + if (std::chrono::system_clock::now() > end_time) { + return false; + } + } + return true; + } + + std::shared_ptr server_node_; + + rclcpp::Node::SharedPtr client_node_; + rclcpp::Publisher::SharedPtr pub_; + rclcpp::Subscription::SharedPtr sub_; + + std::vector msgs_; + void Callback(const visualization_msgs::msg::MarkerArray::SharedPtr msg) { msgs_.push_back(*msg); } +}; + +void LinkConstraintsVisualizationTest::SetUp() { + server_node_ = std::make_shared(); + + client_node_ = rclcpp::Node::make_shared("client"); + pub_ = client_node_->create_publisher( + "link_constraints_visualization/constraints", 1); + sub_ = client_node_->create_subscription( + "link_constraints_visualization/link_constraints_marker_array", tmc_utils::BestEffortQoS(2), + std::bind(&LinkConstraintsVisualizationTest::Callback, this, std::placeholders::_1)); +} + +TEST_F(LinkConstraintsVisualizationTest, RotationFirst) { + // Here, I will do it with Hard/Soft_link_constraints + tmc_planning_msgs::msg::Constraints msg; + msg.hard_link_constraints.resize(1); + msg.hard_link_constraints[0].header.frame_id = "frame_1"; + msg.hard_link_constraints[0].tsr.origin_to_tsr.position.x = 0.1; + msg.hard_link_constraints[0].tsr.tsr_to_end.position.x = 0.2; + msg.hard_link_constraints[0].tsr.min_bounds[0] = 0.3; + msg.hard_link_constraints[0].tsr.max_bounds[0] = 0.3; + msg.hard_link_constraints[0].tsr.min_bounds[5] = 0.5; + msg.hard_link_constraints[0].tsr.max_bounds[5] = 0.5; + msg.soft_link_constraints.resize(1); + msg.soft_link_constraints[0] = msg.hard_link_constraints[0]; + msg.soft_link_constraints[0].header.frame_id = "frame_2"; + msg.soft_link_constraints[0].tsr.rotation_first = true; + + pub_->publish(msg); + ASSERT_TRUE(WaitForMarker()); + + ValidateDeleteAll(msgs_[0]); + ValidateMarkerArrayId(msgs_[1], "hard_link_constraints"); + ValidateMarkerArrayId(msgs_[1], "soft_link_constraints"); + + { + SCOPED_TRACE(""); + const auto markers = ExtractMarkers(msgs_[1], "frame_1"); + ASSERT_EQ(markers.size(), 2); + // Since Rotation_first is False, the position is first + const Eigen::Affine3d pose = Eigen::Translation3d(0.1 + 0.3, 0.0, 0.0) + * Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitZ()) + * Eigen::Translation3d(0.2, 0.0, 0.0); + ValidateGoalMarker(markers[0], markers[1], "hard_link_constraints", pose, 0.2, 0.02, 1.0, 0); + } + { + SCOPED_TRACE(""); + const auto markers = ExtractMarkers(msgs_[1], "frame_2"); + ASSERT_EQ(markers.size(), 2); + // Since Rotation_first is true, the position is later + const Eigen::Affine3d pose = Eigen::Translation3d(0.1, 0.0, 0.0) + * Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitZ()) + * Eigen::Translation3d(0.3 + 0.2, 0.0, 0.0); + ValidateGoalMarker(markers[0], markers[1], "soft_link_constraints", pose, 0.2, 0.04, 0.5, 0); + } +} + +TEST_F(LinkConstraintsVisualizationTest, PositionRange) { + // Here, I will do it with hard_path_link_constraints + tmc_planning_msgs::msg::Constraints msg; + msg.hard_path_link_constraints.resize(2); + // The case with one point was omitted because it was performed by RotationFirst. + // A case with a narrow min/max bounds + msg.hard_path_link_constraints[0].header.frame_id = "frame_3"; + msg.hard_path_link_constraints[0].tsr.min_bounds[0] = 0.0; + msg.hard_path_link_constraints[0].tsr.max_bounds[0] = 0.1; + // Case with a wide range of min/max bounds + msg.hard_path_link_constraints[1].header.frame_id = "frame_4"; + msg.hard_path_link_constraints[1].tsr.min_bounds[1] = -1.0; + msg.hard_path_link_constraints[1].tsr.max_bounds[1] = 1.0; + + pub_->publish(msg); + ASSERT_TRUE(WaitForMarker()); + + ValidateDeleteAll(msgs_[0]); + ValidateMarkerArrayId(msgs_[1], "hard_path_link_constraints"); + + { + SCOPED_TRACE(""); + const auto markers = ExtractMarkers(msgs_[1], "frame_3"); + // If the min/max of the position is narrow, it will be two points, min/max. + ASSERT_EQ(markers.size(), 2); + std::vector positions; + for (const auto& marker : markers) { + ValidateAxisMarker(marker, "hard_path_link_constraints", 0.2, 0.02, 1.0, 0); + positions.push_back(marker.pose.position.x); + } + std::sort(positions.begin(), positions.end()); + EXPECT_DOUBLE_EQ(positions[0], 0.0); + EXPECT_DOUBLE_EQ(positions[1], 0.1); + } + { + SCOPED_TRACE(""); + const auto markers = ExtractMarkers(msgs_[1], "frame_4"); + // If the min/max of the position is wide, it will be three points in min/max/intermediate. + ASSERT_EQ(markers.size(), 3); + std::vector positions; + for (const auto& marker : markers) { + ValidateAxisMarker(marker, "hard_path_link_constraints", 0.2, 0.02, 1.0, 0); + positions.push_back(marker.pose.position.y); + } + std::sort(positions.begin(), positions.end()); + EXPECT_DOUBLE_EQ(positions[0], -1.0); + EXPECT_DOUBLE_EQ(positions[1], 0.0); + EXPECT_DOUBLE_EQ(positions[2], 1.0); + } +} + +TEST_F(LinkConstraintsVisualizationTest, RotationRange) { + tmc_planning_msgs::msg::Constraints msg; + msg.hard_path_link_constraints.resize(2); + // The case with one point was omitted because it was performed by RotationFirst. + // Case with narrow rotation min/max bounds + msg.hard_path_link_constraints[0].header.frame_id = "frame_5"; + msg.hard_path_link_constraints[0].tsr.min_bounds[3] = 0.0; + msg.hard_path_link_constraints[0].tsr.max_bounds[3] = 0.1; + // Case with a wide rotation min/max bounds + msg.hard_path_link_constraints[1].header.frame_id = "frame_6"; + msg.hard_path_link_constraints[1].tsr.min_bounds[4] = -1.0; + msg.hard_path_link_constraints[1].tsr.max_bounds[4] = 1.0; + + pub_->publish(msg); + ASSERT_TRUE(WaitForMarker()); + + ValidateDeleteAll(msgs_[0]); + ValidateMarkerArrayId(msgs_[1], "hard_path_link_constraints"); + + { + SCOPED_TRACE(""); + const auto markers = ExtractMarkers(msgs_[1], "frame_5"); + // If the rotation min/max is narrow, it will be two points, min/max. + ASSERT_EQ(markers.size(), 2); + std::vector quaternions; + for (const auto& marker : markers) { + ValidateAxisMarker(marker, "hard_path_link_constraints", 0.2, 0.02, 1.0, 0); + quaternions.push_back(2.0 * std::asin(marker.pose.orientation.x)); + } + std::sort(quaternions.begin(), quaternions.end()); + EXPECT_NEAR(quaternions[0], 0.0, kEpsilon); + EXPECT_NEAR(quaternions[1], 0.1, kEpsilon); + } + { + SCOPED_TRACE(""); + const auto markers = ExtractMarkers(msgs_[1], "frame_6"); + // If the rotation min/max is wide, it is placed at equal intervals between min/max. + ASSERT_GE(markers.size(), 3); + std::vector quaternions; + for (const auto& marker : markers) { + ValidateAxisMarker(marker, "hard_path_link_constraints", 0.2, 0.02, 1.0, 0); + quaternions.push_back(2.0 * std::asin(marker.pose.orientation.y)); + } + std::sort(quaternions.begin(), quaternions.end()); + EXPECT_NEAR(quaternions.front(), -1.0, kEpsilon); + EXPECT_NEAR(quaternions.back(), 1.0, kEpsilon); + + const double step = quaternions[1] - quaternions[0]; + for (auto i = 2u ; i < quaternions.size(); ++i) { + EXPECT_NEAR(quaternions[i] - quaternions[i - 1], step, kEpsilon); + } + } +} + +TEST_F(LinkConstraintsVisualizationTest, MultiRange) { + tmc_planning_msgs::msg::Constraints msg; + msg.hard_path_link_constraints.resize(1); + msg.hard_path_link_constraints[0].header.frame_id = "frame_7"; + msg.hard_path_link_constraints[0].tsr.min_bounds[0] = 0.0; + msg.hard_path_link_constraints[0].tsr.max_bounds[0] = 0.1; + msg.hard_path_link_constraints[0].tsr.min_bounds[1] = -1.0; + msg.hard_path_link_constraints[0].tsr.max_bounds[1] = 1.0; + msg.hard_path_link_constraints[0].tsr.min_bounds[3] = 0.2; + msg.hard_path_link_constraints[0].tsr.max_bounds[3] = 0.3; + + pub_->publish(msg); + ASSERT_TRUE(WaitForMarker()); + + ValidateDeleteAll(msgs_[0]); + ValidateMarkerArrayId(msgs_[1], "hard_path_link_constraints"); + + SCOPED_TRACE(""); + const auto markers = ExtractMarkers(msgs_[1], "frame_7"); + ASSERT_EQ(markers.size(), 2 * 3 * 2); + std::vector> values; + for (const auto& marker : markers) { + ValidateAxisMarker(marker, "hard_path_link_constraints", 0.2, 0.02, 1.0, 0); + values.push_back({marker.pose.position.x, marker.pose.position.y, 2.0 * std::asin(marker.pose.orientation.x)}); + } + for (auto i = 0u; i < values.size(); ++i) { + EXPECT_NEAR(std::abs(values[i][0] - 0.05), 0.05, kEpsilon); + EXPECT_TRUE((std::abs(values[i][1]) - 1.0) < kEpsilon || std::abs(values[i][1]) < kEpsilon); + EXPECT_NEAR(std::abs(values[i][2] - 0.25), 0.05, kEpsilon); + for (auto j = i + 1; j < values.size(); ++j) { + const auto diff_abs = std::abs(values[i][0] - values[j][0]) + + std::abs(values[i][1] - values[j][1]) + + std::abs(values[i][2] - values[j][2]); + EXPECT_GT(diff_abs, 0.01); + } + } +} + +TEST_F(LinkConstraintsVisualizationTest, EmptyConstraints) { + pub_->publish(tmc_planning_msgs::msg::Constraints()); + ASSERT_TRUE(WaitForMarker()); + + ValidateDeleteAll(msgs_[0]); + EXPECT_EQ(msgs_[1].markers.size(), 0); +} + +TEST_F(LinkConstraintsVisualizationTest, EmptyFrameId) { + tmc_planning_msgs::msg::Constraints msg; + msg.hard_path_link_constraints.resize(2); + msg.hard_path_link_constraints[0].header.frame_id = ""; + msg.hard_path_link_constraints[1].header.frame_id = "frame_8"; + + pub_->publish(msg); + ASSERT_TRUE(WaitForMarker()); + + ValidateDeleteAll(msgs_[0]); + EXPECT_EQ(msgs_[1].markers.size(), 1); +} + +TEST_F(LinkConstraintsVisualizationTest, RobotLocalGoal) { + auto pub = client_node_->create_publisher( + "link_constraints_visualization/robot_local_goal", 1); + + tmc_planning_msgs::msg::RobotLocalGoal msg; + msg.constraints.hard_path_link_constraints.resize(1); + msg.constraints.hard_path_link_constraints[0].header.frame_id = "frame_9"; + + pub->publish(msg); + ASSERT_TRUE(WaitForMarker()); + + ValidateDeleteAll(msgs_[0]); + EXPECT_EQ(msgs_[1].markers.size(), 1); +} + +} // namespace tmc_robot_local_planner_visualization + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/tmc_robot_local_planner_visualization/test/test_planned_trajectory_visualization.cpp b/tmc_robot_local_planner_visualization/test/test_planned_trajectory_visualization.cpp new file mode 100644 index 0000000..0eb84e6 --- /dev/null +++ b/tmc_robot_local_planner_visualization/test/test_planned_trajectory_visualization.cpp @@ -0,0 +1,154 @@ +/* +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. +*/ +#include + +#include + +#include +#include + +#include "../src/planned_trajectory_visualization.cpp" // NOLINT + +namespace { +constexpr double kEpsilon = 1.0e-6; +} // namespace + +namespace tmc_robot_local_planner_visualization { + +class PlannedTrajectoryVisualizationTest : public ::testing::Test { + protected: + void SetUp() override; + + void SpinSome() { + rclcpp::spin_some(client_node_); + rclcpp::spin_some(server_node_); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + bool WaitForTopics() { + const auto end_time = std::chrono::system_clock::now() + std::chrono::duration(0.5); + while (!marker_array_msg_ || !display_msg_) { + SpinSome(); + if (std::chrono::system_clock::now() > end_time) { + return false; + } + } + return true; + } + + std::shared_ptr server_node_; + + rclcpp::Node::SharedPtr client_node_; + rclcpp::Publisher::SharedPtr trajectory_pub_; + rclcpp::Subscription::SharedPtr marker_array_sub_; + rclcpp::Subscription::SharedPtr display_sub_; + + std::optional marker_array_msg_; + void MarkerArrayCallback(const visualization_msgs::msg::MarkerArray::SharedPtr msg) { marker_array_msg_ = *msg; } + + std::optional display_msg_; + void DisplayCallback(const moveit_msgs::msg::DisplayTrajectory::SharedPtr msg) { display_msg_ = *msg; } +}; + +void PlannedTrajectoryVisualizationTest::SetUp() { + rclcpp::NodeOptions options; + options.parameter_overrides() = {rclcpp::Parameter("robot_description", + tmc_manipulation_tests::stanford_manipulator::GetUrdf()), + rclcpp::Parameter("end_effector_frame_id", "link7")}; + server_node_ = std::make_shared(options); + + client_node_ = rclcpp::Node::make_shared("client"); + trajectory_pub_ = client_node_->create_publisher( + "planned_trajectory_visualization/planned_trajectory", 1); + + marker_array_msg_ = std::nullopt; + marker_array_sub_ = client_node_->create_subscription( + "planned_trajectory_visualization/end_effector_marker", tmc_utils::BestEffortQoS(), + std::bind(&PlannedTrajectoryVisualizationTest::MarkerArrayCallback, this, std::placeholders::_1)); + + display_msg_ = std::nullopt; + display_sub_ = client_node_->create_subscription( + "planned_trajectory_visualization/planned_trajectory_display", tmc_utils::BestEffortQoS(), + std::bind(&PlannedTrajectoryVisualizationTest::DisplayCallback, this, std::placeholders::_1)); +} + +TEST_F(PlannedTrajectoryVisualizationTest, PublishTopics) { + moveit_msgs::msg::RobotTrajectory msg; + msg.joint_trajectory.joint_names = {"joint1", "joint2", "joint3", "joint4", "joint5", "joint6"}; + msg.joint_trajectory.points.resize(1); + msg.joint_trajectory.points[0].positions = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + msg.multi_dof_joint_trajectory.joint_names = {"world_joint"}; + msg.multi_dof_joint_trajectory.points.resize(1); + msg.multi_dof_joint_trajectory.points[0].transforms.resize(1); + msg.multi_dof_joint_trajectory.points[0].transforms[0].translation.x = 0.1; + trajectory_pub_->publish(msg); + + ASSERT_TRUE(WaitForTopics()); + + // Do not test the fine shape of the marker + const auto marker_array_msg = marker_array_msg_.value(); + ASSERT_EQ(marker_array_msg.markers.size(), 1); + EXPECT_EQ(marker_array_msg.markers[0].header.frame_id, "odom"); + EXPECT_EQ(marker_array_msg.markers[0].ns, "link7"); + // Value in zero posture + value for bogie movement + EXPECT_NEAR(marker_array_msg.markers[0].pose.position.x, 0.1, kEpsilon); + EXPECT_NEAR(marker_array_msg.markers[0].pose.position.y, -0.25, kEpsilon); + EXPECT_NEAR(marker_array_msg.markers[0].pose.position.z, 0.95, kEpsilon); + EXPECT_NEAR(marker_array_msg.markers[0].pose.orientation.x, 0.0, kEpsilon); + EXPECT_NEAR(marker_array_msg.markers[0].pose.orientation.y, 0.0, kEpsilon); + EXPECT_NEAR(marker_array_msg.markers[0].pose.orientation.z, 0.0, kEpsilon); + EXPECT_NEAR(marker_array_msg.markers[0].pose.orientation.w, 1.0, kEpsilon); + + const auto display_msg = display_msg_.value(); + ASSERT_EQ(display_msg.trajectory.size(), 1); + EXPECT_EQ(display_msg.trajectory[0].joint_trajectory.joint_names, msg.joint_trajectory.joint_names); + EXPECT_EQ(display_msg.trajectory[0].joint_trajectory.points.size(), 1); + EXPECT_EQ(display_msg.trajectory[0].joint_trajectory.points[0].positions, msg.joint_trajectory.points[0].positions); + EXPECT_EQ(display_msg.trajectory[0].multi_dof_joint_trajectory.joint_names, + msg.multi_dof_joint_trajectory.joint_names); + EXPECT_EQ(display_msg.trajectory[0].multi_dof_joint_trajectory.points.size(), 1); + EXPECT_EQ(display_msg.trajectory[0].multi_dof_joint_trajectory.points[0].transforms.size(), 1); + EXPECT_EQ(display_msg.trajectory[0].multi_dof_joint_trajectory.points[0].transforms[0].translation.x, + msg.multi_dof_joint_trajectory.points[0].transforms[0].translation.x); +} + +TEST_F(PlannedTrajectoryVisualizationTest, PublishTopicsWithEmptyTrajectory) { + moveit_msgs::msg::RobotTrajectory msg; + trajectory_pub_->publish(msg); + + EXPECT_FALSE(WaitForTopics()); +} + +} // namespace tmc_robot_local_planner_visualization + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/tmc_simple_path_generator/CHANGELOG.rst b/tmc_simple_path_generator/CHANGELOG.rst new file mode 100644 index 0000000..7b12863 --- /dev/null +++ b/tmc_simple_path_generator/CHANGELOG.rst @@ -0,0 +1,9 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package tmc_simple_path_generator +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.0.0 (2024-10-15) +------------------- +* Initial release +* Contributors: Keisuke Takeshita, Satoru Onoda + diff --git a/tmc_simple_path_generator/CMakeLists.txt b/tmc_simple_path_generator/CMakeLists.txt new file mode 100644 index 0000000..ad29d95 --- /dev/null +++ b/tmc_simple_path_generator/CMakeLists.txt @@ -0,0 +1,104 @@ +cmake_minimum_required(VERSION 3.5) +project(tmc_simple_path_generator) + +find_package(ament_cmake REQUIRED) +find_package(angles REQUIRED) +find_package(Boost REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(tmc_manipulation_types REQUIRED) +find_package(tmc_planning_msgs REQUIRED) +find_package(tmc_robot_kinematics_model REQUIRED) +find_package(tmc_robot_local_planner REQUIRED) +find_package(tmc_robot_local_planner_utils REQUIRED) +find_package(tmc_utils REQUIRED) +find_package(urdf REQUIRED) + +add_library(${PROJECT_NAME} SHARED + src/${PROJECT_NAME}/calculator.cpp + src/${PROJECT_NAME}/common.cpp + src/${PROJECT_NAME}/hard_path_constraints.cpp + src/${PROJECT_NAME}/planar_trajectory_interpolator.cpp + src/${PROJECT_NAME}/sample_goal_generator.cpp + src/${PROJECT_NAME}/sample_middle_generator.cpp + src/${PROJECT_NAME}/sampling_parameters.cpp + src/${PROJECT_NAME}/simple_path_generator.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC include ${EIGEN3_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) +ament_target_dependencies(${PROJECT_NAME} + angles + pluginlib + rclcpp + rclcpp_action + tmc_manipulation_types + tmc_planning_msgs + tmc_robot_kinematics_model + tmc_robot_local_planner + tmc_robot_local_planner_utils + tmc_utils + urdf +) + +add_executable(simple_path_generator src/${PROJECT_NAME}/node_main.cpp) +target_link_libraries(simple_path_generator ${PROJECT_NAME}) +ament_target_dependencies(simple_path_generator rclcpp) + +pluginlib_export_plugin_description_file(tmc_robot_local_planner generator_plugins.xml) +pluginlib_export_plugin_description_file(tmc_simple_path_generator interpolator_plugins.xml) +pluginlib_export_plugin_description_file(tmc_simple_path_generator goal_sampler_plugins.xml) +pluginlib_export_plugin_description_file(tmc_simple_path_generator middle_sampler_plugins.xml) + +install(TARGETS ${PROJECT_NAME} simple_path_generator + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) +install(DIRECTORY include/ + DESTINATION include +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(tmc_manipulation_tests REQUIRED) + + ament_add_gtest(goal_relative_linear_constraint_test test/goal_relative_linear_constraint-test.cpp) + target_link_libraries(goal_relative_linear_constraint_test ${PROJECT_NAME}) + ament_target_dependencies(goal_relative_linear_constraint_test tmc_manipulation_tests) + + ament_add_gtest(hard_path_link_constraints_test test/hard_path_link_constraints-test.cpp) + target_link_libraries(hard_path_link_constraints_test ${PROJECT_NAME}) + ament_target_dependencies(hard_path_link_constraints_test tmc_manipulation_tests) + + ament_add_gtest(planar_trajectory_interpolator_test test/planar_trajectory_interpolator-test.cpp) + target_link_libraries(planar_trajectory_interpolator_test ${PROJECT_NAME}) + + ament_add_gtest(sample_middle_generator_test test/sample_middle_generator-test.cpp) + target_link_libraries(sample_middle_generator_test ${PROJECT_NAME}) + ament_target_dependencies(sample_middle_generator_test tmc_manipulation_tests) + + ament_add_gtest(sample_goal_generator_test test/sample_goal_generator-test.cpp) + target_link_libraries(sample_goal_generator_test ${PROJECT_NAME}) + ament_target_dependencies(sample_goal_generator_test tmc_manipulation_tests) + + ament_add_gtest(simple_path_generator_test test/simple_path_generator-test.cpp) + target_link_libraries(simple_path_generator_test ${PROJECT_NAME}) + ament_target_dependencies(simple_path_generator_test tmc_manipulation_tests) +endif() + +ament_export_include_directories() +ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies( + angles + rclcpp + rclcpp_action + tmc_manipulation_types + tmc_planning_msgs + tmc_robot_kinematics_model + tmc_robot_local_planner + tmc_robot_local_planner_utils + urdf +) + +ament_package() diff --git a/tmc_simple_path_generator/generator_plugins.xml b/tmc_simple_path_generator/generator_plugins.xml new file mode 100644 index 0000000..5b52264 --- /dev/null +++ b/tmc_simple_path_generator/generator_plugins.xml @@ -0,0 +1,7 @@ + + + SinmplePathGenerator + + \ No newline at end of file diff --git a/tmc_simple_path_generator/goal_sampler_plugins.xml b/tmc_simple_path_generator/goal_sampler_plugins.xml new file mode 100644 index 0000000..f769324 --- /dev/null +++ b/tmc_simple_path_generator/goal_sampler_plugins.xml @@ -0,0 +1,12 @@ + + + Single-thread goal sampler + + + Multi-thread goal sampler + + \ No newline at end of file diff --git a/tmc_simple_path_generator/include/tmc_simple_path_generator/hard_path_constraints.hpp b/tmc_simple_path_generator/include/tmc_simple_path_generator/hard_path_constraints.hpp new file mode 100644 index 0000000..df4daa7 --- /dev/null +++ b/tmc_simple_path_generator/include/tmc_simple_path_generator/hard_path_constraints.hpp @@ -0,0 +1,104 @@ +/* +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. +*/ +#ifndef TMC_SIMPLE_PATH_GENERATOR_HARD_PATH_CONSTRAINTS_HPP_ +#define TMC_SIMPLE_PATH_GENERATOR_HARD_PATH_CONSTRAINTS_HPP_ + +#include +#include +#include + +#include + +#include + +namespace tmc_simple_path_generator { + +class HardPathLinkConstraints { + public: + using Ptr = std::shared_ptr; + + explicit HardPathLinkConstraints(const rclcpp::Node::SharedPtr& node); + ~HardPathLinkConstraints() = default; + + // Apply any of Link_constraints restriction conditions to dst_trajectory + bool ConstrainTrajectory(const std::vector& link_constraints, + const SamplingParameters::Ptr& params, + tmc_manipulation_types::TimedRobotTrajectory& dst_trajectory); + + private: + // IK, use Single Thread implementation for the time being + SampleGoalGenerator::Ptr closest_sampler_; + + std::optional CalculateOriginToClosest( + const tmc_robot_local_planner::ILinkConstraint::Ptr& link_constraint, + const tmc_manipulation_types::RobotState& robot_state); + + std::optional ConstrainImpl( + const Eigen::Affine3d& origin_to_closest, + const tmc_robot_local_planner::ILinkConstraint::Ptr& link_constraint, + const tmc_manipulation_types::RobotState& robot_state, + const SamplingParameters::Ptr& params); + + // FK + pluginlib::ClassLoader fk_loader_; + tmc_robot_kinematics_model::IRobotKinematicsModel::Ptr robot_; + + // Random number generator + std::mt19937 engine_; +}; + + +class GoalRelativeLinearConstraint { + public: + using Ptr = std::shared_ptr; + + explicit GoalRelativeLinearConstraint(const rclcpp::Node::SharedPtr& node); + ~GoalRelativeLinearConstraint() = default; + + std::optional> SampleLinearPath( + const tmc_robot_local_planner::LinearConstraint linear_constraint, + const tmc_manipulation_types::RobotState& initial_state, + const tmc_manipulation_types::RobotState& goal_state, + const SamplingParameters::Ptr& params); + + private: + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; + + double linear_step_; + + // FK + pluginlib::ClassLoader fk_loader_; + tmc_robot_kinematics_model::IRobotKinematicsModel::Ptr robot_; + + // IK + SampleGoalGenerator::Ptr sampler_; +}; + +} // namespace tmc_simple_path_generator +#endif // TMC_SIMPLE_PATH_GENERATOR_HARD_PATH_CONSTRAINTS_HPP_ diff --git a/tmc_simple_path_generator/include/tmc_simple_path_generator/planar_trajectory_interpolator.hpp b/tmc_simple_path_generator/include/tmc_simple_path_generator/planar_trajectory_interpolator.hpp new file mode 100644 index 0000000..286d895 --- /dev/null +++ b/tmc_simple_path_generator/include/tmc_simple_path_generator/planar_trajectory_interpolator.hpp @@ -0,0 +1,172 @@ +/* +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. +*/ +/// @brief Planar Trajectory Interpolator Class +#ifndef TMC_SIMPLE_PATH_GENERATOR_PLANAR_TRAJECTORY_INTERPOLATOR_HPP_ +#define TMC_SIMPLE_PATH_GENERATOR_PLANAR_TRAJECTORY_INTERPOLATOR_HPP_ + +#include +#include +#include + +#include + +#include "sampling_parameters.hpp" +#include "trajectory_interpolator.hpp" + + +namespace tmc_simple_path_generator { + +class PlanarTrajectoryInterpolatorBase : public ITrajectoryInterpolator { + public: + PlanarTrajectoryInterpolatorBase() {} + virtual ~PlanarTrajectoryInterpolatorBase() = default; + + void Initialize(const rclcpp::Node::SharedPtr& node, const SamplingParameters::Ptr& params) override; + + // Orbit an interpolation that satisfies the restraint in the shortest time + bool Interpolate( + const tmc_manipulation_types::RobotState& initial_state, + const tmc_manipulation_types::RobotState& goal_state, + double normalized_vel, + tmc_manipulation_types::TimedRobotTrajectory& trajectory_out) override; + + // Calonia interpolation to configure via points + bool Interpolate( + const tmc_manipulation_types::RobotState& initial_state, + const tmc_manipulation_types::RobotState& goal_state, + double normalized_vel, + const tmc_manipulation_types::RobotState& random_middle_state, + tmc_manipulation_types::TimedRobotTrajectory& trajectory_out) override; + + // Orbit an interpolation that satisfies the restraint in the shortest time + bool Interpolate( + const tmc_manipulation_types::RobotState& initial_state, + const std::vector& middle_states_near_goal, + const tmc_manipulation_types::RobotState& goal_state, + double normalized_vel, + tmc_manipulation_types::TimedRobotTrajectory& trajectory_out) override; + + // Calonia interpolation to configure via points + bool Interpolate( + const tmc_manipulation_types::RobotState& initial_state, + const std::vector& middle_states_near_goal, + const tmc_manipulation_types::RobotState& goal_state, + double normalized_vel, + const tmc_manipulation_types::RobotState& random_middle_state, + tmc_manipulation_types::TimedRobotTrajectory& trajectory_out) override; + + protected: + // Orbit -generated parameters + SamplingParameters::Ptr sampling_params_; + + private: + virtual bool InterpolateTrajectory( + const tmc_manipulation_types::RobotState& initial_state, + const std::vector& middle_states_near_goal, + const tmc_manipulation_types::RobotState& goal_state, + double normalized_vel, + tmc_manipulation_types::TimedRobotTrajectory& trajectory_out) = 0; + + virtual bool InterpolateTrajectory( + const tmc_manipulation_types::RobotState& initial_state, + const std::vector& middle_states_near_goal, + const tmc_manipulation_types::RobotState& goal_state, + double normalized_vel, + const tmc_manipulation_types::RobotState& middle_state, + tmc_manipulation_types::TimedRobotTrajectory& trajectory_out) = 0; +}; + + +class PlanarTrajectoryInterpolator : public PlanarTrajectoryInterpolatorBase { + public: + PlanarTrajectoryInterpolator() {} + virtual ~PlanarTrajectoryInterpolator() = default; + + void Initialize(const rclcpp::Node::SharedPtr& node, const SamplingParameters::Ptr& params) override; + + private: + // Dynamic parameter setting handle + std::vector set_param_handlers_; + + // Maximum value of scores at the time of orbit interpolation + tmc_utils::DynamicParameter::Ptr max_sampling_trajectory_; + // Track time stamp + tmc_utils::DynamicParameter::Ptr time_stamp_; + + // spline interpolator + bool InterpolateTrajectory( + const tmc_manipulation_types::RobotState& initial_state, + const std::vector& middle_states_near_goal, + const tmc_manipulation_types::RobotState& goal_state, + double normalized_vel, + tmc_manipulation_types::TimedRobotTrajectory& trajectory_out) override; + + // spline interpolator + bool InterpolateTrajectory( + const tmc_manipulation_types::RobotState& initial_state, + const std::vector& middle_states_near_goal, + const tmc_manipulation_types::RobotState& goal_state, + double normalized_vel, + const tmc_manipulation_types::RobotState& middle_state, + tmc_manipulation_types::TimedRobotTrajectory& trajectory_out) override; + + // Generate an interpolation orbital + void InterpolateTrajectoryImpl( + const tmc_manipulation_types::NameSeq& joint_names, + const tmc_manipulation_types::NameSeq& base_names, + const std::vector& joint_coeff, + const std::vector& base_coeff, + double end_time, + tmc_manipulation_types::TimedRobotTrajectory& trajectory_out); +}; + + +class PlanarTrajectoryInterpolatorPlain : public PlanarTrajectoryInterpolatorBase { + public: + PlanarTrajectoryInterpolatorPlain() {} + virtual ~PlanarTrajectoryInterpolatorPlain() = default; + + private: + bool InterpolateTrajectory( + const tmc_manipulation_types::RobotState& initial_state, + const std::vector& middle_states_near_goal, + const tmc_manipulation_types::RobotState& goal_state, + double normalized_vel, + tmc_manipulation_types::TimedRobotTrajectory& trajectory_out) override; + + bool InterpolateTrajectory( + const tmc_manipulation_types::RobotState& initial_state, + const std::vector& middle_states_near_goal, + const tmc_manipulation_types::RobotState& goal_state, + double normalized_vel, + const tmc_manipulation_types::RobotState& middle_state, + tmc_manipulation_types::TimedRobotTrajectory& trajectory_out) override; +}; + +} // namespace tmc_simple_path_generator +#endif // TMC_SIMPLE_PATH_GENERATOR_PLANAR_TRAJECTORY_INTERPOLATOR_HPP_ diff --git a/tmc_simple_path_generator/include/tmc_simple_path_generator/sample_goal_generator.hpp b/tmc_simple_path_generator/include/tmc_simple_path_generator/sample_goal_generator.hpp new file mode 100644 index 0000000..6d0cb86 --- /dev/null +++ b/tmc_simple_path_generator/include/tmc_simple_path_generator/sample_goal_generator.hpp @@ -0,0 +1,193 @@ +/* +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. +*/ +/// @brief Sample Goal Generator Class +#ifndef TMC_SIMPLE_PATH_GENERATOR_SAMPLE_GOAL_GENERATOR_HPP_ +#define TMC_SIMPLE_PATH_GENERATOR_SAMPLE_GOAL_GENERATOR_HPP_ + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include + +namespace tmc_simple_path_generator { + +class SampleGoalGeneratorBase { + public: + using Ptr = std::shared_ptr; + + /// @brief constructor + SampleGoalGeneratorBase(); + /// @brief Destructor + virtual ~SampleGoalGeneratorBase() = default; + + /// @brief Initialize to fix the seed + void Initialize(const rclcpp::Node::SharedPtr& node, uint32_t random_seed); + /// @brief Initialize to generate seed randomly + void Initialize(const rclcpp::Node::SharedPtr& node); + + /// @brief Sampling the goal posture from joint restraint + bool SampleFromJointConstraints( + const std::vector& joint_constraints, + tmc_manipulation_types::RobotState& goal_state_out); + + /// @brief Sampling the goal posture from the posture restraint + bool SampleFromLinkConstraints( + const tmc_manipulation_types::RobotState& initial_state, + const std::vector& link_constraints, + const SamplingParameters::Ptr& params, + tmc_manipulation_types::RobotState& goal_state_out); + + /// Set whether to add random numbers to the initial value of sampling from posture restraint + void set_is_from_random_initial_state(bool value) { is_from_random_initial_state_ = value; } + + /// @brief Treatment at the end + virtual void Terminate() { + is_from_random_initial_state_ = false; + } + + protected: + // IKSOLVER loader + pluginlib::ClassLoader ik_solver_loader_; + + /// @brief Implementation of initialization of derivative classes + virtual void InitializeImpl(const rclcpp::Node::SharedPtr& node) = 0; + + /// @brief Find ROBOT_STATE to realize the terminal posture origin_to_end + virtual bool SolveIK(const Eigen::Affine3d& origin_to_end, + const tmc_manipulation_types::RobotState& initial_state, + const SamplingParameters::Ptr& params, + const std::string& end_frame, + tmc_manipulation_types::RobotState& solution_out) = 0; + + private: + // Random number generator + std::mt19937 engine_; + // Dynamic parameter setting handle + std::vector set_param_handlers_; + // Width of random elements in robot posture when solving IK [M] + tmc_utils::DynamicParameter::Ptr ik_initial_range_; + + // Whether to add random numbers to the initial value of sampling from posture restraint + bool is_from_random_initial_state_; +}; + + +class SampleGoalGenerator : public SampleGoalGeneratorBase { + public: + /// @brief constructor + SampleGoalGenerator() = default; + /// @brief Destructor + virtual ~SampleGoalGenerator() = default; + + protected: + /// @brief Implementation of initialization + void InitializeImpl(const rclcpp::Node::SharedPtr& node) override; + + /// @brief Find ROBOT_STATE to realize the terminal posture origin_to_end + bool SolveIK(const Eigen::Affine3d& origin_to_end, + const tmc_manipulation_types::RobotState& initial_state, + const SamplingParameters::Ptr& params, + const std::string& end_frame, + tmc_manipulation_types::RobotState& solution_out) override; + + private: + // IKSOLVER instance + std::shared_ptr ik_solver_; +}; + + +class SampleGoalGeneratorMultiThread : public SampleGoalGeneratorBase { + public: + /// @brief constructor + SampleGoalGeneratorMultiThread(); + /// @brief Destructor + virtual ~SampleGoalGeneratorMultiThread(); + + /// @brief Treatment at the end + void Terminate() override; + + protected: + /// @brief Implementation of initialization + void InitializeImpl(const rclcpp::Node::SharedPtr& node) override; + + /// @brief Find ROBOT_STATE to realize the terminal posture origin_to_end + bool SolveIK(const Eigen::Affine3d& origin_to_end, + const tmc_manipulation_types::RobotState& initial_state, + const SamplingParameters::Ptr& params, + const std::string& end_frame, + tmc_manipulation_types::RobotState& solution_out) override; + + private: + // IKSOLVER instance + std::shared_ptr ik_solver_; + + // Variables/functions for multi -threaded processing + std::vector workers_; + void SolveIkThread(const std::string& solver_type, const std::string& robot_description); + + struct Request { + tmc_robot_kinematics_model::IKRequest request; + tmc_manipulation_types::RobotState initial_state; + }; + std::queue request_queue_; + std::mutex request_queue_mutex_; + + struct Response { + tmc_robot_kinematics_model::IKResult result; + tmc_manipulation_types::RobotState initial_state; + tmc_manipulation_types::JointState joint_solution; + Eigen::Affine3d origin_to_base; + }; + std::queue response_queue_; + std::mutex response_queue_mutex_; + + std::condition_variable condition_; + bool stop_thread_; + + std::mutex stop_ik_mutex_; + bool stop_ik_; + + std::mutex num_of_running_ik_mutex_; + uint32_t num_of_running_ik_; +}; + +} // namespace tmc_simple_path_generator + +#endif // TMC_SIMPLE_PATH_GENERATOR_SAMPLE_GOAL_GENERATOR_HPP + diff --git a/tmc_simple_path_generator/include/tmc_simple_path_generator/sample_middle_generator.hpp b/tmc_simple_path_generator/include/tmc_simple_path_generator/sample_middle_generator.hpp new file mode 100644 index 0000000..9992bbe --- /dev/null +++ b/tmc_simple_path_generator/include/tmc_simple_path_generator/sample_middle_generator.hpp @@ -0,0 +1,94 @@ +/* +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. +*/ +#ifndef TMC_SIMPLE_PATH_GENERATOR_SAMPLE_MIDDLE_GENERATOR_HPP_ +#define TMC_SIMPLE_PATH_GENERATOR_SAMPLE_MIDDLE_GENERATOR_HPP_ + +#include +#include +#include + +#include + +#include +#include + +#include + +namespace tmc_simple_path_generator { + +// It's a strange name, but it's a hassle to change the name of SampleGoalGenerator now, so I will match it. +class ISampleMiddleGenerator { + public: + using Ptr = std::shared_ptr; + + virtual ~ISampleMiddleGenerator() = default; + + // Initialization + virtual void Initialize(const rclcpp::Node::SharedPtr& node, const SamplingParameters::Ptr& params) = 0; + + // Calculate a random intermediate state + virtual std::optional ComputeRandomMiddleState( + const tmc_manipulation_types::RobotState& initial_state, + const tmc_manipulation_types::RobotState& goal_state, + const SamplingParameters::Ptr& params) = 0; +}; + +class PlanerMiddleStateGenerator : public ISampleMiddleGenerator { + public: + PlanerMiddleStateGenerator() = default; + ~PlanerMiddleStateGenerator() = default; + + // Initialization + void Initialize(const rclcpp::Node::SharedPtr& node, const SamplingParameters::Ptr& params) override; + + // Calculate a random intermediate state + std::optional ComputeRandomMiddleState( + const tmc_manipulation_types::RobotState& initial_state, + const tmc_manipulation_types::RobotState& goal_state, + const SamplingParameters::Ptr& params) override; + + private: + // Calculate the middle state + bool ComputeMiddleState( + const tmc_manipulation_types::RobotState& initial_state, + const tmc_manipulation_types::RobotState& goal_state, + tmc_manipulation_types::RobotState& middle_state_out); + + // Orbit -generated parameters + SamplingParameters::Ptr sampling_params_; + // Dynamic parameter setting handle + std::vector set_param_handlers_; + + // Width of random element elements in the middle bogie position [M] + tmc_utils::DynamicParameter::Ptr middle_state_base_position_range_; + // The width of the random element element of the middle state of bogie rotation [RAD] + tmc_utils::DynamicParameter::Ptr middle_state_base_rotation_range_; +}; + +} // namespace tmc_simple_path_generator +#endif // TMC_SIMPLE_PATH_GENERATOR_SAMPLE_MIDDLE_GENERATOR_HPP_ diff --git a/tmc_simple_path_generator/include/tmc_simple_path_generator/sampling_parameters.hpp b/tmc_simple_path_generator/include/tmc_simple_path_generator/sampling_parameters.hpp new file mode 100644 index 0000000..d9fa5d8 --- /dev/null +++ b/tmc_simple_path_generator/include/tmc_simple_path_generator/sampling_parameters.hpp @@ -0,0 +1,71 @@ +/* +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. +*/ +/// @brief Parameters for track generation +#ifndef TMC_SIMPLE_PATH_GENERATOR_SAMPLING_PARAMETERS_HPP_ +#define TMC_SIMPLE_PATH_GENERATOR_SAMPLING_PARAMETERS_HPP_ + +#include +#include +#include + +#include + +#include +#include + +namespace tmc_simple_path_generator { + +struct SamplingParameters { + using Ptr = std::shared_ptr; + + // Joint name + tmc_manipulation_types::NameSeq joint_names; + // Bogie name + tmc_manipulation_types::NameSeq base_names; + // Joint weight + Eigen::VectorXd joint_weights; + // Bogie weight + Eigen::VectorXd base_weights; + // Joint maximum limit + std::vector joint_limit_upper; + // Lower limit of joints + std::vector joint_limit_lower; + // Bogie movement type + int base_movement_type; + + SamplingParameters() {} + explicit SamplingParameters(const rclcpp::Node::SharedPtr& node); + + SamplingParameters::Ptr RemoveJoints(const std::vector& ignore_joints) const; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace tmc_simple_path_generator + +#endif // TMC_SIMPLE_PATH_GENERATOR_SAMPLING_PARAMETERS_HPP_ diff --git a/tmc_simple_path_generator/include/tmc_simple_path_generator/simple_path_generator.hpp b/tmc_simple_path_generator/include/tmc_simple_path_generator/simple_path_generator.hpp new file mode 100644 index 0000000..adff2b1 --- /dev/null +++ b/tmc_simple_path_generator/include/tmc_simple_path_generator/simple_path_generator.hpp @@ -0,0 +1,141 @@ +/* +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. +*/ +/// @brief Simple Path Generator Class +#ifndef TMC_SIMPLE_PATH_GENERATOR_SIMPLE_PATH_GENERATOR_HPP_ +#define TMC_SIMPLE_PATH_GENERATOR_SIMPLE_PATH_GENERATOR_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +namespace tmc_simple_path_generator { + +class SimplePathGeneratorPlugin : public tmc_robot_local_planner::IGenerator { + public: + SimplePathGeneratorPlugin(); + virtual ~SimplePathGeneratorPlugin() = default; + + void Initialize(const rclcpp::Node::SharedPtr& node) override; + + bool Generate(const tmc_robot_local_planner::Constraints& constraints, + const tmc_manipulation_types::RobotState& initial_state, + double normalized_velocity, + const std::vector& ignore_joints, + std::function interrupt, + std::vector& trajectories_out) override; + + private: + // clock + rclcpp::Clock::SharedPtr clock_; + + // Generator timeout + tmc_utils::DynamicParameter::Ptr generate_timeout_; + // Number of tracks aiming for straight goals + tmc_utils::DynamicParameter::Ptr max_simple_trajectory_num_; + // If the maximum number of trajectory to be generated is 0 or less, it will be generated to a timeout + tmc_utils::DynamicParameter::Ptr max_trajectory_num_; + // Dynamic parameter setting handle + std::vector set_param_handlers_; + + // SampleGoalGenerator Lauder + pluginlib::ClassLoader sample_goal_generator_loader_; + // SampleGoalGenerator instance + SampleGoalGeneratorBase::Ptr sample_goal_generator_; + + // SampleMidDlegenerator loader + pluginlib::ClassLoader sample_middle_generator_loader_; + // SampleMiddleGenerator instance + ISampleMiddleGenerator::Ptr sample_middle_generator_; + + // Orbit -generated parameters + SamplingParameters::Ptr sampling_params_; + + // TrajectoryInterPolator loader + pluginlib::ClassLoader trajectory_interpolator_loader_; + // TrajectoryInterPolator instance + ITrajectoryInterpolator::Ptr trajectory_interpolator_; + + // Restraint of the specified frame route + HardPathLinkConstraints::Ptr hard_path_link_constraints_; + GoalRelativeLinearConstraint::Ptr goal_relative_linear_constraint_; +}; + +class SimplePathGenerator : public rclcpp::Node { + public: + SimplePathGenerator(); + explicit SimplePathGenerator(const rclcpp::NodeOptions& options); + + virtual ~SimplePathGenerator() = default; + + // I want to do Shared_from_this, so initialize after instance generation + void Initialize(); + + private: + using ServerGoalHandle = rclcpp_action::ServerGoalHandle; + using ServerGoalHandlePtr = std::shared_ptr; + + rclcpp_action::GoalResponse GoalCallback( + const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal); + rclcpp_action::CancelResponse CancelCallback(const ServerGoalHandlePtr goal_handle); + void FeedbackSetupCallback(ServerGoalHandlePtr goal_handle); + void Execute(const ServerGoalHandlePtr goal_handle); + + rclcpp_action::Server::SharedPtr server_; + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + // Origin frame + std::string origin_frame_; + // Origin frame + double timeout_; + + SimplePathGeneratorPlugin impl_; +}; +} // namespace tmc_simple_path_generator + +#endif // TMC_SIMPLE_PATH_GENERATOR_SIMPLE_PATH_GENERATOR_HPP_ diff --git a/tmc_simple_path_generator/include/tmc_simple_path_generator/trajectory_interpolator.hpp b/tmc_simple_path_generator/include/tmc_simple_path_generator/trajectory_interpolator.hpp new file mode 100644 index 0000000..8fd2f41 --- /dev/null +++ b/tmc_simple_path_generator/include/tmc_simple_path_generator/trajectory_interpolator.hpp @@ -0,0 +1,86 @@ +/* +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. +*/ +/// @brief Trajectory Interpolator Class +#ifndef TMC_SIMPLE_PATH_GENERATOR_TRAJECTORY_INTERPOLATOR_HPP_ +#define TMC_SIMPLE_PATH_GENERATOR_TRAJECTORY_INTERPOLATOR_HPP_ + +#include +#include +#include + +#include + +#include +#include + +namespace tmc_simple_path_generator { + +class ITrajectoryInterpolator { + public: + typedef std::shared_ptr Ptr; + + virtual ~ITrajectoryInterpolator() {} + + virtual void Initialize(const rclcpp::Node::SharedPtr& node, const SamplingParameters::Ptr& params) = 0; + + // Orbit an interpolation that satisfies the restraint in the shortest time + virtual bool Interpolate( + const tmc_manipulation_types::RobotState& initial_state, + const tmc_manipulation_types::RobotState& goal_state, + double normalized_vel, + tmc_manipulation_types::TimedRobotTrajectory& trajectory_out) = 0; + + // Calonia interpolation to configure via points + virtual bool Interpolate( + const tmc_manipulation_types::RobotState& initial_state, + const tmc_manipulation_types::RobotState& goal_state, + double normalized_vel, + const tmc_manipulation_types::RobotState& random_middle_state, + tmc_manipulation_types::TimedRobotTrajectory& trajectory_out) = 0; + + // Orbit an interpolation that satisfies the restraint in the shortest time + virtual bool Interpolate( + const tmc_manipulation_types::RobotState& initial_state, + const std::vector& middle_states_near_goal, + const tmc_manipulation_types::RobotState& goal_state, + double normalized_vel, + tmc_manipulation_types::TimedRobotTrajectory& trajectory_out) = 0; + + // Calonia interpolation to configure via points + virtual bool Interpolate( + const tmc_manipulation_types::RobotState& initial_state, + const std::vector& middle_states_near_goal, + const tmc_manipulation_types::RobotState& goal_state, + double normalized_vel, + const tmc_manipulation_types::RobotState& random_middle_state, + tmc_manipulation_types::TimedRobotTrajectory& trajectory_out) = 0; +}; + +} // namespace tmc_simple_path_generator + +#endif // TMC_SIMPLE_PATH_GENERATOR_TRAJECTORY_INTERPOLATOR_HPP_ diff --git a/tmc_simple_path_generator/interpolator_plugins.xml b/tmc_simple_path_generator/interpolator_plugins.xml new file mode 100644 index 0000000..9534a42 --- /dev/null +++ b/tmc_simple_path_generator/interpolator_plugins.xml @@ -0,0 +1,12 @@ + + + planar trajectory interpolator + + + not elaborate planar trajectory interpolator + + \ No newline at end of file diff --git a/tmc_simple_path_generator/middle_sampler_plugins.xml b/tmc_simple_path_generator/middle_sampler_plugins.xml new file mode 100644 index 0000000..337a5fe --- /dev/null +++ b/tmc_simple_path_generator/middle_sampler_plugins.xml @@ -0,0 +1,7 @@ + + + Middle state sampler for omni-base + + \ No newline at end of file diff --git a/tmc_simple_path_generator/package.xml b/tmc_simple_path_generator/package.xml new file mode 100644 index 0000000..9abe4d0 --- /dev/null +++ b/tmc_simple_path_generator/package.xml @@ -0,0 +1,37 @@ + + tmc_simple_path_generator + 2.0.0 + The tmc_simple_path_generator package + + HSR Support + + Keisuke Takeshita + Satoru Onoda + + BSD 3-clause Clear License + + ament_cmake + + eigen + + angles + boost + pluginlib + rclcpp + rclcpp_action + tmc_manipulation_types + tmc_manipulation_types_bridge + tmc_planning_msgs + tmc_robot_kinematics_model + tmc_robot_local_planner + tmc_robot_local_planner_utils + tmc_utils + urdf + + ament_cmake_gtest + tmc_manipulation_tests + + + ament_cmake + + \ No newline at end of file diff --git a/tmc_simple_path_generator/src/tmc_simple_path_generator/calculator.cpp b/tmc_simple_path_generator/src/tmc_simple_path_generator/calculator.cpp new file mode 100644 index 0000000..4b44973 --- /dev/null +++ b/tmc_simple_path_generator/src/tmc_simple_path_generator/calculator.cpp @@ -0,0 +1,86 @@ +/* +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. +*/ +/// @brief Calculator functions + +#include "calculator.hpp" + +#include +#include + +namespace tmc_simple_path_generator { + +// Find the interpolation coefficient +std::vector CalcPoly(const Eigen::VectorXd& x0, + const Eigen::VectorXd& v0, + const Eigen::VectorXd& x1, + double te) { + if (fabs(te) < std::numeric_limits::epsilon()) { + throw std::invalid_argument("zero end time."); + } + std::vector a(5); + a[4] = Eigen::VectorXd::Zero(x0.size()); + a[3] = (v0 * te + 2.0 * x0 - 2.0 * x1) / (te * te * te); + a[2] = -a[3] * te - v0 / te - (x0 - x1) / (te * te); + a[1] = v0; + a[0] = x0; + return a; +} + +// Find the interpolation coefficient +std::vector CalcPolyViaPoint(const Eigen::VectorXd& x0, + const Eigen::VectorXd& v0, + const Eigen::VectorXd& x1, + const Eigen::VectorXd& xa, + double te) { + if (fabs(te) < std::numeric_limits::epsilon()) { + throw std::invalid_argument("zero end time."); + } + std::vector a(5); + a[4] = -2 * (4 * x0 + 4 * x1 - 8 * xa + te * v0) / (te * te * te * te); + a[3] = (18 * x0 + 14 * x1 - 32 * xa + 5 * te * v0) / (te * te * te); + a[2] = -(11 * x0 + 5 * x1 - 16 * xa + 4 * te * v0)/ (te * te); + a[1] = v0; + a[0] = x0; + return a; +} + +// Invite the location +Eigen::VectorXd PolyViaPoint(const std::vector& a, double t) { + return a[4] * t * t * t * t + a[3] * t * t * t + a[2] * t * t + a[1] * t + a[0]; +} + +// Find speed +Eigen::VectorXd VelViaPoint(const std::vector& a, double t) { + return 4 * a[4] * t * t * t + 3 * a[3] * t * t + 2 * a[2] * t + a[1]; +} + +// Find acceleration +Eigen::VectorXd AccViaPoint(const std::vector& a, double t) { + return 12 * a[4] * t * t + 6 * a[3] * t + 2 * a[2]; +} +} // namespace tmc_simple_path_generator diff --git a/tmc_simple_path_generator/src/tmc_simple_path_generator/calculator.hpp b/tmc_simple_path_generator/src/tmc_simple_path_generator/calculator.hpp new file mode 100644 index 0000000..e816a78 --- /dev/null +++ b/tmc_simple_path_generator/src/tmc_simple_path_generator/calculator.hpp @@ -0,0 +1,80 @@ +/* +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. +*/ +/// @brief Calculator functions +#ifndef TMC_SIMPLE_PATH_GENERATOR_CALCULATOR_HPP_ +#define TMC_SIMPLE_PATH_GENERATOR_CALCULATOR_HPP_ + +#include + +#include + +namespace tmc_simple_path_generator { + +/// @brief Find the interpolation coefficient +/// @param x0 Initial state position +/// @param v0 Initial state speed +/// @param x1 Goal state position +/// @param te End time +/// @return Interpolation coefficient +std::vector CalcPoly(const Eigen::VectorXd& x0, + const Eigen::VectorXd& v0, + const Eigen::VectorXd& x1, + double te); + +/// @brief Find the interpolation coefficient +/// @param X0 Initial state position +/// @param v0 Initial state speed +/// @param x1 Goal state position +/// @param xA Intermediate state position +/// @param te End time +/// @return Interpolation coefficient +std::vector CalcPolyViaPoint(const Eigen::VectorXd& x0, + const Eigen::VectorXd& v0, + const Eigen::VectorXd& x1, + const Eigen::VectorXd& xa, + double te); + +/// @brief Invite the location +/// @param a interpolation coefficient list +/// @param t up time +/// @return Location +Eigen::VectorXd PolyViaPoint(const std::vector& a, double t); + +/// @brief Find speed +/// @param a interpolation coefficient list +/// @param t up time +/// @return speed +Eigen::VectorXd VelViaPoint(const std::vector& a, double t); + +/// @brief Find acceleration +/// @param a interpolation coefficient list +/// @param t up time +/// @return Acceleration +Eigen::VectorXd AccViaPoint(const std::vector& a, double t); +} // namespace tmc_simple_path_generator +#endif // TMC_SIMPLE_PATH_GENERATOR_CALCULATOR_HPP_ diff --git a/tmc_simple_path_generator/src/tmc_simple_path_generator/common.cpp b/tmc_simple_path_generator/src/tmc_simple_path_generator/common.cpp new file mode 100644 index 0000000..83c9741 --- /dev/null +++ b/tmc_simple_path_generator/src/tmc_simple_path_generator/common.cpp @@ -0,0 +1,281 @@ +/* +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. +*/ +/// @brief Common functions + +#include "common.hpp" + +#include +#include +#include + +#include +#include + +#include + +#include + +using Eigen::VectorXd; + +namespace { + +template +bool Extract(const tmc_manipulation_types::NameSeq& joint_names, + const tmc_manipulation_types::NameSeq& target_names, + const T& vector_in, + U& vector_out) { + vector_out.resize(target_names.size()); + for (const auto& target_name : target_names | boost::adaptors::indexed()) { + auto index = tmc_manipulation_types::GetJointIndex(joint_names, target_name.value()); + if (index < vector_in.size()) { + vector_out[target_name.index()] = vector_in[index]; + } else { + CONSOLE_BRIDGE_logError("Index out of range."); + return false; + } + } + return true; +} + +template +void Overwrite(const tmc_manipulation_types::NameSeq& joint_names, + const tmc_manipulation_types::NameSeq& target_names, + const T& vector_in, + U& vector_out) { + for (const auto& target_name : target_names | boost::adaptors::indexed()) { + auto index = tmc_manipulation_types::GetJointIndex(joint_names, target_name.value()); + if (index < vector_out.size()) { + vector_out[index] = vector_in[target_name.index()]; + } + } +} + +} // namespace + +namespace tmc_simple_path_generator { + +// Overwrite Robot_state_in_out +bool OverwriteRobotState( + tmc_manipulation_types::RobotState& robot_state_in_out, + const tmc_manipulation_types::NameSeq& joint_names, + const tmc_manipulation_types::NameSeq& base_names, + const VectorXd& joint_positions, + const tmc_manipulation_types::PoseSeq& base_poses) { + if ((joint_names.size() != joint_positions.size()) || + (base_names.size() != base_poses.size())) { + CONSOLE_BRIDGE_logError("Size mismatch."); + return false; + } + Overwrite(robot_state_in_out.joint_state.name, joint_names, + joint_positions, robot_state_in_out.joint_state.position); + Overwrite(robot_state_in_out.multi_dof_joint_state.names, base_names, + base_poses, robot_state_in_out.multi_dof_joint_state.poses); + return true; +} + +// Extract information from Robot_state +bool ExtractFromRobotState( + const tmc_manipulation_types::RobotState& robot_state, + const tmc_manipulation_types::NameSeq& joint_names, + const tmc_manipulation_types::NameSeq& base_names, + VectorXd& joint_positions_out, + tmc_manipulation_types::PoseSeq& base_poses_out) { + if (!Extract(robot_state.joint_state.name, joint_names, + robot_state.joint_state.position, joint_positions_out)) { + return false; + } + if (!Extract(robot_state.multi_dof_joint_state.names, base_names, + robot_state.multi_dof_joint_state.poses, base_poses_out)) { + return false; + } + return true; +} + +// Add a uniform random number of Delta to Robot_state +bool AddRandomState( + const tmc_manipulation_types::RobotState& robot_state_in, + const tmc_manipulation_types::NameSeq& joint_names, + const tmc_manipulation_types::NameSeq& base_names, + const std::vector& joint_limit_upper, + const std::vector& joint_limit_lower, + double delta, + tmc_manipulation_types::RobotState& robot_state_out) { + if (joint_names.size() != joint_limit_upper.size() || + joint_names.size() != joint_limit_lower.size()) { + return false; + } + + VectorXd joint_positions_in; + joint_positions_in.resize(joint_names.size()); + tmc_manipulation_types::PoseSeq base_poses_in; + base_poses_in.resize(base_names.size()); + if (!ExtractFromRobotState(robot_state_in, joint_names, base_names, joint_positions_in, base_poses_in)) { + return false; + } + + VectorXd joint_positions_out = joint_positions_in + delta * VectorXd::Random(joint_positions_in.size()); + for (uint32_t i = 0; i < joint_positions_out.size(); ++i) { + joint_positions_out[i] = std::max(std::min(joint_positions_out[i], joint_limit_upper[i]), joint_limit_lower[i]); + } + tmc_manipulation_types::PoseSeq base_poses_out; + for (uint32_t i = 0; i < base_names.size(); ++i) { + base_poses_out.push_back( + base_poses_in[i] * tmc_robot_local_planner_utils::GetTransform(delta * Eigen::Vector3d::Random())); + } + robot_state_out = robot_state_in; + return OverwriteRobotState(robot_state_out, joint_names, base_names, + joint_positions_out, base_poses_out); +} + +// Replace Robot_state with a random joint posture, sampling uniformly in the upper and lower limits +bool ReplaceWithUniformRandomJointPositions( + const tmc_manipulation_types::RobotState& robot_state_in, + const std::vector& joint_names, + const std::vector& joint_limit_upper, + const std::vector& joint_limit_lower, + tmc_manipulation_types::RobotState& robot_state_out) { + if (joint_names.size() != joint_limit_upper.size() || + joint_names.size() != joint_limit_lower.size()) { + return false; + } + VectorXd joint_positions(VectorXd::Random(joint_names.size())); + for (uint32_t i = 0; i < joint_names.size(); ++i) { + auto range = joint_limit_upper[i] - joint_limit_lower[i]; + auto sum = joint_limit_upper[i] + joint_limit_lower[i]; + joint_positions[i] = (sum + range * joint_positions[i]) / 2.0; + } + robot_state_out = robot_state_in; + return OverwriteRobotState(robot_state_out, joint_names, {}, joint_positions, {}); +} + +// An uniform sampling that adds a random offset to the bogie position attitude +bool AddUniformRandomBasePoseOffset( + const tmc_manipulation_types::RobotState& robot_state_in, + const tmc_manipulation_types::NameSeq& base_names, + double position_range, + double rotation_range, + tmc_manipulation_types::RobotState& robot_state_out) { + VectorXd joint_positions; + tmc_manipulation_types::PoseSeq base_poses; + ExtractFromRobotState(robot_state_in, {}, base_names, joint_positions, + base_poses); + Eigen::Vector3d pose_2d(Eigen::Vector3d::Random()); + pose_2d[0] *= position_range; + pose_2d[1] *= position_range; + pose_2d[2] *= rotation_range; + auto offset_pose = tmc_robot_local_planner_utils::GetTransform(pose_2d); + + robot_state_out = robot_state_in; + return OverwriteRobotState(robot_state_out, {}, base_names, {}, + {base_poses[0] * offset_pose}); +} + +// Remove joints that are not used +void RemoveIgnored(const std::vector& joint_names, + const std::vector& ignore_joints, + std::vector& result_out, + std::vector& remaining_indices_out) { + // Joint_names, IGNORE_JOINTS, both are 10 or such numbers, so they will not reduce the calculation with wise algorithms. + for (uint32_t i = 0; i < joint_names.size(); ++i) { + if (std::find(ignore_joints.begin(), ignore_joints.end(), joint_names[i]) == ignore_joints.end()) { + result_out.push_back(joint_names[i]); + remaining_indices_out.push_back(i); + } + } +} + +// Remove joints that are not used +void RemoveIgnored(const std::vector& joint_names, + const std::vector& ignore_joints, + std::vector& result_out) { + std::vector remaing_indices; + RemoveIgnored(joint_names, ignore_joints, result_out, remaing_indices); +} + +// Calculation of weighted distance +double CalculateLength(const Eigen::VectorXd& prev, + const Eigen::VectorXd& next, + const Eigen::VectorXd& weights) { + return ((next - prev).array().abs() * weights.array()).maxCoeff(); +} + +// Calculation of weighted joint distance +double CalculateJointLength(const tmc_manipulation_types::RobotState& prev, + const tmc_manipulation_types::RobotState& next, + const Eigen::VectorXd& weights) { + return CalculateLength(prev.joint_state.position, next.joint_state.position, weights); +} + +// Calculation of heavy bogie distance +double CalculateBaseLength(const Eigen::Affine3d& prev, + const Eigen::Affine3d& next, + const Eigen::VectorXd& weights) { + const auto prev_vec = tmc_robot_local_planner_utils::Get2DPose(prev); + auto next_vec = tmc_robot_local_planner_utils::Get2DPose(next); + next_vec[2] = prev_vec[2] + angles::shortest_angular_distance(prev_vec[2], next_vec[2]); + return CalculateLength(prev_vec, next_vec, weights); +} + +// Connect to Trajectory by connecting the Robotstate group +void Convert(const tmc_manipulation_types::RobotState& initial_state, + const std::vector& way_points, + const Eigen::VectorXd& joint_weights, + const Eigen::VectorXd& base_weights, + tmc_manipulation_types::TimedRobotTrajectory& trajectory_out) { + trajectory_out.joint_trajectory.joint_names = initial_state.joint_state.name; + trajectory_out.multi_dof_joint_trajectory.joint_names = initial_state.multi_dof_joint_state.names; + + trajectory_out.joint_trajectory.points.resize(1 + way_points.size()); + trajectory_out.multi_dof_joint_trajectory.points.resize(1 + way_points.size()); + + trajectory_out.joint_trajectory.points[0].positions = initial_state.joint_state.position; + trajectory_out.joint_trajectory.points[0].velocities = initial_state.joint_state.velocity; + trajectory_out.multi_dof_joint_trajectory.points[0].transforms = initial_state.multi_dof_joint_state.poses; + trajectory_out.multi_dof_joint_trajectory.points[0].velocities = initial_state.multi_dof_joint_state.twist; + + for (auto i = 0; i < way_points.size(); ++i) { + double length = std::max(CalculateLength(trajectory_out.joint_trajectory.points[i].positions, + way_points[i].joint_state.position, joint_weights), + CalculateBaseLength(trajectory_out.multi_dof_joint_trajectory.points[i].transforms[0], + way_points[i].multi_dof_joint_state.poses[0], base_weights)); + + trajectory_out.joint_trajectory.points[i + 1].positions = way_points[i].joint_state.position; + trajectory_out.joint_trajectory.points[i + 1].velocities = Eigen::VectorXd::Zero( + way_points[i].joint_state.position.size()); + trajectory_out.joint_trajectory.points[i + 1].time_from_start = + trajectory_out.joint_trajectory.points[i].time_from_start + length; + + trajectory_out.multi_dof_joint_trajectory.points[i + 1].transforms = way_points[i].multi_dof_joint_state.poses; + trajectory_out.multi_dof_joint_trajectory.points[i + 1].velocities.resize( + way_points[i].multi_dof_joint_state.poses.size(), tmc_manipulation_types::Twist::Zero()); + trajectory_out.multi_dof_joint_trajectory.points[i + 1].time_from_start = + trajectory_out.multi_dof_joint_trajectory.points[i].time_from_start + length; + } +} + +} // namespace tmc_simple_path_generator diff --git a/tmc_simple_path_generator/src/tmc_simple_path_generator/common.hpp b/tmc_simple_path_generator/src/tmc_simple_path_generator/common.hpp new file mode 100644 index 0000000..2de3530 --- /dev/null +++ b/tmc_simple_path_generator/src/tmc_simple_path_generator/common.hpp @@ -0,0 +1,131 @@ +/* +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. +*/ +/// @brief Common functions + +#include +#include + +#include + +#include +#include + +namespace tmc_simple_path_generator { + +/// @brief Overwrite robot_state_in_out +bool OverwriteRobotState( + tmc_manipulation_types::RobotState& robot_state_in_out, + const tmc_manipulation_types::NameSeq& joint_names, + const tmc_manipulation_types::NameSeq& base_names, + const Eigen::VectorXd& joint_positions, + const tmc_manipulation_types::PoseSeq& base_poses); + +/// @brief Extract information from robot_state +bool ExtractFromRobotState( + const tmc_manipulation_types::RobotState& robot_state, + const tmc_manipulation_types::NameSeq& joint_names, + const tmc_manipulation_types::NameSeq& base_names, + Eigen::VectorXd& joint_positions_out, + tmc_manipulation_types::PoseSeq& base_poses_out); + +/// @brief Add a uniform random number of Delta to robot_state_in +bool AddRandomState( + const tmc_manipulation_types::RobotState& robot_state_in, + const tmc_manipulation_types::NameSeq& joint_names, + const tmc_manipulation_types::NameSeq& base_names, + const std::vector& joint_limit_upper, + const std::vector& joint_limit_lower, + double delta, + tmc_manipulation_types::RobotState& robot_state_out); + +/// @brief Replace robot_state_in with a random joint posture, sampling uniformly in the upper and lower limits +bool ReplaceWithUniformRandomJointPositions( + const tmc_manipulation_types::RobotState& robot_state_in, + const tmc_manipulation_types::NameSeq& joint_names, + const std::vector& joint_limit_upper, + const std::vector& joint_limit_lower, + tmc_manipulation_types::RobotState& robot_state_out); + +/// @brief An uniform sampling that adds a random offset to the bogie position attitude +bool AddUniformRandomBasePoseOffset( + const tmc_manipulation_types::RobotState& robot_state_in, + const tmc_manipulation_types::NameSeq& base_names, + double position_range, + double rotation_range, + tmc_manipulation_types::RobotState& robot_state_out); + +// Remove joints that are not used +void RemoveIgnored(const std::vector& joint_names, const std::vector& ignore_joints, + std::vector& result_out, std::vector& remaining_indices_out); + +// Remove joints that are not used +void RemoveIgnored(const std::vector& joint_names, const std::vector& ignore_joints, + std::vector& result_out); + +// Extract only the specified index value +template +TYPE ExtractValues(const TYPE& values, const std::vector& indices) { + TYPE output; + output.resize(indices.size()); + for (uint32_t i = 0; i < indices.size(); ++i) { + output[i] = values[indices[i]]; + } + return output; +} + +// Calculation of weighted distance +double CalculateLength(const Eigen::VectorXd& prev, + const Eigen::VectorXd& next, + const Eigen::VectorXd& weights); + +// Calculation of weighted joint distance +double CalculateJointLength(const tmc_manipulation_types::RobotState& prev, + const tmc_manipulation_types::RobotState& next, + const Eigen::VectorXd& weights); + +// Calculation of heavy bogie distance +double CalculateBaseLength(const Eigen::Affine3d& prev, + const Eigen::Affine3d& next, + const Eigen::VectorXd& weights); + +// Connect to Trajectory by connecting the Robotstate group +void Convert(const tmc_manipulation_types::RobotState& initial_state, + const std::vector& way_points, + const Eigen::VectorXd& joint_weights, + const Eigen::VectorXd& base_weights, + tmc_manipulation_types::TimedRobotTrajectory& trajectory_out); + +// Select one from multiple restraint conditions, C ++ 17 STD :: Sample is also difficult to use. +template +ConstraintPtr SampleConstraints(const std::vector& constraints, + Generator&& generator) { + std::uniform_int_distribution<> range(0, constraints.size() - 1); + return constraints[range(generator)]; +} + +} // namespace tmc_simple_path_generator diff --git a/tmc_simple_path_generator/src/tmc_simple_path_generator/hard_path_constraints.cpp b/tmc_simple_path_generator/src/tmc_simple_path_generator/hard_path_constraints.cpp new file mode 100644 index 0000000..d7edb31 --- /dev/null +++ b/tmc_simple_path_generator/src/tmc_simple_path_generator/hard_path_constraints.cpp @@ -0,0 +1,289 @@ +/* +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. +*/ + +#include + +#include +#include + +#include "common.hpp" + +namespace { +constexpr double kStepThresholdRate = std::sqrt(5.0) / 2.0; + +tmc_manipulation_types::RobotState ExtractRobotState( + const tmc_manipulation_types::TimedRobotTrajectory& trajectory, uint32_t index) { + tmc_manipulation_types::RobotState robot_state; + robot_state.joint_state.name = trajectory.joint_trajectory.joint_names; + robot_state.multi_dof_joint_state.names = trajectory.multi_dof_joint_trajectory.joint_names; + robot_state.joint_state.position = trajectory.joint_trajectory.points[index].positions; + robot_state.multi_dof_joint_state.poses = trajectory.multi_dof_joint_trajectory.points[index].transforms; + return robot_state; +} + +void UpdateTrajectoryPoint(const tmc_manipulation_types::RobotState& robot_state, uint32_t index, + tmc_manipulation_types::TimedRobotTrajectory& dst_trajectory) { + dst_trajectory.joint_trajectory.points[index].positions = robot_state.joint_state.position; + dst_trajectory.multi_dof_joint_trajectory.points[index].transforms = robot_state.multi_dof_joint_state.poses; +} + +void InsertTrajectoryPoint(const tmc_manipulation_types::RobotState& robot_state, uint32_t index, + tmc_manipulation_types::TimedRobotTrajectory& dst_trajectory) { + tmc_manipulation_types::TimedJointTrajectoryPoint joint_trajectory_point; + joint_trajectory_point.positions = robot_state.joint_state.position; + dst_trajectory.joint_trajectory.points.insert( + dst_trajectory.joint_trajectory.points.begin() + index, joint_trajectory_point); + + tmc_manipulation_types::TimedMultiDOFJointTrajectoryPoint multi_dof_joint_trajectory_point; + multi_dof_joint_trajectory_point.transforms = robot_state.multi_dof_joint_state.poses; + dst_trajectory.multi_dof_joint_trajectory.points.insert( + dst_trajectory.multi_dof_joint_trajectory.points.begin() + index, multi_dof_joint_trajectory_point); +} + +} // namespace + +namespace tmc_simple_path_generator { + +HardPathLinkConstraints::HardPathLinkConstraints(const rclcpp::Node::SharedPtr& node) + : fk_loader_("tmc_robot_kinematics_model", "tmc_robot_kinematics_model::IRobotKinematicsModel"), + engine_(std::random_device()()) { + // Initialization of FK + const auto kinematics_type = tmc_utils::GetParameter(node, "kinematics_type", ""); + if (kinematics_type.empty()) { + robot_ = std::make_shared(); + } else { + robot_ = fk_loader_.createSharedInstance(kinematics_type); + } + + auto robot_description = tmc_utils::GetParameter(node, "robot_description_kinematics", ""); + if (robot_description.empty()) { + robot_description = tmc_utils::GetParameter(node, "robot_description", ""); + } + robot_->Initialize(robot_description); + + // Initialization of IK + closest_sampler_ = std::make_shared(); + closest_sampler_->Initialize(node); + closest_sampler_->set_is_from_random_initial_state(false); +} + +bool HardPathLinkConstraints::ConstrainTrajectory( + const std::vector& link_constraints, + const SamplingParameters::Ptr& params, + tmc_manipulation_types::TimedRobotTrajectory& dst_trajectory) { + if (dst_trajectory.joint_trajectory.points.size() < 2 || + dst_trajectory.multi_dof_joint_trajectory.points.size() < 2) { + return false; + } + if (dst_trajectory.joint_trajectory.points.size() != dst_trajectory.multi_dof_joint_trajectory.points.size()) { + return false; + } + if (link_constraints.empty()) { + return true; + } + + auto constraint = SampleConstraints(link_constraints, engine_); + + // The intermediate point is modified so that it is forcibly selected constraint. + for (auto i = 1u; i < dst_trajectory.joint_trajectory.points.size() - 1; ++i) { + const auto robot_state = ExtractRobotState(dst_trajectory, i); + // Calculate only when you need to fix it + const auto origin_to_closest = CalculateOriginToClosest(constraint, robot_state); + if (!origin_to_closest) { + continue; + } + const auto closest_state = ConstrainImpl(origin_to_closest.value(), constraint, robot_state, params); + if (!closest_state) { + return false; + } + UpdateTrajectoryPoint(closest_state.value(), i, dst_trajectory); + } + // After INITIAL, add the orbital point to meet the constraint selected before Goal + { + const auto robot_state = ExtractRobotState(dst_trajectory, 0); + const auto origin_to_closest = CalculateOriginToClosest(constraint, robot_state); + if (origin_to_closest) { + const auto closest_state = ConstrainImpl(origin_to_closest.value(), constraint, robot_state, params); + if (!closest_state) { + return false; + } + InsertTrajectoryPoint(closest_state.value(), 1, dst_trajectory); + } + } + { + const auto robot_state = ExtractRobotState(dst_trajectory, dst_trajectory.joint_trajectory.points.size() - 1); + const auto origin_to_closest = CalculateOriginToClosest(constraint, robot_state); + if (origin_to_closest) { + const auto closest_state = ConstrainImpl(origin_to_closest.value(), constraint, robot_state, params); + if (!closest_state) { + return false; + } + InsertTrajectoryPoint(closest_state.value(), dst_trajectory.joint_trajectory.points.size() - 1, dst_trajectory); + } + } + return true; +} + +std::optional HardPathLinkConstraints::CalculateOriginToClosest( + const tmc_robot_local_planner::ILinkConstraint::Ptr& link_constraint, + const tmc_manipulation_types::RobotState& robot_state) { + robot_->SetNamedAngle(robot_state.joint_state); + robot_->SetRobotTransform(robot_state.multi_dof_joint_state.poses[0]); + + const auto origin_to_link = robot_->GetObjectTransform(link_constraint->GetLinkName()); + const auto origin_to_closest = link_constraint->CalcClosest(origin_to_link); + + // There is no particular basis for the number 1mm + if ((origin_to_closest.translation() - origin_to_link.translation()).norm() > 1.0e-3) { + return origin_to_closest; + } else { + return std::nullopt; + } +} + +std::optional HardPathLinkConstraints::ConstrainImpl( + const Eigen::Affine3d& origin_to_closest, + const tmc_robot_local_planner::ILinkConstraint::Ptr& link_constraint, + const tmc_manipulation_types::RobotState& robot_state, + const SamplingParameters::Ptr& params) { + tmc_manipulation_types::TaskSpaceRegion tsr( + origin_to_closest, Eigen::Affine3d::Identity(), + tmc_manipulation_types::RegionValues::Zero(), tmc_manipulation_types::RegionValues::Zero(), + link_constraint->GetOriginName(), link_constraint->GetLinkName()); + const auto closest_pose_constraint = std::make_shared(tsr, 0); + + tmc_manipulation_types::RobotState closest_state; + if (closest_sampler_->SampleFromLinkConstraints(robot_state, {closest_pose_constraint}, params, closest_state)) { + return closest_state; + } else { + return std::nullopt; + } +} + + +GoalRelativeLinearConstraint::GoalRelativeLinearConstraint(const rclcpp::Node::SharedPtr& node) + : logger_(node->get_logger()), + clock_(node->get_clock()), + fk_loader_("tmc_robot_kinematics_model", "tmc_robot_kinematics_model::IRobotKinematicsModel") { + linear_step_ = tmc_utils::GetParameter(node, "linear_constraint_step", 0.02); + + const auto kinematics_type = tmc_utils::GetParameter(node, "kinematics_type", ""); + if (kinematics_type.empty()) { + robot_ = std::make_shared(); + } else { + robot_ = fk_loader_.createSharedInstance(kinematics_type); + } + + auto robot_description = tmc_utils::GetParameter(node, "robot_description_kinematics", ""); + if (robot_description.empty()) { + robot_description = tmc_utils::GetParameter(node, "robot_description", ""); + } + robot_->Initialize(robot_description); + + sampler_ = std::make_shared(); + sampler_->Initialize(node); + sampler_->set_is_from_random_initial_state(false); +} + +std::optional> GoalRelativeLinearConstraint::SampleLinearPath( + const tmc_robot_local_planner::LinearConstraint linear_constraint, + const tmc_manipulation_types::RobotState& initial_state, + const tmc_manipulation_types::RobotState& goal_state, + const SamplingParameters::Ptr& params) { + if (linear_constraint.end_frame_id.empty()) { + return std::vector(); + } + if (linear_constraint.axis.isZero()) { + RCLCPP_ERROR_THROTTLE(logger_, *clock_, 1000, "GoalRelativeLinearConstraint: axis is zero"); + return std::nullopt; + } + if (linear_constraint.distance < std::numeric_limits::epsilon()) { + RCLCPP_ERROR_THROTTLE(logger_, *clock_, 1000, "GoalRelativeLinearConstraint: distance is zero or negative"); + return std::nullopt; + } + // The only check for the frame name is this method, and it is a little computable. + try { + robot_->GetObjectTransform(linear_constraint.end_frame_id); + } catch (const std::domain_error& e) { + RCLCPP_ERROR_THROTTLE(logger_, *clock_, 1000, "GoalRelativeLinearConstraint: %s", e.what()); + return std::nullopt; + } + + // The 0th fixing is not good, but I don't think about correspondence because the whole Generator is such an implementation. + robot_->SetNamedAngle(initial_state.joint_state); + robot_->SetRobotTransform(initial_state.multi_dof_joint_state.poses[0]); + const auto origin_to_initial = robot_->GetObjectTransform(linear_constraint.end_frame_id); + + robot_->SetNamedAngle(goal_state.joint_state); + robot_->SetRobotTransform(goal_state.multi_dof_joint_state.poses[0]); + const auto origin_to_goal = robot_->GetObjectTransform(linear_constraint.end_frame_id); + + const auto axis = linear_constraint.axis.normalized(); + + std::vector distances; + for (auto distance = linear_step_; distance < linear_constraint.distance; distance += linear_step_) { + distances.push_back(distance); + } + distances.push_back(linear_constraint.distance); + + // No Goal_state is required later, but add it to the implementation of the previous solution to the initial value of IK. + std::vector middle_states_out = {goal_state}; + for (const auto distance : distances) { + const auto goal_to_waypoint = Eigen::Translation3d(distance * axis) * Eigen::AngleAxisd::Identity(); + const auto origin_to_waypoint = origin_to_goal * goal_to_waypoint; + + // If there is a initial position position near the straight line, discontinue the calculation there. + // Strictly speaking, the distance between the dot and the line is that it should be at an appropriate distance proportional to STEP. + const auto distance_from_initial = (origin_to_waypoint.translation() - origin_to_initial.translation()).norm(); + if (distance_from_initial < kStepThresholdRate * linear_step_) { + break; + } + + tmc_manipulation_types::TaskSpaceRegion tsr(origin_to_waypoint, + Eigen::Affine3d::Identity(), + tmc_manipulation_types::RegionValues::Zero(), + tmc_manipulation_types::RegionValues::Zero(), + "", linear_constraint.end_frame_id); + const auto constraint = std::make_shared(tsr, 0); + + tmc_manipulation_types::RobotState waypoint_state; + if (!sampler_->SampleFromLinkConstraints(middle_states_out.back(), {constraint}, params, waypoint_state)) { + // If IK cannot solve it, think that it is worse to return a failure, or throw a command value that can not solve the IK. + return std::nullopt; + } + middle_states_out.push_back(waypoint_state); + } + + // Correct the order and delete the Goal_state added for calculation + std::reverse(middle_states_out.begin(), middle_states_out.end()); + middle_states_out.pop_back(); + + return middle_states_out; +} + +} // namespace tmc_simple_path_generator diff --git a/tmc_simple_path_generator/src/tmc_simple_path_generator/node_main.cpp b/tmc_simple_path_generator/src/tmc_simple_path_generator/node_main.cpp new file mode 100644 index 0000000..9e42179 --- /dev/null +++ b/tmc_simple_path_generator/src/tmc_simple_path_generator/node_main.cpp @@ -0,0 +1,39 @@ +/* +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. +*/ +/// @brief main of simple path generator +#include +#include + +int main(int argc, char* argv[]) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + node->Initialize(); + rclcpp::spin(node); + rclcpp::shutdown(); + return EXIT_SUCCESS; +} diff --git a/tmc_simple_path_generator/src/tmc_simple_path_generator/planar_trajectory_interpolator.cpp b/tmc_simple_path_generator/src/tmc_simple_path_generator/planar_trajectory_interpolator.cpp new file mode 100644 index 0000000..d7f0810 --- /dev/null +++ b/tmc_simple_path_generator/src/tmc_simple_path_generator/planar_trajectory_interpolator.cpp @@ -0,0 +1,339 @@ +/* +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. +*/ +/// @brief Planar Trajectory Interpolator Class + +#include + +#include +#include + +#include +#include + +#include + +#include + +#include "calculator.hpp" +#include "common.hpp" + +namespace { + +// Speed ​​is required only for Initial +bool CheckRobotInitialState(const tmc_manipulation_types::RobotState& robot_state) { + if (robot_state.joint_state.name.size() == 0 || robot_state.multi_dof_joint_state.names.size() == 0) { + return false; + } + if (robot_state.joint_state.name.size() != robot_state.joint_state.position.size() || + robot_state.joint_state.name.size() != robot_state.joint_state.velocity.size()) { + return false; + } + if (robot_state.multi_dof_joint_state.names.size() != robot_state.multi_dof_joint_state.poses.size() || + robot_state.multi_dof_joint_state.names.size() != robot_state.multi_dof_joint_state.twist.size()) { + return false; + } + return true; +} + +bool CheckRobotState(const tmc_manipulation_types::RobotState& robot_state) { + if (robot_state.joint_state.name.size() == 0 || robot_state.multi_dof_joint_state.names.size() == 0) { + return false; + } + if (robot_state.joint_state.name.size() != robot_state.joint_state.position.size()) { + return false; + } + if (robot_state.multi_dof_joint_state.names.size() != robot_state.multi_dof_joint_state.poses.size()) { + return false; + } + return true; +} + +} // namespace + +namespace tmc_simple_path_generator { + +void PlanarTrajectoryInterpolatorBase::Initialize(const rclcpp::Node::SharedPtr& node, + const SamplingParameters::Ptr& params) { + sampling_params_ = params; +} + +// Orbit an interpolation that satisfies the restraint in the shortest time +bool PlanarTrajectoryInterpolatorBase::Interpolate( + const tmc_manipulation_types::RobotState& initial_state, + const tmc_manipulation_types::RobotState& goal_state, + double normalized_vel, + tmc_manipulation_types::TimedRobotTrajectory& trajectory_out) { + return Interpolate(initial_state, {}, goal_state, normalized_vel, trajectory_out); +} + +// Calonia interpolation to configure via points +bool PlanarTrajectoryInterpolatorBase::Interpolate( + const tmc_manipulation_types::RobotState& initial_state, + const tmc_manipulation_types::RobotState& goal_state, + double normalized_vel, + const tmc_manipulation_types::RobotState& random_middle_state, + tmc_manipulation_types::TimedRobotTrajectory& trajectory_out) { + return Interpolate(initial_state, {}, goal_state, normalized_vel, random_middle_state, trajectory_out); +} + +// Orbit an interpolation that satisfies the restraint in the shortest time +bool PlanarTrajectoryInterpolatorBase::Interpolate( + const tmc_manipulation_types::RobotState& initial_state, + const std::vector& middle_states_near_goal, + const tmc_manipulation_types::RobotState& goal_state, + double normalized_vel, + tmc_manipulation_types::TimedRobotTrajectory& trajectory_out) { + if (!CheckRobotInitialState(initial_state) || !CheckRobotState(goal_state)) { + return false; + } + for (const auto& middle_state : middle_states_near_goal) { + if (!CheckRobotState(middle_state)) { + return false; + } + } + return InterpolateTrajectory(initial_state, middle_states_near_goal, goal_state, normalized_vel, trajectory_out); +} + +// Calonia interpolation to configure via points +bool PlanarTrajectoryInterpolatorBase::Interpolate( + const tmc_manipulation_types::RobotState& initial_state, + const std::vector& middle_states_near_goal, + const tmc_manipulation_types::RobotState& goal_state, + double normalized_vel, + const tmc_manipulation_types::RobotState& random_middle_state, + tmc_manipulation_types::TimedRobotTrajectory& trajectory_out) { + if (!CheckRobotInitialState(initial_state) || + !CheckRobotState(goal_state) || + !CheckRobotState(random_middle_state)) { + return false; + } + for (const auto& middle_state : middle_states_near_goal) { + if (!CheckRobotState(middle_state)) { + return false; + } + } + return InterpolateTrajectory(initial_state, middle_states_near_goal, goal_state, normalized_vel, random_middle_state, + trajectory_out); +} + +void PlanarTrajectoryInterpolator::Initialize(const rclcpp::Node::SharedPtr& node, + const SamplingParameters::Ptr& params) { + PlanarTrajectoryInterpolatorBase::Initialize(node, params); + + max_sampling_trajectory_ = std::make_shared>( + node, "max_sampling_trajectory", 30); + time_stamp_ = std::make_shared>(node, "time_stamp", 0.1); + + set_param_handlers_.emplace_back(node->add_on_set_parameters_callback( + std::bind(&tmc_utils::DynamicParameter::SetParameterCallback, + max_sampling_trajectory_, std::placeholders::_1))); + set_param_handlers_.emplace_back(node->add_on_set_parameters_callback( + std::bind(&tmc_utils::DynamicParameter::SetParameterCallback, time_stamp_, std::placeholders::_1))); +} + +// spline interpolator +// Middle_states_near_goal Spline interpolation is not implemented +bool PlanarTrajectoryInterpolator::InterpolateTrajectory( + const tmc_manipulation_types::RobotState& initial_state, + __attribute__((unused)) const std::vector& middle_states_near_goal, + const tmc_manipulation_types::RobotState& goal_state, + double normalized_vel, + tmc_manipulation_types::TimedRobotTrajectory& trajectory_out) { + if (normalized_vel < std::numeric_limits::epsilon()) { + CONSOLE_BRIDGE_logError("normalized_vel have to be positive real."); + return false; + } + auto joint_length = CalculateJointLength(initial_state, goal_state, sampling_params_->joint_weights); + + auto base_x0 = tmc_robot_local_planner_utils::Get2DPose(initial_state.multi_dof_joint_state.poses[0]); + Eigen::Vector3d base_v0(initial_state.multi_dof_joint_state.twist[0](0), + initial_state.multi_dof_joint_state.twist[0](1), + initial_state.multi_dof_joint_state.twist[0](5)); + auto base_x1 = tmc_robot_local_planner_utils::Get2DPose(goal_state.multi_dof_joint_state.poses[0]); + base_x1[2] = base_x0[2] + angles::shortest_angular_distance(base_x0[2], base_x1[2]); + auto base_length = CalculateLength(base_x0, base_x1, sampling_params_->base_weights); + + auto length = std::max(joint_length, base_length); + if (length <= std::numeric_limits::epsilon()) { + return false; + } + auto end_time = length / normalized_vel; + + auto joint_coeff = CalcPoly(initial_state.joint_state.position, + initial_state.joint_state.velocity, + goal_state.joint_state.position, + end_time); + auto base_coeff = CalcPoly(base_x0, base_v0, base_x1, end_time); + + InterpolateTrajectoryImpl(initial_state.joint_state.name, + initial_state.multi_dof_joint_state.names, + joint_coeff, base_coeff, end_time, + trajectory_out); + return true; +} + +// spline interpolator +bool PlanarTrajectoryInterpolator::InterpolateTrajectory( + const tmc_manipulation_types::RobotState& initial_state, + __attribute__((unused)) const std::vector& middle_states_near_goal, + const tmc_manipulation_types::RobotState& goal_state, + double normalized_vel, + const tmc_manipulation_types::RobotState& middle_state, + tmc_manipulation_types::TimedRobotTrajectory& trajectory_out) { + if (normalized_vel < std::numeric_limits::epsilon()) { + CONSOLE_BRIDGE_logError("normalized_vel have to be positive real."); + return false; + } + auto joint_length = CalculateJointLength(initial_state, middle_state, sampling_params_->joint_weights) + + CalculateJointLength(middle_state, goal_state, sampling_params_->joint_weights); + + auto base_x0 = tmc_robot_local_planner_utils::Get2DPose(initial_state.multi_dof_joint_state.poses[0]); + Eigen::Vector3d base_v0(initial_state.multi_dof_joint_state.twist[0](0), + initial_state.multi_dof_joint_state.twist[0](1), + initial_state.multi_dof_joint_state.twist[0](5)); + auto base_x1 = tmc_robot_local_planner_utils::Get2DPose(goal_state.multi_dof_joint_state.poses[0]); + base_x1[2] = base_x0[2] + angles::shortest_angular_distance(base_x0[2], base_x1[2]); + auto base_xa = tmc_robot_local_planner_utils::Get2DPose(middle_state.multi_dof_joint_state.poses[0]); + base_xa[2] = base_x1[2] + angles::shortest_angular_distance(base_x1[2], base_xa[2]); + auto base_length = CalculateLength(base_x0, base_x1, sampling_params_->base_weights) + + CalculateLength(base_x1, base_xa, sampling_params_->base_weights); + + auto length = std::max(joint_length, base_length); + if (length <= std::numeric_limits::epsilon()) { + return false; + } + auto end_time = length / normalized_vel; + + auto joint_coeff = CalcPolyViaPoint(initial_state.joint_state.position, + initial_state.joint_state.velocity, + goal_state.joint_state.position, + middle_state.joint_state.position, + end_time); + auto base_coeff = CalcPolyViaPoint(base_x0, base_v0, base_x1, base_xa, end_time); + + InterpolateTrajectoryImpl(initial_state.joint_state.name, + initial_state.multi_dof_joint_state.names, + joint_coeff, base_coeff, end_time, + trajectory_out); + return true; +} + +// Generate an interpolation orbital +void PlanarTrajectoryInterpolator::InterpolateTrajectoryImpl( + const tmc_manipulation_types::NameSeq& joint_names, + const tmc_manipulation_types::NameSeq& base_names, + const std::vector& joint_coeff, + const std::vector& base_coeff, + double end_time, + tmc_manipulation_types::TimedRobotTrajectory& trajectory_out) { + auto desired_steps = static_cast(ceil(end_time / time_stamp_->value())); + std::vector time_from_starts; + if (desired_steps >= max_sampling_trajectory_->value()) { + // Finely chop it halfway, and the rest is widened + time_from_starts.reserve(max_sampling_trajectory_->value()); + for (uint32_t i = 0; i < max_sampling_trajectory_->value() / 2; ++i) { + time_from_starts.push_back(static_cast(i) * time_stamp_->value()); + } + auto remainings = max_sampling_trajectory_->value() - time_from_starts.size(); + auto coeff_b = time_from_starts.back(); + auto coeff_a = (end_time - coeff_b) / remainings; + for (uint32_t i = 0; i < remainings; ++i) { + time_from_starts.push_back(static_cast(i + 1) * coeff_a + coeff_b); + } + } else { + time_from_starts.reserve(desired_steps + 1); + for (uint32_t i = 0; i < desired_steps; ++i) { + time_from_starts.push_back(static_cast(i) * time_stamp_->value()); + } + time_from_starts.push_back(end_time); + } + auto actual_steps = std::min(desired_steps + 1, max_sampling_trajectory_->value()); + tmc_manipulation_types::TimedJointTrajectory joint_trajectory; + joint_trajectory.joint_names = joint_names; + joint_trajectory.points.resize(actual_steps); + tmc_manipulation_types::TimedMultiDOFJointTrajectory multi_dof_joint_trajectory; + multi_dof_joint_trajectory.joint_names = base_names; + multi_dof_joint_trajectory.points.resize(actual_steps); + for (const auto& time_from_start : time_from_starts | boost::adaptors::indexed()) { + auto index = time_from_start.index(); + joint_trajectory.points[index].positions = PolyViaPoint(joint_coeff, time_from_start.value()); + joint_trajectory.points[index].velocities = VelViaPoint(joint_coeff, time_from_start.value()); + joint_trajectory.points[index].accelerations = AccViaPoint(joint_coeff, time_from_start.value()); + joint_trajectory.points[index].time_from_start = time_from_start.value(); + multi_dof_joint_trajectory.points[index].transforms.push_back( + tmc_robot_local_planner_utils::GetTransform(PolyViaPoint(base_coeff, time_from_start.value()))); + multi_dof_joint_trajectory.points[index].velocities.push_back( + tmc_robot_local_planner_utils::GetTwist(VelViaPoint(base_coeff, time_from_start.value()))); + multi_dof_joint_trajectory.points[index].accelerations.push_back( + tmc_robot_local_planner_utils::GetTwist(AccViaPoint(base_coeff, time_from_start.value()))); + multi_dof_joint_trajectory.points[index].time_from_start = time_from_start.value(); + } + trajectory_out.joint_trajectory = joint_trajectory; + trajectory_out.multi_dof_joint_trajectory = multi_dof_joint_trajectory; +} + + +bool PlanarTrajectoryInterpolatorPlain::InterpolateTrajectory( + const tmc_manipulation_types::RobotState& initial_state, + const std::vector& middle_states_near_goal, + const tmc_manipulation_types::RobotState& goal_state, + double normalized_vel, + tmc_manipulation_types::TimedRobotTrajectory& trajectory_out) { + // The copy is wasteful, but it doesn't mean that you can rewrite the input. + auto middle_states = middle_states_near_goal; + middle_states.push_back(goal_state); + + Convert(initial_state, middle_states, + sampling_params_->joint_weights, sampling_params_->base_weights, trajectory_out); + return true; +} + +bool PlanarTrajectoryInterpolatorPlain::InterpolateTrajectory( + const tmc_manipulation_types::RobotState& initial_state, + const std::vector& middle_states_near_goal, + const tmc_manipulation_types::RobotState& goal_state, + double normalized_vel, + const tmc_manipulation_types::RobotState& middle_state, + tmc_manipulation_types::TimedRobotTrajectory& trajectory_out) { + auto middle_states = middle_states_near_goal; + middle_states.insert(middle_states.begin(), middle_state); + middle_states.push_back(goal_state); + + Convert(initial_state, middle_states, + sampling_params_->joint_weights, sampling_params_->base_weights, trajectory_out); + return true; +} + +} // namespace tmc_simple_path_generator + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(tmc_simple_path_generator::PlanarTrajectoryInterpolator, + tmc_simple_path_generator::ITrajectoryInterpolator); +PLUGINLIB_EXPORT_CLASS(tmc_simple_path_generator::PlanarTrajectoryInterpolatorPlain, + tmc_simple_path_generator::ITrajectoryInterpolator); diff --git a/tmc_simple_path_generator/src/tmc_simple_path_generator/sample_goal_generator.cpp b/tmc_simple_path_generator/src/tmc_simple_path_generator/sample_goal_generator.cpp new file mode 100644 index 0000000..495f57f --- /dev/null +++ b/tmc_simple_path_generator/src/tmc_simple_path_generator/sample_goal_generator.cpp @@ -0,0 +1,375 @@ +/* +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. +*/ +/// @brief Sample Goal Generator Class + +#include + +#include +#include + +#include + +#include +#include + +#include "common.hpp" + +namespace { + +// Size check +bool CheckRobotState(const tmc_manipulation_types::RobotState& robot_state) { + if (robot_state.joint_state.name.size() != robot_state.joint_state.position.size()) { + return false; + } + if (robot_state.multi_dof_joint_state.names.size() != robot_state.multi_dof_joint_state.poses.size()) { + return false; + } + return true; +} + +tmc_robot_kinematics_model::IKRequest GenerateIKRequest( + const Eigen::Affine3d& origin_to_end, + const tmc_manipulation_types::RobotState& initial_state, + const tmc_simple_path_generator::SamplingParameters::Ptr& params, + const std::string& end_frame) { + tmc_robot_kinematics_model::IKRequest req( + static_cast(params->base_movement_type)); + req.frame_name = end_frame; + req.frame_to_end = Eigen::Affine3d::Identity(); + req.initial_angle.name = initial_state.joint_state.name; + req.initial_angle.position = initial_state.joint_state.position; + req.use_joints = params->joint_names; + req.weight.resize(params->joint_weights.size() + params->base_weights.size()); + req.weight << params->joint_weights, params->base_weights; + req.origin_to_base = initial_state.multi_dof_joint_state.poses[0]; + req.ref_origin_to_end = origin_to_end; + return req; +} + +} // namespace + +namespace tmc_simple_path_generator { + +// constructor +SampleGoalGeneratorBase::SampleGoalGeneratorBase() + : ik_solver_loader_("tmc_robot_kinematics_model", "tmc_robot_kinematics_model::IKSolver"), + is_from_random_initial_state_(true) {} + +// Initialize to fix the seed +void SampleGoalGeneratorBase::Initialize(const rclcpp::Node::SharedPtr& node, uint32_t random_seed) { + engine_.seed(random_seed); + + ik_initial_range_ = std::make_shared>(node, "ik_initial_range", 0.5); + set_param_handlers_.emplace_back(node->add_on_set_parameters_callback( + std::bind(&tmc_utils::DynamicParameter::SetParameterCallback, ik_initial_range_, std::placeholders::_1))); + + InitializeImpl(node); +} + +// Initialize to generate seed randomly +void SampleGoalGeneratorBase::Initialize(const rclcpp::Node::SharedPtr& node) { + Initialize(node, std::random_device()()); +} + +// Sampling the goal posture from joint restraint +bool SampleGoalGeneratorBase::SampleFromJointConstraints( + const std::vector& joint_constraints, + tmc_manipulation_types::RobotState& goal_state_out) { + if (!CheckRobotState(goal_state_out)) { + return false; + } + if (joint_constraints.size() == 0) { + return false; + } + auto choosen_constraint = SampleConstraints(joint_constraints, engine_); + auto constraint_state = choosen_constraint->Sample(); + return OverwriteRobotState(goal_state_out, + constraint_state.joint_state.name, + constraint_state.multi_dof_joint_state.names, + constraint_state.joint_state.position, + constraint_state.multi_dof_joint_state.poses); +} + +// Sampling the goal posture from the posture restraint +bool SampleGoalGeneratorBase::SampleFromLinkConstraints( + const tmc_manipulation_types::RobotState& initial_state, + const std::vector& link_constraints, + const SamplingParameters::Ptr& params, + tmc_manipulation_types::RobotState& goal_state_out) { + if (!CheckRobotState(initial_state)) { + return false; + } + if (link_constraints.size() == 0) { + return false; + } + + tmc_manipulation_types::RobotState varianced_initial_state; + if (is_from_random_initial_state_) { + if (!AddRandomState(initial_state, params->joint_names, params->base_names, params->joint_limit_upper, + params->joint_limit_lower, ik_initial_range_->value(), varianced_initial_state)) { + return false; + } + } else { + varianced_initial_state = initial_state; + } + + auto choosen_constraint = SampleConstraints(link_constraints, engine_); + auto origin_to_ref = choosen_constraint->Sample(); + return SolveIK(origin_to_ref, varianced_initial_state, params, + choosen_constraint->GetLinkName(), goal_state_out); +} + + +// Implementation of initialization +void SampleGoalGenerator::InitializeImpl(const rclcpp::Node::SharedPtr& node) { + const auto solver_type = tmc_utils::GetParameter( + node, "solver_type", "tmc_robot_kinematics_model/NumericIKSolver"); + ik_solver_ = ik_solver_loader_.createSharedInstance(solver_type); + + auto robot_description = tmc_utils::GetParameter(node, "robot_description_kinematics", ""); + if (robot_description.empty()) { + robot_description = tmc_utils::GetParameter(node, "robot_description", ""); + } + ik_solver_->set_robot_description(robot_description); +} + +// Find ROBOT_STATE to realize the terminal posture origin_to_end +bool SampleGoalGenerator::SolveIK(const Eigen::Affine3d& origin_to_end, + const tmc_manipulation_types::RobotState& initial_state, + const SamplingParameters::Ptr& params, + const std::string& end_frame, + tmc_manipulation_types::RobotState& solution_out) { + const auto req = GenerateIKRequest(origin_to_end, initial_state, params, end_frame); + + tmc_manipulation_types::JointState res; + Eigen::Affine3d origin_to_hand_result; + Eigen::Affine3d origin_to_base_solution; + tmc_robot_kinematics_model::IKResult result; + try { + result = ik_solver_->Solve(req, res, origin_to_base_solution, origin_to_hand_result); + } catch (std::exception& e) { + CONSOLE_BRIDGE_logWarn("%s", e.what()); + return false; + } + if (result == tmc_robot_kinematics_model::kSuccess) { + solution_out = initial_state; + if (OverwriteRobotState(solution_out, params->joint_names, params->base_names, res.position, + tmc_manipulation_types::PoseSeq(params->base_names.size(), origin_to_base_solution))) { + CONSOLE_BRIDGE_logDebug("IK success"); + return true; + } else { + CONSOLE_BRIDGE_logWarn("IK Failed"); + return false; + } + } else { + CONSOLE_BRIDGE_logDebug("IK Failed %d", result); + return false; + } +} + + +// constructor +SampleGoalGeneratorMultiThread::SampleGoalGeneratorMultiThread() + : stop_thread_(false), stop_ik_(false), num_of_running_ik_(0) {} + +// Destructor +SampleGoalGeneratorMultiThread::~SampleGoalGeneratorMultiThread() { + { + std::unique_lock lock(request_queue_mutex_); + stop_thread_ = true; + } + condition_.notify_all(); + for (auto& worker : workers_) { + worker.join(); + } +} + +// Treatment at the end +void SampleGoalGeneratorMultiThread::Terminate() { + SampleGoalGeneratorBase::Terminate(); + + { + std::unique_lock lock(request_queue_mutex_); + std::queue().swap(request_queue_); + } + { + std::unique_lock lock(stop_ik_mutex_); + stop_ik_ = true; + } + while (true) { + std::unique_lock lock(num_of_running_ik_mutex_, std::try_to_lock); + if (lock && (num_of_running_ik_ == 0)) break; + } + { + std::unique_lock lock(response_queue_mutex_); + std::queue().swap(response_queue_); + } + { + std::unique_lock lock(stop_ik_mutex_); + stop_ik_ = false; + } +} + +// Implementation of initialization +void SampleGoalGeneratorMultiThread::InitializeImpl(const rclcpp::Node::SharedPtr& node) { + int32_t thread_pool_size = tmc_utils::GetParameter(node, "thread_pool_size", 8); + if (thread_pool_size <= 0) { + throw std::domain_error("thread_pool_size must be positive."); + } + + const auto solver_type = tmc_utils::GetParameter( + node, "solver_type", "tmc_robot_kinematics_model/NumericIKSolver"); + + auto robot_description = tmc_utils::GetParameter(node, "robot_description_kinematics", ""); + if (robot_description.empty()) { + robot_description = tmc_utils::GetParameter(node, "robot_description", ""); + } + + for (auto i = 0; i < thread_pool_size; ++i) { + workers_.emplace_back( + std::thread(std::bind(&SampleGoalGeneratorMultiThread::SolveIkThread, this, solver_type, robot_description))); + } +} + +// Find ROBOT_STATE to realize the terminal posture origin_to_end +bool SampleGoalGeneratorMultiThread::SolveIK(const Eigen::Affine3d& origin_to_end, + const tmc_manipulation_types::RobotState& initial_state, + const SamplingParameters::Ptr& params, + const std::string& end_frame, + tmc_manipulation_types::RobotState& solution_out) { + Request req; + req.request = GenerateIKRequest(origin_to_end, initial_state, params, end_frame); + req.initial_state = initial_state; + + // Pack REQUEST in a queue + bool add_request_queue = false; + { + std::unique_lock lock(request_queue_mutex_); + // Even if you accumulate too much queue, you only need to use memory for waste, so it will be twice as much as the number of threads. + if (request_queue_.size() < workers_.size() * 2) { + request_queue_.emplace(req); + add_request_queue = true; + } + } + if (add_request_queue) condition_.notify_one(); + + // Receive if you have Response + Response res; + { + std::unique_lock lock(response_queue_mutex_); + if (response_queue_.empty()) { + return false; + } else { + res = std::move(response_queue_.front()); + response_queue_.pop(); + } + } + + // Response judgment/conversion + if (res.result != tmc_robot_kinematics_model::kSuccess) { + CONSOLE_BRIDGE_logDebug("IK Failed %d", res.result); + return false; + } + + solution_out = res.initial_state; + // You can think that params will not change while you are solving IK + if (OverwriteRobotState(solution_out, + params->joint_names, params->base_names, + res.joint_solution.position, + tmc_manipulation_types::PoseSeq(params->base_names.size(), res.origin_to_base))) { + CONSOLE_BRIDGE_logDebug("IK success"); + return true; + } else { + CONSOLE_BRIDGE_logWarn("IK Failed"); + return false; + } +} + +void SampleGoalGeneratorMultiThread::SolveIkThread(const std::string& solver_type, + const std::string& robot_description) { + tmc_robot_kinematics_model::IKSolver::Ptr ik_solver; + { + std::unique_lock lock(request_queue_mutex_); + ik_solver = ik_solver_loader_.createSharedInstance(solver_type); + ik_solver->set_robot_description(robot_description); + } + + std::function interrupt = [this]() -> bool{ + std::unique_lock lock(this->stop_ik_mutex_, std::try_to_lock); + if (lock) { + return this->stop_ik_; + } else { + return false; + } + }; + + Request req; + for (;;) { + { + std::unique_lock lock(request_queue_mutex_); + condition_.wait(lock, [this]{ return this->stop_thread_ || !this->request_queue_.empty(); }); + if (stop_thread_ && request_queue_.empty()) { + return; + } + req = std::move(request_queue_.front()); + request_queue_.pop(); + } + Eigen::Affine3d origin_to_hand_result; + Response res; + res.initial_state = req.initial_state; + { + std::unique_lock lock(num_of_running_ik_mutex_); + ++num_of_running_ik_; + } + try { + res.result = ik_solver->Solve( + req.request, interrupt, res.joint_solution, res.origin_to_base, origin_to_hand_result); + } catch (std::exception& e) { + CONSOLE_BRIDGE_logWarn("%s", e.what()); + res.result = tmc_robot_kinematics_model::kFail; + } + { + std::unique_lock lock(response_queue_mutex_); + response_queue_.push(std::move(res)); + } + { + std::unique_lock lock(num_of_running_ik_mutex_); + --num_of_running_ik_; + } + } +} + +} // namespace tmc_simple_path_generator + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(tmc_simple_path_generator::SampleGoalGenerator, + tmc_simple_path_generator::SampleGoalGeneratorBase); + +PLUGINLIB_EXPORT_CLASS(tmc_simple_path_generator::SampleGoalGeneratorMultiThread, + tmc_simple_path_generator::SampleGoalGeneratorBase); diff --git a/tmc_simple_path_generator/src/tmc_simple_path_generator/sample_middle_generator.cpp b/tmc_simple_path_generator/src/tmc_simple_path_generator/sample_middle_generator.cpp new file mode 100644 index 0000000..e169d54 --- /dev/null +++ b/tmc_simple_path_generator/src/tmc_simple_path_generator/sample_middle_generator.cpp @@ -0,0 +1,115 @@ +/* +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. +*/ +#include + +#include + +#include + +#include "common.hpp" + +namespace tmc_simple_path_generator { + +void PlanerMiddleStateGenerator::Initialize(const rclcpp::Node::SharedPtr& node, + const SamplingParameters::Ptr& params) { + sampling_params_ = params; + + middle_state_base_position_range_ = std::make_shared>( + node, "middle_state_base_position_range", 0.5); + middle_state_base_rotation_range_ = std::make_shared>( + node, "middle_state_base_rotation_range", 0.05); + + set_param_handlers_.emplace_back(node->add_on_set_parameters_callback( + std::bind(&tmc_utils::DynamicParameter::SetParameterCallback, + middle_state_base_position_range_, std::placeholders::_1))); + set_param_handlers_.emplace_back(node->add_on_set_parameters_callback( + std::bind(&tmc_utils::DynamicParameter::SetParameterCallback, + middle_state_base_rotation_range_, std::placeholders::_1))); +} + +std::optional PlanerMiddleStateGenerator::ComputeRandomMiddleState( + const tmc_manipulation_types::RobotState& initial_state, + const tmc_manipulation_types::RobotState& goal_state, + const SamplingParameters::Ptr& params) { + tmc_manipulation_types::RobotState middle_state; + if (!ComputeMiddleState(initial_state, goal_state, middle_state)) { + return std::nullopt; + } + // I want to reflect the joints that are not used here, so use Params given by arguments. + if (!ReplaceWithUniformRandomJointPositions(middle_state, params->joint_names, params->joint_limit_upper, + params->joint_limit_lower, middle_state)) { + return std::nullopt; + } + if (!params->base_names.empty()) { + if (!AddUniformRandomBasePoseOffset(middle_state, params->base_names, + middle_state_base_position_range_->value(), + middle_state_base_rotation_range_->value(), middle_state)) { + return std::nullopt; + } + } + return middle_state; +} + +bool PlanerMiddleStateGenerator::ComputeMiddleState( + const tmc_manipulation_types::RobotState& initial_state, + const tmc_manipulation_types::RobotState& goal_state, + tmc_manipulation_types::RobotState& middle_state_out) { + Eigen::VectorXd init_joint_positions; + tmc_manipulation_types::PoseSeq init_base_poses; + // Here, we want to target all joints, so use sampling_params_ + if (!ExtractFromRobotState(initial_state, sampling_params_->joint_names, sampling_params_->base_names, + init_joint_positions, init_base_poses)) { + return false; + } + Eigen::VectorXd end_joint_positions; + tmc_manipulation_types::PoseSeq end_base_poses; + if (!ExtractFromRobotState(goal_state, sampling_params_->joint_names, sampling_params_->base_names, + end_joint_positions, end_base_poses)) { + return false; + } + auto middle_joint_positions = (init_joint_positions + end_joint_positions) / 2.0; + + auto init_2d_pose = tmc_robot_local_planner_utils::Get2DPose(init_base_poses[0]); + auto end_2d_pose = tmc_robot_local_planner_utils::Get2DPose(end_base_poses[0]); + auto middle_yaw = angles::normalize_angle( + init_2d_pose[2] + angles::shortest_angular_distance(init_2d_pose[2], + end_2d_pose[2]) / 2.0); + Eigen::Vector3d middle_2d_pose((init_2d_pose[0] + end_2d_pose[0]) / 2.0, + (init_2d_pose[1] + end_2d_pose[1]) / 2.0, + middle_yaw); + auto middle_base_pose = tmc_robot_local_planner_utils::GetTransform(middle_2d_pose); + middle_state_out = initial_state; + return OverwriteRobotState(middle_state_out, sampling_params_->joint_names, sampling_params_->base_names, + middle_joint_positions, {middle_base_pose}); +} +} // namespace tmc_simple_path_generator + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(tmc_simple_path_generator::PlanerMiddleStateGenerator, + tmc_simple_path_generator::ISampleMiddleGenerator); diff --git a/tmc_simple_path_generator/src/tmc_simple_path_generator/sampling_parameters.cpp b/tmc_simple_path_generator/src/tmc_simple_path_generator/sampling_parameters.cpp new file mode 100644 index 0000000..dc76ded --- /dev/null +++ b/tmc_simple_path_generator/src/tmc_simple_path_generator/sampling_parameters.cpp @@ -0,0 +1,121 @@ +/* +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. +*/ +/// @brief Parameters for track generation +#include + +#include +#include +#include +#include + +#include + +#include + +#include "common.hpp" + +namespace { +template +void GetMapToKeyValue(const T& map_in, U& keys, V& values) { + for (auto it = map_in.begin(); it != map_in.end(); ++it) { + keys.push_back(it->first); + values.push_back(it->second); + } +} +} // namespace + +namespace tmc_simple_path_generator { + +SamplingParameters::SamplingParameters(const rclcpp::Node::SharedPtr& node) { + this->joint_names = tmc_utils::GetParameter>(node, "joint_names", {}); + if (this->joint_names.empty()) { + RCLCPP_FATAL(node->get_logger(), "Parameter joint_names isn't set."); + exit(EXIT_FAILURE); + } + // If you set it to CONST, the MAP will not pass, so it is not a nonST + auto joint_weights_vec = tmc_utils::GetParameter>(node, "joint_weights", {}); + this->joint_weights.resize(joint_weights_vec.size()); + this->joint_weights = Eigen::Map(&joint_weights_vec[0], joint_weights_vec.size()); + + this->base_names = tmc_utils::GetParameter>(node, "base_names", {}); + if (this->base_names.empty()) { + RCLCPP_FATAL(node->get_logger(), "Parameter base_names isn't set."); + exit(EXIT_FAILURE); + } + auto base_weights_vec = tmc_utils::GetParameter>(node, "base_weights", {}); + this->base_weights.resize(base_weights_vec.size()); + this->base_weights = Eigen::Map(&base_weights_vec[0], base_weights_vec.size()); + + const auto urdf_str = tmc_utils::GetParameter(node, "robot_description", ""); + auto urdf = urdf::Model(); + if (!urdf.initString(urdf_str)) { + RCLCPP_FATAL(node->get_logger(), "Failed to parse URDF contained in robot_description"); + exit(EXIT_FAILURE); + } + for (const auto& joint_name : this->joint_names) { + if (urdf.getJoint(joint_name)) { + auto urdf_joint_limits = urdf.getJoint(joint_name)->limits; + this->joint_limit_upper.push_back(urdf_joint_limits->upper); + this->joint_limit_lower.push_back(urdf_joint_limits->lower); + } else { + RCLCPP_FATAL(node->get_logger(), "No joint position limit information in robot_description"); + exit(EXIT_FAILURE); + } + } + + int base_movement_type_int = tmc_utils::GetParameter(node, "base_movement_type", -1); + if (base_movement_type_int < 0) { + RCLCPP_FATAL(node->get_logger(), "Parameter base_movement_type isn't set"); + exit(EXIT_FAILURE); + } + this->base_movement_type = (tmc_manipulation_types::BaseMovementType)base_movement_type_int; +} + +SamplingParameters::Ptr SamplingParameters::RemoveJoints(const std::vector& ignore_joints) const { + auto new_params = std::make_shared(); + + std::vector used_joint_name_indices; + RemoveIgnored(this->joint_names, ignore_joints, new_params->joint_names, used_joint_name_indices); + + new_params->joint_weights = ExtractValues(this->joint_weights, used_joint_name_indices); + new_params->joint_limit_upper = ExtractValues(this->joint_limit_upper, used_joint_name_indices); + new_params->joint_limit_lower = ExtractValues(this->joint_limit_lower, used_joint_name_indices); + + RemoveIgnored(this->base_names, ignore_joints, new_params->base_names); + if (new_params->base_names.empty()) { + new_params->base_weights.resize(0); + new_params->base_movement_type = tmc_manipulation_types::kNone; + } else { + new_params->base_weights = this->base_weights; + new_params->base_movement_type = this->base_movement_type; + } + return new_params; +} + +} // namespace tmc_simple_path_generator + diff --git a/tmc_simple_path_generator/src/tmc_simple_path_generator/simple_path_generator.cpp b/tmc_simple_path_generator/src/tmc_simple_path_generator/simple_path_generator.cpp new file mode 100644 index 0000000..b673812 --- /dev/null +++ b/tmc_simple_path_generator/src/tmc_simple_path_generator/simple_path_generator.cpp @@ -0,0 +1,298 @@ +/* +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. +*/ +/// @brief Simple Path Generator Class + +#include + +#include + +#include +#include + +namespace { +// Coordinate conversion timeout +const double kTFTimeout = 0.05; +// Origin frame +const char* const kOriginFrame = "odom"; + +bool CheckRobotState(const moveit_msgs::msg::RobotState robot_state) { + if (robot_state.joint_state.name.size() != robot_state.joint_state.position.size() || + robot_state.joint_state.name.size() != robot_state.joint_state.velocity.size()) { + return false; + } + if (robot_state.multi_dof_joint_state.joint_names.size() != robot_state.multi_dof_joint_state.transforms.size() || + robot_state.multi_dof_joint_state.joint_names.size() != robot_state.multi_dof_joint_state.twist.size()) { + return false; + } + return true; +} +} // namespace + + +namespace tmc_simple_path_generator { + +SimplePathGeneratorPlugin::SimplePathGeneratorPlugin() + : sample_goal_generator_loader_("tmc_simple_path_generator", + "tmc_simple_path_generator::SampleGoalGeneratorBase"), + sample_middle_generator_loader_("tmc_simple_path_generator", + "tmc_simple_path_generator::ISampleMiddleGenerator"), + trajectory_interpolator_loader_("tmc_simple_path_generator", + "tmc_simple_path_generator::ITrajectoryInterpolator") {} + +void SimplePathGeneratorPlugin::Initialize(const rclcpp::Node::SharedPtr& node) { + clock_ = node->get_clock(); + + generate_timeout_ = std::make_shared>(node, "generator_timeout", 0.03); + set_param_handlers_.emplace_back(node->add_on_set_parameters_callback( + std::bind(&tmc_utils::DynamicParameter::SetParameterCallback, + generate_timeout_, std::placeholders::_1))); + + max_simple_trajectory_num_ = std::make_shared>( + node, "max_simple_trajectory_num", 5); + set_param_handlers_.emplace_back(node->add_on_set_parameters_callback( + std::bind(&tmc_utils::DynamicParameter::SetParameterCallback, + max_simple_trajectory_num_, std::placeholders::_1))); + + max_trajectory_num_ = std::make_shared>( + node, "max_trajectory_num", 0); + set_param_handlers_.emplace_back(node->add_on_set_parameters_callback( + std::bind(&tmc_utils::DynamicParameter::SetParameterCallback, + max_trajectory_num_, std::placeholders::_1))); + + const auto goal_sampler_type = tmc_utils::GetParameter( + node, "goal_sampler_type", "tmc_simple_path_generator/SampleGoalGenerator"); + sample_goal_generator_ = sample_goal_generator_loader_.createSharedInstance(goal_sampler_type); + sample_goal_generator_->Initialize(node); + + sampling_params_ = std::make_shared(node); + + // If it is specified, use the specified plugin + // If not, switch the class with bases_movement_type + const auto middle_sampler_type = tmc_utils::GetParameter(node, "middle_sampler_type", ""); + if (middle_sampler_type.empty()) { + if (sampling_params_->base_movement_type == tmc_manipulation_types::kPlanar) { + sample_middle_generator_ = sample_middle_generator_loader_.createSharedInstance( + "tmc_simple_path_generator/PlanerMiddleStateGenerator"); + } else { + RCLCPP_FATAL(node->get_logger(), "Unknown base movement type"); + exit(EXIT_FAILURE); + } + } else { + sample_middle_generator_ = sample_middle_generator_loader_.createSharedInstance(middle_sampler_type); + } + sample_middle_generator_->Initialize(node, sampling_params_); + + const auto interpolation_type = tmc_utils::GetParameter(node, "interpolation_type", ""); + if (interpolation_type.empty()) { + if (sampling_params_->base_movement_type == tmc_manipulation_types::kPlanar) { + trajectory_interpolator_ = trajectory_interpolator_loader_.createSharedInstance( + "tmc_simple_path_generator/PlanarTrajectoryInterpolator"); + } else { + RCLCPP_FATAL(node->get_logger(), "Unknown base movement type"); + exit(EXIT_FAILURE); + } + } else { + trajectory_interpolator_ = trajectory_interpolator_loader_.createSharedInstance(interpolation_type); + } + trajectory_interpolator_->Initialize(node, sampling_params_); + + hard_path_link_constraints_ = std::make_shared(node); + goal_relative_linear_constraint_ = std::make_shared(node); +} + +bool SimplePathGeneratorPlugin::Generate(const tmc_robot_local_planner::Constraints& constraints, + const tmc_manipulation_types::RobotState& initial_state, + double normalized_velocity, + const std::vector& ignore_joints, + std::function interrupt, + std::vector& trajectories_out) { + auto sampling_params = sampling_params_->RemoveJoints(ignore_joints); + + // The first time is currently a sample from the posture + sample_goal_generator_->set_is_from_random_initial_state(false); + + auto end_stamp = clock_->now() + std::chrono::duration(generate_timeout_->value()); + while (clock_->now() < end_stamp) { + // End decision + if (interrupt()) { + return false; + } + if (max_trajectory_num_->value() > 0 && trajectories_out.size() >= max_trajectory_num_->value()) { + break; + } + + // Sampling of goal posture + auto goal_state = initial_state; + if (!constraints.hard_joint_constraints.empty()) { + if (!sample_goal_generator_->SampleFromJointConstraints(constraints.hard_joint_constraints, + goal_state)) { + continue; + } + } + if (!constraints.hard_link_constraints.empty()) { + const auto result = sample_goal_generator_->SampleFromLinkConstraints( + initial_state, constraints.hard_link_constraints, sampling_params, goal_state); + sample_goal_generator_->set_is_from_random_initial_state(true); + if (!result) continue; + } + + // Intermediate posture sampling + std::optional random_middle_state; + if (trajectories_out.size() >= max_simple_trajectory_num_->value()) { + random_middle_state = sample_middle_generator_->ComputeRandomMiddleState( + initial_state, goal_state, sampling_params); + if (!random_middle_state) { + continue; + } + } + + // Goal relative straight route restraint + auto initial_state_for_goal_relative = initial_state; + if (random_middle_state) { + initial_state_for_goal_relative = random_middle_state.value(); + } + const auto middle_states_near_goal = goal_relative_linear_constraint_->SampleLinearPath( + constraints.goal_relative_linear_constraint, initial_state_for_goal_relative, goal_state, sampling_params); + if (!middle_states_near_goal) { + continue; + } + + // Generate the trajectory by collecting the transit points + tmc_manipulation_types::TimedRobotTrajectory trajectory; + if (random_middle_state) { + if (!trajectory_interpolator_->Interpolate( + initial_state, middle_states_near_goal.value(), goal_state, + normalized_velocity, random_middle_state.value(), trajectory)) { + continue; + } + } else { + if (!trajectory_interpolator_->Interpolate(initial_state, middle_states_near_goal.value(), goal_state, + normalized_velocity, trajectory)) { + continue; + } + } + + // Link route restraint + if (!constraints.hard_path_link_constraints.empty()) { + if (!hard_path_link_constraints_->ConstrainTrajectory(constraints.hard_path_link_constraints, + sampling_params, trajectory)) { + continue; + } + } + trajectories_out.emplace_back(trajectory); + } + sample_goal_generator_->Terminate(); + + return !trajectories_out.empty(); +} + +SimplePathGenerator::SimplePathGenerator() : SimplePathGenerator(rclcpp::NodeOptions()) {} + +SimplePathGenerator::SimplePathGenerator(const rclcpp::NodeOptions& options) + : Node("generator", options), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_), impl_() {} + +void SimplePathGenerator::Initialize() { + const auto node = shared_from_this(); + impl_.Initialize(node); + + origin_frame_ = tmc_utils::GetParameter(node, "origin_frame", kOriginFrame); + timeout_ = tmc_utils::GetParameter(node, "timeout", kTFTimeout); + + server_ = rclcpp_action::create_server( + this, "~/generate", + std::bind(&SimplePathGenerator::GoalCallback, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&SimplePathGenerator::CancelCallback, this, std::placeholders::_1), + std::bind(&SimplePathGenerator::FeedbackSetupCallback, this, std::placeholders::_1)); +} + +rclcpp_action::GoalResponse SimplePathGenerator::GoalCallback( + const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal) { + if (!CheckRobotState(goal->initial_state)) { + return rclcpp_action::GoalResponse::REJECT; + } + if (goal->constraints.hard_link_constraints.empty() && goal->constraints.hard_joint_constraints.empty()) { + RCLCPP_ERROR(this->get_logger(), "Constraint isn't set"); + return rclcpp_action::GoalResponse::REJECT; + } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse SimplePathGenerator::CancelCallback(const ServerGoalHandlePtr goal_handle) { + return rclcpp_action::CancelResponse::ACCEPT; +} + +void SimplePathGenerator::FeedbackSetupCallback(ServerGoalHandlePtr goal_handle) { + std::thread{std::bind(&SimplePathGenerator::Execute, this, std::placeholders::_1), goal_handle}.detach(); +} + +void SimplePathGenerator::Execute(const ServerGoalHandlePtr goal_handle) { + const auto goal = goal_handle->get_goal(); + + tmc_robot_local_planner::Constraints constraints; + tmc_planning_msgs::msg::Constraints goal_constraints = goal->constraints; + if (tmc_robot_local_planner_utils::TransformConstraints(origin_frame_, timeout_, tf_buffer_, goal_constraints)) { + tmc_robot_local_planner_utils::ConvertConstraints(goal_constraints, constraints); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to transform"); + auto action_result = std::make_shared(); + goal_handle->abort(action_result); + return; + } + + tmc_manipulation_types::RobotState initial_state; + tmc_manipulation_types_bridge::RobotStateMsgToRobotState(goal->initial_state, initial_state); + + auto interrupt_func = [goal_handle]() { return goal_handle->is_canceling(); }; + std::vector trajectories; + const auto gen_result = impl_.Generate(constraints, initial_state, goal->normalized_velocity, goal->ignore_joints, + interrupt_func, trajectories); + auto action_result = std::make_shared(); + if (gen_result) { + action_result->robot_trajectories.reserve(trajectories.size()); + for (const auto trajectory : trajectories) { + moveit_msgs::msg::RobotTrajectory trajectory_msg; + tmc_manipulation_types_bridge::TimedRobotTrajectoryToRobotTrajectoryMsg(trajectory, trajectory_msg); + action_result->robot_trajectories.emplace_back(trajectory_msg); + } + goal_handle->succeed(action_result); + } else if (goal_handle->is_canceling()) { + goal_handle->canceled(action_result); + } else { + goal_handle->abort(action_result); + } +} + +} // namespace tmc_simple_path_generator + +#include // NOLINT + +PLUGINLIB_EXPORT_CLASS(tmc_simple_path_generator::SimplePathGeneratorPlugin, + tmc_robot_local_planner::IGenerator) diff --git a/tmc_simple_path_generator/test/goal_relative_linear_constraint-test.cpp b/tmc_simple_path_generator/test/goal_relative_linear_constraint-test.cpp new file mode 100644 index 0000000..ba4c8c4 --- /dev/null +++ b/tmc_simple_path_generator/test/goal_relative_linear_constraint-test.cpp @@ -0,0 +1,288 @@ +/* +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. +*/ + +#include + +#include + +#include +#include +#include + +#include + +namespace { + +const std::vector kJointNames = {"joint1", "joint2", "joint3", "joint4", "joint5", "joint6"}; +const std::vector kBaseNames = {"world_joint"}; +const char* const kEndFrame = "link7"; + +constexpr double kEpsilon = 1.0e-3; +constexpr double kLinearStep = 0.01; + +tmc_manipulation_types::RobotState GenerateDefaultState() { + tmc_manipulation_types::RobotState state; + state.joint_state.name = kJointNames; + state.joint_state.position = Eigen::VectorXd::Zero(6); + state.multi_dof_joint_state.names = kBaseNames; + state.multi_dof_joint_state.poses = {Eigen::Affine3d::Identity()}; + return state; +} + +tmc_manipulation_types::RobotState AddBasePoseOffset(const tmc_manipulation_types::RobotState& input, + double offset_x, double offset_y) { + auto state_with_offset = input; + state_with_offset.multi_dof_joint_state.poses[0] *= Eigen::Translation3d(offset_x, offset_y, 0.0); + return state_with_offset; +} + +Eigen::Affine3d CalculateEndFramePose( + const tmc_robot_kinematics_model::IRobotKinematicsModel::Ptr& robot, + const tmc_manipulation_types::RobotState& state) { + robot->SetRobotTransform(state.multi_dof_joint_state.poses[0]); + robot->SetNamedAngle(state.joint_state); + return robot->GetObjectTransform(kEndFrame); +} + +tmc_simple_path_generator::SamplingParameters::Ptr GenerateSamplingParameters() { + auto params = std::make_shared(); + params->joint_names = kJointNames; + params->joint_weights.resize(kJointNames.size()); + params->joint_weights << 5.0, 1.0, 0.5, 0.5, 0.5, 1.0; + params->base_names = {"world_joint"}; + params->base_weights = Eigen::Vector3d(5.0, 5.0, 1.0); + params->joint_limit_upper.resize(kJointNames.size(), 3.0); + params->joint_limit_lower.resize(kJointNames.size(), -3.0); + params->base_movement_type = tmc_manipulation_types::kPlanar; + return params; +} + +} // namespace + +namespace tmc_simple_path_generator { + +class GoalRelativeLinearConstraintTest : public ::testing::Test { + protected: + void SetUp() override; + + GoalRelativeLinearConstraint::Ptr path_constraints_; + tmc_robot_kinematics_model::IRobotKinematicsModel::Ptr robot_; + + Eigen::Affine3d CalculateEndFramePose(const tmc_manipulation_types::RobotState& state); +}; + +void GoalRelativeLinearConstraintTest::SetUp() { + const auto robot_description = tmc_manipulation_tests::stanford_manipulator::GetUrdf(); + robot_ = std::make_shared(robot_description); + + auto node = rclcpp::Node::make_shared("test_node"); + node->declare_parameter("robot_description_kinematics", robot_description); + node->declare_parameter("linear_constraint_step", kLinearStep); + + path_constraints_ = std::make_shared(node); +} + +Eigen::Affine3d GoalRelativeLinearConstraintTest::CalculateEndFramePose( + const tmc_manipulation_types::RobotState& state) { + robot_->SetRobotTransform(state.multi_dof_joint_state.poses[0]); + robot_->SetNamedAngle(state.joint_state); + return robot_->GetObjectTransform(kEndFrame); +} + +TEST_F(GoalRelativeLinearConstraintTest, SampleLinearPath) { + tmc_robot_local_planner::LinearConstraint linear_constraint; + linear_constraint.end_frame_id = kEndFrame; + linear_constraint.axis = -Eigen::Vector3d::UnitX(); + linear_constraint.distance = 0.1; + + const auto initial_state = GenerateDefaultState(); + const auto goal_state = AddBasePoseOffset(initial_state, 0.5, 0.2); + + const auto linear_path = path_constraints_->SampleLinearPath( + linear_constraint, initial_state, goal_state, GenerateSamplingParameters()); + ASSERT_TRUE(linear_path.has_value()); + + // It can be +1 with a computational error of the number + const uint32_t expected_size = static_cast(linear_constraint.distance / kLinearStep); + EXPECT_GE(linear_path.value().size(), expected_size); + EXPECT_LE(linear_path.value().size(), expected_size + 1); + + const auto origin_to_goal = CalculateEndFramePose(goal_state); + + double prev_distance = linear_constraint.distance; + for (const auto& way_point : linear_path.value()) { + const auto origin_to_way_point = CalculateEndFramePose(way_point); + const auto displacement = origin_to_way_point.translation() - origin_to_goal.translation(); + // There is a width of the convergence judgment of IK, so you cannot expect a strict agreement + EXPECT_NEAR(displacement.normalized().dot(linear_constraint.axis), 1.0, kEpsilon); + + const auto distance = displacement.norm(); + EXPECT_GE(distance, kLinearStep - kEpsilon); + EXPECT_LE(distance, linear_constraint.distance + kEpsilon); + + const auto diff = prev_distance - distance; + // Almost zero or almost Klinearstep + EXPECT_TRUE(std::abs(diff) < kEpsilon || std::abs(diff - kLinearStep) < kEpsilon); + + prev_distance = distance; + } +} + +TEST_F(GoalRelativeLinearConstraintTest, InitialStateOnLinearConstraint) { + tmc_robot_local_planner::LinearConstraint linear_constraint; + linear_constraint.end_frame_id = kEndFrame; + linear_constraint.axis = -Eigen::Vector3d::UnitX(); + linear_constraint.distance = 0.1; + + const auto initial_state = GenerateDefaultState(); + const auto goal_state = AddBasePoseOffset(initial_state, 0.05, kLinearStep / 2.0); + + const auto linear_path = path_constraints_->SampleLinearPath( + linear_constraint, initial_state, goal_state, GenerateSamplingParameters()); + ASSERT_TRUE(linear_path.has_value()); + + // Since the initial state is almost on a straight line restriction, the distance is up to 0.04 + EXPECT_EQ(linear_path.value().size(), 4); + + const auto origin_to_goal = CalculateEndFramePose(goal_state); + + double prev_distance = 0.05; + for (const auto& way_point : linear_path.value()) { + const auto origin_to_way_point = CalculateEndFramePose(way_point); + const auto displacement = origin_to_way_point.translation() - origin_to_goal.translation(); + EXPECT_NEAR(displacement.normalized().dot(linear_constraint.axis), 1.0, kEpsilon); + + const auto distance = displacement.norm(); + EXPECT_GE(distance, kLinearStep - kEpsilon); + EXPECT_LE(distance, 0.04 + kEpsilon); + + const auto diff = prev_distance - distance; + EXPECT_NEAR(diff, kLinearStep, kEpsilon); + + prev_distance = distance; + } +} + +TEST_F(GoalRelativeLinearConstraintTest, NoIKSolutions) { + tmc_robot_local_planner::LinearConstraint linear_constraint; + linear_constraint.end_frame_id = kEndFrame; + linear_constraint.axis = Eigen::Vector3d::UnitZ(); + linear_constraint.distance = 10.0; + + const auto initial_state = GenerateDefaultState(); + const auto goal_state = AddBasePoseOffset(initial_state, 0.5, 0.0); + + const auto linear_path = path_constraints_->SampleLinearPath( + linear_constraint, initial_state, goal_state, GenerateSamplingParameters()); + // If the IK cannot be solved, it will be a failure because the restraint condition cannot be met. + EXPECT_FALSE(linear_path.has_value()); +} + +TEST_F(GoalRelativeLinearConstraintTest, LinearConstraintNotUsed) { + tmc_robot_local_planner::LinearConstraint linear_constraint; + linear_constraint.end_frame_id = ""; + linear_constraint.axis = -Eigen::Vector3d::UnitX(); + linear_constraint.distance = 0.1; + + const auto initial_state = GenerateDefaultState(); + const auto goal_state = AddBasePoseOffset(initial_state, 0.5, 0.2); + + const auto linear_path = path_constraints_->SampleLinearPath( + linear_constraint, initial_state, goal_state, GenerateSamplingParameters()); + // End_frame_id is an exceptional success + ASSERT_TRUE(linear_path.has_value()); + EXPECT_TRUE(linear_path.value().empty()); +} + +TEST_F(GoalRelativeLinearConstraintTest, InvalidEndFrame) { + tmc_robot_local_planner::LinearConstraint linear_constraint; + linear_constraint.end_frame_id = "invalid_frame"; + linear_constraint.axis = -Eigen::Vector3d::UnitX(); + linear_constraint.distance = 0.1; + + const auto initial_state = GenerateDefaultState(); + const auto goal_state = AddBasePoseOffset(initial_state, 0.5, 0.2); + + const auto linear_path = path_constraints_->SampleLinearPath( + linear_constraint, initial_state, goal_state, GenerateSamplingParameters()); + // If End_frame_id does not exist, it will fail + EXPECT_FALSE(linear_path.has_value()); +} + +TEST_F(GoalRelativeLinearConstraintTest, ZeroAxis) { + tmc_robot_local_planner::LinearConstraint linear_constraint; + linear_constraint.end_frame_id = kEndFrame; + linear_constraint.axis = Eigen::Vector3d::Zero(); + linear_constraint.distance = 0.1; + + const auto initial_state = GenerateDefaultState(); + const auto goal_state = AddBasePoseOffset(initial_state, 0.5, 0.2); + + const auto linear_path = path_constraints_->SampleLinearPath( + linear_constraint, initial_state, goal_state, GenerateSamplingParameters()); + // AXIS is an abnormal input, so it will be a failure. + EXPECT_FALSE(linear_path.has_value()); +} + +TEST_F(GoalRelativeLinearConstraintTest, ZeroDistance) { + tmc_robot_local_planner::LinearConstraint linear_constraint; + linear_constraint.end_frame_id = kEndFrame; + linear_constraint.axis = -Eigen::Vector3d::UnitX(); + linear_constraint.distance = 0.0; + + const auto initial_state = GenerateDefaultState(); + const auto goal_state = AddBasePoseOffset(initial_state, 0.5, 0.2); + + const auto linear_path = path_constraints_->SampleLinearPath( + linear_constraint, initial_state, goal_state, GenerateSamplingParameters()); + // The distance is zero, so it is a failure because it is an abnormal input. + EXPECT_FALSE(linear_path.has_value()); +} + +TEST_F(GoalRelativeLinearConstraintTest, NegativeDistance) { + tmc_robot_local_planner::LinearConstraint linear_constraint; + linear_constraint.end_frame_id = kEndFrame; + linear_constraint.axis = Eigen::Vector3d::UnitX(); + linear_constraint.distance = -0.1; + + const auto initial_state = GenerateDefaultState(); + const auto goal_state = AddBasePoseOffset(initial_state, 0.5, 0.2); + + const auto linear_path = path_constraints_->SampleLinearPath( + linear_constraint, initial_state, goal_state, GenerateSamplingParameters()); + // The distance is negative, but it fails as an unusual input + EXPECT_FALSE(linear_path.has_value()); +} + +} // namespace tmc_simple_path_generator + +int main(int argc, char **argv) { + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/tmc_simple_path_generator/test/hard_path_link_constraints-test.cpp b/tmc_simple_path_generator/test/hard_path_link_constraints-test.cpp new file mode 100644 index 0000000..a43d7b4 --- /dev/null +++ b/tmc_simple_path_generator/test/hard_path_link_constraints-test.cpp @@ -0,0 +1,314 @@ +/* +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. +*/ + +#include + +#include + +#include +#include +#include + +#include + +namespace { + +const std::vector kJointNames = {"joint1", "joint2", "joint3", "joint4", "joint5", "joint6"}; +const std::vector kBaseNames = {"world_joint"}; +const char* const kEndFrame = "link7"; + +constexpr double kEpsilon = 1.0e-3; + +tmc_manipulation_types::RobotState GenerateDefaultState() { + tmc_manipulation_types::RobotState state; + state.joint_state.name = kJointNames; + state.joint_state.position = Eigen::VectorXd::Zero(6); + state.multi_dof_joint_state.names = kBaseNames; + state.multi_dof_joint_state.poses = {Eigen::Affine3d::Identity()}; + return state; +} + +tmc_manipulation_types::RobotState AddBasePoseOffset(const tmc_manipulation_types::RobotState& input, + double offset_x, double offset_y) { + auto state_with_offset = input; + state_with_offset.multi_dof_joint_state.poses[0] *= Eigen::Translation3d(offset_x, offset_y, 0.0); + return state_with_offset; +} + +tmc_manipulation_types::TimedRobotTrajectory GenerateTrajectory( + const std::vector& robot_states) { + tmc_manipulation_types::TimedRobotTrajectory trajectory; + trajectory.joint_trajectory.joint_names = kJointNames; + trajectory.multi_dof_joint_trajectory.joint_names = kBaseNames; + + for (const auto& state : robot_states) { + tmc_manipulation_types::TimedJointTrajectoryPoint joint_point; + joint_point.positions = state.joint_state.position; + trajectory.joint_trajectory.points.push_back(joint_point); + + tmc_manipulation_types::TimedMultiDOFJointTrajectoryPoint multi_dof_point; + multi_dof_point.transforms = state.multi_dof_joint_state.poses; + trajectory.multi_dof_joint_trajectory.points.push_back(multi_dof_point); + } + return trajectory; +} + +Eigen::Affine3d CalculateEndFramePose( + const tmc_robot_kinematics_model::IRobotKinematicsModel::Ptr& robot, + const tmc_manipulation_types::RobotState& state) { + robot->SetRobotTransform(state.multi_dof_joint_state.poses[0]); + robot->SetNamedAngle(state.joint_state); + return robot->GetObjectTransform(kEndFrame); +} + +tmc_manipulation_types::RobotState ExtractPositions(const tmc_manipulation_types::TimedRobotTrajectory& trajectory, + uint32_t index) { + tmc_manipulation_types::RobotState state; + state.joint_state.name = trajectory.joint_trajectory.joint_names; + state.joint_state.position = trajectory.joint_trajectory.points[index].positions; + state.multi_dof_joint_state.names = trajectory.multi_dof_joint_trajectory.joint_names; + state.multi_dof_joint_state.poses = trajectory.multi_dof_joint_trajectory.points[index].transforms; + return state; +} + +tmc_simple_path_generator::SamplingParameters::Ptr GenerateSamplingParameters() { + auto params = std::make_shared(); + params->joint_names = kJointNames; + params->joint_weights.resize(kJointNames.size()); + params->joint_weights << 5.0, 1.0, 0.5, 0.5, 0.5, 1.0; + params->base_names = {"world_joint"}; + params->base_weights = Eigen::Vector3d(5.0, 5.0, 1.0); + params->joint_limit_upper.resize(kJointNames.size(), 3.0); + params->joint_limit_lower.resize(kJointNames.size(), -3.0); + params->base_movement_type = tmc_manipulation_types::kPlanar; + return params; +} + +tmc_robot_local_planner::ILinkConstraint::Ptr GenerateLinkConstraint( + const Eigen::Affine3d& origin_to_hand_goal, double max_bound_x) { + tmc_manipulation_types::RegionValues max_bounds; + max_bounds[0] = max_bound_x; + tmc_manipulation_types::TaskSpaceRegion tsr(origin_to_hand_goal, Eigen::Affine3d::Identity(), + tmc_manipulation_types::RegionValues::Zero(), max_bounds, + "odom", kEndFrame); + return std::make_shared(tsr, 0); +} + +} // namespace + +namespace tmc_simple_path_generator { + +class HardPathLinkConstraintsTest : public ::testing::Test { + protected: + void SetUp() override; + + void TestEndFramePosition(const tmc_manipulation_types::TimedRobotTrajectory& trajectory, + uint32_t index, + double expected_x, + double expected_y); + + HardPathLinkConstraints::Ptr path_constraints_; + tmc_robot_kinematics_model::IRobotKinematicsModel::Ptr robot_; + + double offset_x; + double offset_y; +}; + +void HardPathLinkConstraintsTest::SetUp() { + const auto robot_description = tmc_manipulation_tests::stanford_manipulator::GetUrdf(); + robot_ = std::make_shared(robot_description); + + const auto origin_to_constraint = CalculateEndFramePose(robot_, GenerateDefaultState()); + offset_x = origin_to_constraint.translation().x(); + offset_y = origin_to_constraint.translation().y(); + + auto node = rclcpp::Node::make_shared("test_node"); + node->declare_parameter("robot_description_kinematics", robot_description); + + path_constraints_ = std::make_shared(node); +} + +void HardPathLinkConstraintsTest::TestEndFramePosition( + const tmc_manipulation_types::TimedRobotTrajectory& trajectory, + uint32_t index, + double expected_x, + double expected_y) { + const auto state = ExtractPositions(trajectory, index); + const auto translation = CalculateEndFramePose(robot_, state).translation(); + EXPECT_NEAR(translation.x(), expected_x + offset_x, kEpsilon); + EXPECT_NEAR(translation.y(), expected_y + offset_y, kEpsilon); +} + +TEST_F(HardPathLinkConstraintsTest, ConstrainAllPoints) { + // Stanford_manipulator matches the robot root and end effector posture if the joint angle is zero. + // So you can easily test by moving the XY plane. + + auto trajectory = GenerateTrajectory({AddBasePoseOffset(GenerateDefaultState(), 0.5, 0.0), + AddBasePoseOffset(GenerateDefaultState(), 0.2, -0.3), + AddBasePoseOffset(GenerateDefaultState(), 0.1, 0.3), + AddBasePoseOffset(GenerateDefaultState(), -0.1, 0.0)}); + + const auto origin_to_constraint = CalculateEndFramePose(robot_, GenerateDefaultState()); + const std::vector link_constraints = { + GenerateLinkConstraint(origin_to_constraint, 0.3)}; + + ASSERT_TRUE(path_constraints_->ConstrainTrajectory(link_constraints, GenerateSamplingParameters(), trajectory)); + ASSERT_EQ(trajectory.joint_trajectory.points.size(), 6); + ASSERT_EQ(trajectory.multi_dof_joint_trajectory.points.size(), 6); + + const auto pos_y = origin_to_constraint.translation().y(); + TestEndFramePosition(trajectory, 0, 0.5, 0.0); + TestEndFramePosition(trajectory, 1, 0.3, 0.0); + TestEndFramePosition(trajectory, 2, 0.2, 0.0); + TestEndFramePosition(trajectory, 3, 0.1, 0.0); + TestEndFramePosition(trajectory, 4, 0.0, 0.0); + TestEndFramePosition(trajectory, 5, -0.1, 0.0); +} + +TEST_F(HardPathLinkConstraintsTest, ConstrainTwoPointTrajectory) { + auto trajectory = GenerateTrajectory({AddBasePoseOffset(GenerateDefaultState(), 0.5, 0.0), + AddBasePoseOffset(GenerateDefaultState(), -0.1, 0.0)}); + + const auto origin_to_constraint = CalculateEndFramePose(robot_, GenerateDefaultState()); + const std::vector link_constraints = { + GenerateLinkConstraint(origin_to_constraint, 0.3)}; + + ASSERT_TRUE(path_constraints_->ConstrainTrajectory(link_constraints, GenerateSamplingParameters(), trajectory)); + ASSERT_EQ(trajectory.joint_trajectory.points.size(), 4); + ASSERT_EQ(trajectory.multi_dof_joint_trajectory.points.size(), 4); + + TestEndFramePosition(trajectory, 0, 0.5, 0.0); + TestEndFramePosition(trajectory, 1, 0.3, 0.0); + TestEndFramePosition(trajectory, 2, 0.0, 0.0); + TestEndFramePosition(trajectory, 3, -0.1, 0.0); +} + +TEST_F(HardPathLinkConstraintsTest, ConstrainSatisfiedTrajectory) { + auto trajectory = GenerateTrajectory({AddBasePoseOffset(GenerateDefaultState(), 0.3, 0.0), + AddBasePoseOffset(GenerateDefaultState(), 0.2, 0.0), + AddBasePoseOffset(GenerateDefaultState(), 0.1, 0.0)}); + + const auto origin_to_constraint = CalculateEndFramePose(robot_, GenerateDefaultState()); + const std::vector link_constraints = { + GenerateLinkConstraint(origin_to_constraint, 0.4)}; + + ASSERT_TRUE(path_constraints_->ConstrainTrajectory(link_constraints, GenerateSamplingParameters(), trajectory)); + ASSERT_EQ(trajectory.joint_trajectory.points.size(), 3); + ASSERT_EQ(trajectory.multi_dof_joint_trajectory.points.size(), 3); + + TestEndFramePosition(trajectory, 0, 0.3, 0.0); + TestEndFramePosition(trajectory, 1, 0.2, 0.0); + TestEndFramePosition(trajectory, 2, 0.1, 0.0); +} + +TEST_F(HardPathLinkConstraintsTest, SampleConstraints) { + auto input_trajectory = GenerateTrajectory({AddBasePoseOffset(GenerateDefaultState(), 0.5, 0.0), + AddBasePoseOffset(GenerateDefaultState(), 0.2, 0.0)}); + + const auto origin_to_constraint_1 = CalculateEndFramePose(robot_, GenerateDefaultState()); + const auto origin_to_constraint_2 = CalculateEndFramePose(robot_, + AddBasePoseOffset(GenerateDefaultState(), 0.2, 0.0)); + const std::vector link_constraints = { + GenerateLinkConstraint(origin_to_constraint_1, 0.0), GenerateLinkConstraint(origin_to_constraint_2, 0.0)}; + + uint32_t count_1 = 0; + uint32_t count_2 = 0; + for (uint32_t i = 0; i < 100; ++i) { + auto trajectory = input_trajectory; + ASSERT_TRUE(path_constraints_->ConstrainTrajectory(link_constraints, GenerateSamplingParameters(), trajectory)); + ASSERT_EQ(trajectory.joint_trajectory.points.size(), trajectory.multi_dof_joint_trajectory.points.size()); + + // GOAL meets one constraint, so you can get 3 points + if (trajectory.joint_trajectory.points.size() == 4) { + TestEndFramePosition(trajectory, 0, 0.5, 0.0); + TestEndFramePosition(trajectory, 1, 0.0, 0.0); + TestEndFramePosition(trajectory, 2, 0.0, 0.0); + TestEndFramePosition(trajectory, 3, 0.2, 0.0); + ++count_1; + } else if (trajectory.joint_trajectory.points.size() == 3) { + TestEndFramePosition(trajectory, 0, 0.5, 0.0); + TestEndFramePosition(trajectory, 1, 0.2, 0.0); + TestEndFramePosition(trajectory, 2, 0.2, 0.0); + ++count_2; + } else { + FAIL(); + return; + } + } + + // Since it is a binary distribution of 100 samples, 3σ is 15 + EXPECT_NEAR(count_1, 50, 15); + EXPECT_NEAR(count_2, 50, 15); +} + +TEST_F(HardPathLinkConstraintsTest, NoConstraints) { + // Become a success with through + auto trajectory = GenerateTrajectory({AddBasePoseOffset(GenerateDefaultState(), 0.5, 0.0), + AddBasePoseOffset(GenerateDefaultState(), 0.1, 0.3), + AddBasePoseOffset(GenerateDefaultState(), -0.1, 0.0)}); + + ASSERT_TRUE(path_constraints_->ConstrainTrajectory({}, GenerateSamplingParameters(), trajectory)); + ASSERT_EQ(trajectory.joint_trajectory.points.size(), 3); + ASSERT_EQ(trajectory.multi_dof_joint_trajectory.points.size(), 3); + + TestEndFramePosition(trajectory, 0, 0.5, 0.0); + TestEndFramePosition(trajectory, 1, 0.1, 0.3); + TestEndFramePosition(trajectory, 2, -0.1, 0.0); +} + +TEST_F(HardPathLinkConstraintsTest, ConstrainOnePointTrajectory) { + auto trajectory = GenerateTrajectory({AddBasePoseOffset(GenerateDefaultState(), 0.5, 0.0)}); + + const auto origin_to_constraint = CalculateEndFramePose(robot_, GenerateDefaultState()); + const std::vector link_constraints = { + GenerateLinkConstraint(origin_to_constraint, 0.3)}; + + EXPECT_FALSE(path_constraints_->ConstrainTrajectory(link_constraints, GenerateSamplingParameters(), trajectory)); +} + +TEST_F(HardPathLinkConstraintsTest, InvalidTrajectory) { + auto trajectory = GenerateTrajectory({AddBasePoseOffset(GenerateDefaultState(), 0.5, 0.0), + AddBasePoseOffset(GenerateDefaultState(), 0.2, -0.3), + AddBasePoseOffset(GenerateDefaultState(), 0.1, 0.3), + AddBasePoseOffset(GenerateDefaultState(), -0.1, 0.0)}); + trajectory.joint_trajectory.points.pop_back(); + + const auto origin_to_constraint = CalculateEndFramePose(robot_, GenerateDefaultState()); + const std::vector link_constraints = { + GenerateLinkConstraint(origin_to_constraint, 0.3)}; + + EXPECT_FALSE(path_constraints_->ConstrainTrajectory(link_constraints, GenerateSamplingParameters(), trajectory)); +} + +} // namespace tmc_simple_path_generator + +int main(int argc, char **argv) { + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/tmc_simple_path_generator/test/planar_trajectory_interpolator-test.cpp b/tmc_simple_path_generator/test/planar_trajectory_interpolator-test.cpp new file mode 100644 index 0000000..c3c9665 --- /dev/null +++ b/tmc_simple_path_generator/test/planar_trajectory_interpolator-test.cpp @@ -0,0 +1,800 @@ +/* +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. +*/ +/// @brief PLANARTRAJECTORYINTERPOLATOR test + +#include + +#include + +#include + +#include +#include + +using tmc_robot_local_planner_utils::Get2DPose; +using tmc_robot_local_planner_utils::Get2DTwist; + +namespace { +const std::vector kJointNames = {"arm", "hand", "shoulder"}; +const std::vector kBaseNames = {"world_joint"}; +constexpr double kEpsilon = 1.0e-6; + +// Extract Positions from TimeDJointTrajectory +bool ExtractJointPositions( + const tmc_manipulation_types::TimedJointTrajectory& trajectory, + const tmc_manipulation_types::NameSeq& joint_names, + std::vector& dst_positions) { + dst_positions.clear(); + for (const auto& name : joint_names) { + auto name_it = std::find(trajectory.joint_names.begin(), + trajectory.joint_names.end(), name); + if (name_it == trajectory.joint_names.end()) { + return false; + } + auto index = std::distance(trajectory.joint_names.begin(), name_it); + Eigen::VectorXd positions(trajectory.points.size()); + for (const auto& point : trajectory.points | boost::adaptors::indexed()) { + if (index < point.value().positions.size()) { + positions[point.index()] = point.value().positions[index]; + } else { + return false; + } + } + dst_positions.push_back(positions); + } + return true; +} + +// Extract Velocities from TimeDJOINTTRAJECTORY +bool ExtractJointVelocities( + const tmc_manipulation_types::TimedJointTrajectory& trajectory, + const tmc_manipulation_types::NameSeq& joint_names, + std::vector& dst_velocities) { + dst_velocities.clear(); + for (const auto& name : joint_names) { + auto name_it = std::find(trajectory.joint_names.begin(), + trajectory.joint_names.end(), name); + if (name_it == trajectory.joint_names.end()) { + return false; + } + auto index = std::distance(trajectory.joint_names.begin(), name_it); + Eigen::VectorXd velocities(trajectory.points.size()); + for (const auto& point : trajectory.points | boost::adaptors::indexed()) { + if (index < point.value().velocities.size()) { + velocities[point.index()] = point.value().velocities[index]; + } else { + return false; + } + } + dst_velocities.push_back(velocities); + } + return true; +} + +// Extract the position x, y, yaw from TimeDMULTIDOLTIINTTRAJECTORY +bool ExtractBase2DPoses( + const tmc_manipulation_types::TimedMultiDOFJointTrajectory& trajectory, + const std::string joint_name, + std::vector& dst_positions) { + dst_positions.resize(3, Eigen::VectorXd(trajectory.points.size())); + auto name_it = std::find(trajectory.joint_names.begin(), + trajectory.joint_names.end(), joint_name); + if (name_it == trajectory.joint_names.end()) { + return false; + } + auto index = std::distance(trajectory.joint_names.begin(), name_it); + for (const auto& point : trajectory.points | boost::adaptors::indexed()) { + auto pose = Get2DPose(point.value().transforms[index]); + dst_positions[0][point.index()] = pose[0]; + dst_positions[1][point.index()] = pose[1]; + dst_positions[2][point.index()] = pose[2]; + } + return true; +} + +// Extract the speed x, y, yaw from TimeDMULTIDOLTIDOFJECTORY +bool ExtractBase2DTwist( + const tmc_manipulation_types::TimedMultiDOFJointTrajectory& trajectory, + const std::string joint_name, + std::vector& dst_velocities) { + dst_velocities.resize(3, Eigen::VectorXd(trajectory.points.size())); + auto name_it = std::find(trajectory.joint_names.begin(), + trajectory.joint_names.end(), joint_name); + if (name_it == trajectory.joint_names.end()) { + return false; + } + auto index = std::distance(trajectory.joint_names.begin(), name_it); + for (const auto& point : trajectory.points | boost::adaptors::indexed()) { + auto twist = Get2DTwist(point.value().velocities[index]); + dst_velocities[0][point.index()] = twist[0]; + dst_velocities[1][point.index()] = twist[1]; + dst_velocities[2][point.index()] = twist[2]; + } + return true; +} + +// Counting a change in increase and decrease, assuming that you start from zero +uint32_t CountDirectionSwitch(const Eigen::VectorXd& positions) { + // Explore the first point that is not zero + double last_diff = 0.0; + uint32_t start_index = positions.size(); + for (uint32_t i = 0; i < positions.size(); ++i) { + if (fabs(positions[i]) > std::numeric_limits::min()) { + start_index = i; + last_diff = positions[i]; + break; + } + } + if (start_index == positions.size()) { + // There is no change because it is zero all the time + return 0; + } + uint32_t result = 0; + for (uint32_t i = start_index + 1; i < positions.size(); ++i) { + auto diff = positions[i] - positions[i - 1]; + if (fabs(diff) < std::numeric_limits::min()) { + // If you are zero, make a decision + continue; + } + if (last_diff * diff < 0.0) { + ++result; + } + last_diff = diff; + } + return result; +} + +void Eq(const Eigen::Affine3d& actual, const Eigen::Affine3d& expected) { + EXPECT_EQ(actual.translation(), expected.translation()); + EXPECT_EQ(actual.linear(), expected.linear()); +} + +tmc_simple_path_generator::SamplingParameters::Ptr GenerateTestParameters() { + auto params = std::make_shared(); + params->joint_names = kJointNames; + params->joint_weights = Eigen::Vector3d(5.0, 1.0, 0.5); + params->base_names = kBaseNames; + params->base_weights = Eigen::Vector3d(5.0, 5.0, 1.0); + params->joint_limit_upper = {1.0, 1.0, 1.0}; + params->joint_limit_lower = {-1.0, -1.0, -1.0}; + return params; +} + +tmc_manipulation_types::RobotState GenerateRobotState(double value = 0.0) { + tmc_manipulation_types::RobotState state; + state.joint_state.name = kJointNames; + state.joint_state.position = Eigen::VectorXd::Ones(kJointNames.size()) * value; + state.joint_state.velocity = Eigen::VectorXd::Ones(kJointNames.size()) * value; + state.multi_dof_joint_state.names = kBaseNames; + state.multi_dof_joint_state.poses.resize(1); + state.multi_dof_joint_state.poses[0] = + Eigen::Translation3d(value, value, 0.0) * Eigen::Quaterniond(Eigen::AngleAxisd(value, Eigen::Vector3d::UnitZ())); + state.multi_dof_joint_state.twist.push_back(Eigen::VectorXd::Ones(6) * value); + return state; +} + +tmc_manipulation_types::RobotState GenerateInitialState() { + return GenerateRobotState(); +} + +tmc_manipulation_types::RobotState GenerateGoalState() { + return GenerateRobotState(1.0); +} + +} // namespace + +namespace tmc_simple_path_generator { + +TEST(PlanarTrajectoryInterpolatorPluginTest, CreateInstance) { + pluginlib::ClassLoader class_loader( + "tmc_simple_path_generator", "tmc_simple_path_generator::ITrajectoryInterpolator"); + auto interpolator = class_loader.createSharedInstance("tmc_simple_path_generator/PlanarTrajectoryInterpolator"); + interpolator = class_loader.createSharedInstance("tmc_simple_path_generator/PlanarTrajectoryInterpolatorPlain"); +} + + +class PlanarTrajectoryInterpolatorTest : public ::testing::Test { + protected: + void SetUp() override; + + rclcpp::Node::SharedPtr node_; + ITrajectoryInterpolator::Ptr planar_; +}; + +void PlanarTrajectoryInterpolatorTest::SetUp() { + node_ = rclcpp::Node::make_shared("test_node"); + + planar_ = std::make_shared(); + planar_->Initialize(node_, GenerateTestParameters()); +} + +// If there is no initial speed +TEST_F(PlanarTrajectoryInterpolatorTest, InterpolateSimpleWithoutInitialVelocities) { + const auto initial_state = GenerateInitialState(); + const auto goal_state = GenerateGoalState(); + + tmc_manipulation_types::TimedRobotTrajectory trajectory; + ASSERT_TRUE(planar_->Interpolate(initial_state, goal_state, 0.5, trajectory)); + + std::vector joint_positions; + ASSERT_TRUE(ExtractJointPositions(trajectory.joint_trajectory, kJointNames, joint_positions)); + for (uint32_t i = 0; i < kJointNames.size(); ++i) { + EXPECT_EQ(0, CountDirectionSwitch(joint_positions[i])); + EXPECT_LE(0.0, joint_positions[i].tail(1)[0]); + EXPECT_DOUBLE_EQ(initial_state.joint_state.position[i], joint_positions[i].head(1)[0]); + EXPECT_DOUBLE_EQ(goal_state.joint_state.position[i], joint_positions[i].tail(1)[0]); + } + std::vector joint_velocities; + ASSERT_TRUE(ExtractJointVelocities(trajectory.joint_trajectory, kJointNames, joint_velocities)); + for (uint32_t i = 0; i < kJointNames.size(); ++i) { + EXPECT_DOUBLE_EQ(initial_state.joint_state.velocity[i], joint_velocities[i].head(1)[0]); + EXPECT_DOUBLE_EQ(0.0, joint_velocities[i].tail(1)[0]); + } + + std::vector base_positions; + ASSERT_TRUE(ExtractBase2DPoses(trajectory.multi_dof_joint_trajectory, kBaseNames[0], base_positions)); + const auto initial_base_pose = Get2DPose(initial_state.multi_dof_joint_state.poses[0]); + const auto goal_base_pose = Get2DPose(goal_state.multi_dof_joint_state.poses[0]); + for (uint32_t i = 0; i < base_positions.size(); ++i) { + EXPECT_EQ(0, CountDirectionSwitch(base_positions[i])); + EXPECT_LE(0.0, base_positions[i].tail(1)[0]); + EXPECT_DOUBLE_EQ(initial_base_pose[i], base_positions[i].head(1)[0]); + EXPECT_DOUBLE_EQ(goal_base_pose[i], base_positions[i].tail(1)[0]); + } + std::vector base_velocities; + ASSERT_TRUE(ExtractBase2DTwist(trajectory.multi_dof_joint_trajectory, kBaseNames[0], base_velocities)); + auto initial_base_twist = Get2DTwist(initial_state.multi_dof_joint_state.twist[0]); + for (uint32_t i = 0; i < base_velocities.size(); ++i) { + EXPECT_DOUBLE_EQ(initial_base_twist[i], base_velocities[i].head(1)[0]); + EXPECT_DOUBLE_EQ(0.0, base_velocities[i].tail(1)[0]); + } +} + +// If you have a initial speed +TEST_F(PlanarTrajectoryInterpolatorTest, InterpolateSimpleWithInitialVelocities) { + auto initial_state = GenerateInitialState(); + initial_state.joint_state.velocity << 0.1, 0.1, 0.0; + initial_state.multi_dof_joint_state.twist[0] << 0.1, -1.0, 0.1, 0.0, 0.0, 0.0; + const auto goal_state = GenerateGoalState(); + + tmc_manipulation_types::TimedRobotTrajectory trajectory; + ASSERT_TRUE(planar_->Interpolate(initial_state, goal_state, 0.5, trajectory)); + + std::vector joint_positions; + ASSERT_TRUE(ExtractJointPositions(trajectory.joint_trajectory, kJointNames, joint_positions)); + for (uint32_t i = 0; i < kJointNames.size(); ++i) { + EXPECT_EQ(0, CountDirectionSwitch(joint_positions[i])); + EXPECT_NEAR(initial_state.joint_state.position[i], joint_positions[i].head(1)[0], kEpsilon); + EXPECT_NEAR(goal_state.joint_state.position[i], joint_positions[i].tail(1)[0], kEpsilon); + } + std::vector joint_velocities; + ASSERT_TRUE(ExtractJointVelocities(trajectory.joint_trajectory, kJointNames, joint_velocities)); + for (uint32_t i = 0; i < kJointNames.size(); ++i) { + EXPECT_NEAR(initial_state.joint_state.velocity[i], joint_velocities[i].head(1)[0], kEpsilon); + EXPECT_NEAR(0.0, joint_velocities[i].tail(1)[0], kEpsilon); + } + + std::vector base_positions; + ASSERT_TRUE(ExtractBase2DPoses(trajectory.multi_dof_joint_trajectory, kBaseNames[0], base_positions)); + // The direction of the bogie Y changes only once + EXPECT_EQ(0, CountDirectionSwitch(base_positions[0])); + EXPECT_EQ(1, CountDirectionSwitch(base_positions[1])); + EXPECT_EQ(0, CountDirectionSwitch(base_positions[2])); + auto initial_base_pose = Get2DPose(initial_state.multi_dof_joint_state.poses[0]); + auto goal_base_pose = Get2DPose(goal_state.multi_dof_joint_state.poses[0]); + for (uint32_t i = 0; i < base_positions.size(); ++i) { + EXPECT_NEAR(initial_base_pose[i], base_positions[i].head(1)[0], kEpsilon); + EXPECT_NEAR(goal_base_pose[i], base_positions[i].tail(1)[0], kEpsilon); + } + + std::vector base_velocities; + ASSERT_TRUE(ExtractBase2DTwist(trajectory.multi_dof_joint_trajectory, kBaseNames[0], base_velocities)); + auto initial_base_twist = Get2DTwist(initial_state.multi_dof_joint_state.twist[0]); + for (uint32_t i = 0; i < base_velocities.size(); ++i) { + EXPECT_NEAR(initial_base_twist[i], base_velocities[i].head(1)[0], kEpsilon); + EXPECT_NEAR(0.0, base_velocities[i].tail(1)[0], kEpsilon); + } +} + +// Unauthorized Normalized_velocity +TEST_F(PlanarTrajectoryInterpolatorTest, InvalidNormalizedVelocity) { + tmc_manipulation_types::TimedRobotTrajectory trajectory; + EXPECT_FALSE(planar_->Interpolate(GenerateInitialState(), GenerateGoalState(), -0.1, trajectory)); +} + +// Track interpolation using intermediate posture +TEST_F(PlanarTrajectoryInterpolatorTest, InterpolateWithMiddleState) { + const auto initial_state = GenerateInitialState(); + const auto goal_state = GenerateGoalState(); + + // If the outside of the INITIAL and GOAL is an intermediate posture, a switching will occur. + std::vector> middle_state_and_switch_count = { + {GenerateRobotState(-1.0), 1}, {GenerateRobotState(0.5), 0}, {GenerateRobotState(2.0), 1} + }; + + for (const auto& [middle_state, switch_count] : middle_state_and_switch_count) { + tmc_manipulation_types::TimedRobotTrajectory trajectory; + ASSERT_TRUE(planar_->Interpolate(initial_state, goal_state, 0.5, middle_state, trajectory)); + + std::vector joint_positions; + ASSERT_TRUE(ExtractJointPositions(trajectory.joint_trajectory, kJointNames, joint_positions)); + + for (const auto& positions : joint_positions) { + EXPECT_EQ(switch_count, CountDirectionSwitch(positions)); + } + + for (uint32_t i = 0; i < kJointNames.size(); ++i) { + EXPECT_NEAR(initial_state.joint_state.position[i], joint_positions[i].head(1)[0], kEpsilon); + EXPECT_NEAR(goal_state.joint_state.position[i], joint_positions[i].tail(1)[0], kEpsilon); + } + std::vector joint_velocities; + ASSERT_TRUE(ExtractJointVelocities(trajectory.joint_trajectory, kJointNames, joint_velocities)); + for (uint32_t i = 0; i < kJointNames.size(); ++i) { + EXPECT_NEAR(initial_state.joint_state.velocity[i], joint_velocities[i].head(1)[0], kEpsilon); + EXPECT_NEAR(0.0, joint_velocities[i].tail(1)[0], kEpsilon); + } + + std::vector base_positions; + ASSERT_TRUE(ExtractBase2DPoses(trajectory.multi_dof_joint_trajectory, kBaseNames[0], base_positions)); + + for (const auto& positions : base_positions) { + EXPECT_EQ(switch_count, CountDirectionSwitch(positions)); + } + + auto initial_base_pose = Get2DPose(initial_state.multi_dof_joint_state.poses[0]); + auto goal_base_pose = Get2DPose(goal_state.multi_dof_joint_state.poses[0]); + for (uint32_t i = 0; i < base_positions.size(); ++i) { + EXPECT_NEAR(initial_base_pose[i], base_positions[i].head(1)[0], kEpsilon); + EXPECT_NEAR(goal_base_pose[i], base_positions[i].tail(1)[0], kEpsilon); + } + std::vector base_velocities; + ASSERT_TRUE(ExtractBase2DTwist(trajectory.multi_dof_joint_trajectory, kBaseNames[0], base_velocities)); + auto initial_base_twist = Get2DTwist(initial_state.multi_dof_joint_state.twist[0]); + for (uint32_t i = 0; i < base_velocities.size(); ++i) { + EXPECT_NEAR(initial_base_twist[i], base_velocities[i].head(1)[0], kEpsilon); + EXPECT_NEAR(0.0, base_velocities[i].tail(1)[0], kEpsilon); + } + } +} + +// The time_from_start in the orbit generated is changing according to the trajectory length, or a case with orbital scoring or less +TEST_F(PlanarTrajectoryInterpolatorTest, TimeFromStartsShortTrajectory) { + tmc_manipulation_types::TimedRobotTrajectory trajectory; + ASSERT_TRUE(planar_->Interpolate(GenerateInitialState(), GenerateGoalState(), 1.75, trajectory)); + + ASSERT_EQ(30, trajectory.joint_trajectory.points.size()); + ASSERT_EQ(30, trajectory.multi_dof_joint_trajectory.points.size()); + ASSERT_EQ(trajectory.joint_trajectory.points.size(), + trajectory.multi_dof_joint_trajectory.points.size()); + + // Time check + for (uint32_t i = 0; i < trajectory.joint_trajectory.points.size(); ++i) { + EXPECT_DOUBLE_EQ(trajectory.joint_trajectory.points[i].time_from_start, + trajectory.multi_dof_joint_trajectory.points[i].time_from_start); + } + std::vector time_intervals = {trajectory.joint_trajectory.points.front().time_from_start}; + for (uint32_t i = 1; i < trajectory.joint_trajectory.points.size(); ++i) { + time_intervals.push_back(trajectory.joint_trajectory.points[i].time_from_start - + trajectory.joint_trajectory.points[i - 1].time_from_start); + } + + // The first point is 0.0 + EXPECT_NEAR(0.0, time_intervals[0], kEpsilon); + // If it is less than the maximum value, the time is 0.1, less than 0.1 to fill the last. + for (uint32_t i = 1; i < time_intervals.size() - 1; ++i) { + EXPECT_NEAR(0.1, time_intervals[i], kEpsilon); + } + EXPECT_GT(0.1, time_intervals.back()); +} + +// Whether the Time_from_start in the orbit generated is changing according to the trajectory length, or a case where the number of orbital points exceeds the maximum value. +TEST_F(PlanarTrajectoryInterpolatorTest, TimeFromStartsLongTrajectory) { + tmc_manipulation_types::TimedRobotTrajectory trajectory; + ASSERT_TRUE(planar_->Interpolate(GenerateInitialState(), GenerateGoalState(), 1.0, trajectory)); + + ASSERT_EQ(30, trajectory.joint_trajectory.points.size()); + ASSERT_EQ(30, trajectory.multi_dof_joint_trajectory.points.size()); + + // Time check + for (uint32_t i = 0; i < trajectory.joint_trajectory.points.size(); ++i) { + EXPECT_DOUBLE_EQ(trajectory.joint_trajectory.points[i].time_from_start, + trajectory.multi_dof_joint_trajectory.points[i].time_from_start); + } + std::vector time_intervals = {trajectory.joint_trajectory.points.front().time_from_start}; + for (uint32_t i = 1; i < trajectory.joint_trajectory.points.size(); ++i) { + time_intervals.push_back(trajectory.joint_trajectory.points[i].time_from_start - + trajectory.joint_trajectory.points[i - 1].time_from_start); + } + + // The first point is 0.0 + EXPECT_NEAR(0.0, time_intervals[0], kEpsilon); + // Half is 0.1 chopped, and the rest is larger than 0.1, which is 0.1 where the orbital length matches. + for (uint32_t i = 1; i < 15; ++i) { + EXPECT_NEAR(0.1, time_intervals[i], kEpsilon); + } + for (uint32_t i = 16; i < time_intervals.size(); ++i) { + EXPECT_NEAR(time_intervals[15], time_intervals[i], kEpsilon); + } +} + +// There is no name in the initial joint state +TEST_F(PlanarTrajectoryInterpolatorTest, NoInitialJointNames) { + auto initial_state = GenerateInitialState(); + initial_state.joint_state.name.clear(); + + tmc_manipulation_types::TimedRobotTrajectory trajectory; + EXPECT_FALSE(planar_->Interpolate(initial_state, GenerateGoalState(), 0.5, trajectory)); + EXPECT_FALSE(planar_->Interpolate(initial_state, GenerateGoalState(), 0.5, GenerateRobotState(2.0), trajectory)); +} + +// There is no initial joint state +TEST_F(PlanarTrajectoryInterpolatorTest, NoInitialJointPositions) { + auto initial_state = GenerateInitialState(); + initial_state.joint_state.position = Eigen::VectorXd(); + + tmc_manipulation_types::TimedRobotTrajectory trajectory; + EXPECT_FALSE(planar_->Interpolate(initial_state, GenerateGoalState(), 0.5, trajectory)); + EXPECT_FALSE(planar_->Interpolate(initial_state, GenerateGoalState(), 0.5, GenerateRobotState(2.0), trajectory)); +} + +// There is no speed in the initial joint state +TEST_F(PlanarTrajectoryInterpolatorTest, NoInitialJointVelocities) { + auto initial_state = GenerateInitialState(); + initial_state.joint_state.velocity = Eigen::VectorXd(); + + tmc_manipulation_types::TimedRobotTrajectory trajectory; + EXPECT_FALSE(planar_->Interpolate(initial_state, GenerateGoalState(), 0.5, trajectory)); + EXPECT_FALSE(planar_->Interpolate(initial_state, GenerateGoalState(), 0.5, GenerateRobotState(2.0), trajectory)); +} + +// There is no name in the initial bogie state +TEST_F(PlanarTrajectoryInterpolatorTest, NoInitialBaseJointNames) { + auto initial_state = GenerateInitialState(); + initial_state.multi_dof_joint_state.names.clear(); + + tmc_manipulation_types::TimedRobotTrajectory trajectory; + EXPECT_FALSE(planar_->Interpolate(initial_state, GenerateGoalState(), 0.5, trajectory)); + EXPECT_FALSE(planar_->Interpolate(initial_state, GenerateGoalState(), 0.5, GenerateRobotState(2.0), trajectory)); +} + +// There is no posture in the initial bogie condition +TEST_F(PlanarTrajectoryInterpolatorTest, NoInitialBaseJointPoses) { + auto initial_state = GenerateInitialState(); + initial_state.multi_dof_joint_state.poses.clear(); + + tmc_manipulation_types::TimedRobotTrajectory trajectory; + EXPECT_FALSE(planar_->Interpolate(initial_state, GenerateGoalState(), 0.5, trajectory)); + EXPECT_FALSE(planar_->Interpolate(initial_state, GenerateGoalState(), 0.5, GenerateRobotState(2.0), trajectory)); +} + +// There is no speed in the initial bogie state +TEST_F(PlanarTrajectoryInterpolatorTest, NoInitialBaseTwists) { + auto initial_state = GenerateInitialState(); + initial_state.multi_dof_joint_state.poses.clear(); + + tmc_manipulation_types::TimedRobotTrajectory trajectory; + EXPECT_FALSE(planar_->Interpolate(initial_state, GenerateGoalState(), 0.5, trajectory)); + EXPECT_FALSE(planar_->Interpolate(initial_state, GenerateGoalState(), 0.5, GenerateRobotState(2.0), trajectory)); +} + +// There is no name in the goal joint state +TEST_F(PlanarTrajectoryInterpolatorTest, NoGoalJointNames) { + auto goal_state = GenerateGoalState(); + goal_state.joint_state.name.clear(); + + tmc_manipulation_types::TimedRobotTrajectory trajectory; + EXPECT_FALSE(planar_->Interpolate(GenerateInitialState(), goal_state, 0.5, trajectory)); + EXPECT_FALSE(planar_->Interpolate(GenerateInitialState(), goal_state, 0.5, GenerateRobotState(2.0), trajectory)); +} + +// There is no position in the goal joint state +TEST_F(PlanarTrajectoryInterpolatorTest, NoGoalJointPositions) { + auto goal_state = GenerateGoalState(); + goal_state.joint_state.position = Eigen::VectorXd(); + + tmc_manipulation_types::TimedRobotTrajectory trajectory; + EXPECT_FALSE(planar_->Interpolate(GenerateInitialState(), goal_state, 0.5, trajectory)); + EXPECT_FALSE(planar_->Interpolate(GenerateInitialState(), goal_state, 0.5, GenerateRobotState(2.0), trajectory)); +} + +// There is no speed in the goal joint, but it does not have to be +TEST_F(PlanarTrajectoryInterpolatorTest, NoGoalJointVelocities) { + auto goal_state = GenerateGoalState(); + goal_state.joint_state.velocity = Eigen::VectorXd(); + + tmc_manipulation_types::TimedRobotTrajectory trajectory; + EXPECT_TRUE(planar_->Interpolate(GenerateInitialState(), goal_state, 0.5, trajectory)); + EXPECT_TRUE(planar_->Interpolate(GenerateInitialState(), goal_state, 0.5, GenerateRobotState(2.0), trajectory)); +} + +// There is no name in the goal trolle +TEST_F(PlanarTrajectoryInterpolatorTest, NoGoalBaseJointNames) { + auto goal_state = GenerateGoalState(); + goal_state.multi_dof_joint_state.names.clear(); + + tmc_manipulation_types::TimedRobotTrajectory trajectory; + EXPECT_FALSE(planar_->Interpolate(GenerateInitialState(), goal_state, 0.5, trajectory)); + EXPECT_FALSE(planar_->Interpolate(GenerateInitialState(), goal_state, 0.5, GenerateRobotState(2.0), trajectory)); +} + +// There is no posture in the goal bogie condition +TEST_F(PlanarTrajectoryInterpolatorTest, NoGoalBaseJointPoses) { + auto goal_state = GenerateGoalState(); + goal_state.multi_dof_joint_state.poses.clear(); + + tmc_manipulation_types::TimedRobotTrajectory trajectory; + EXPECT_FALSE(planar_->Interpolate(GenerateInitialState(), goal_state, 0.5, trajectory)); + EXPECT_FALSE(planar_->Interpolate(GenerateInitialState(), goal_state, 0.5, GenerateRobotState(2.0), trajectory)); +} + +// There is no speed in the goal trolleys, but it doesn't have to be +TEST_F(PlanarTrajectoryInterpolatorTest, NoGoalBaseTwists) { + auto goal_state = GenerateGoalState(); + goal_state.multi_dof_joint_state.twist.clear(); + + tmc_manipulation_types::TimedRobotTrajectory trajectory; + EXPECT_TRUE(planar_->Interpolate(GenerateInitialState(), goal_state, 0.5, trajectory)); + EXPECT_TRUE(planar_->Interpolate(GenerateInitialState(), goal_state, 0.5, GenerateRobotState(2.0), trajectory)); +} + +// There is no name in the intermediate joint state +TEST_F(PlanarTrajectoryInterpolatorTest, NoMiddleJointNames) { + auto middle_state = GenerateRobotState(2.0); + middle_state.joint_state.name.clear(); + + tmc_manipulation_types::TimedRobotTrajectory trajectory; + EXPECT_FALSE(planar_->Interpolate(GenerateInitialState(), GenerateGoalState(), 0.5, middle_state, trajectory)); +} + +// There is no position in the intermediate joint state +TEST_F(PlanarTrajectoryInterpolatorTest, NoMiddleJointPositions) { + auto middle_state = GenerateRobotState(2.0); + middle_state.joint_state.position = Eigen::VectorXd(); + + tmc_manipulation_types::TimedRobotTrajectory trajectory; + EXPECT_FALSE(planar_->Interpolate(GenerateInitialState(), GenerateGoalState(), 0.5, middle_state, trajectory)); +} + +// There is no speed in the intermediate joint, but it does not have to be +TEST_F(PlanarTrajectoryInterpolatorTest, NoMiddleJointVelocities) { + auto middle_state = GenerateRobotState(2.0); + middle_state.joint_state.velocity = Eigen::VectorXd(); + + tmc_manipulation_types::TimedRobotTrajectory trajectory; + EXPECT_TRUE(planar_->Interpolate(GenerateInitialState(), GenerateGoalState(), 0.5, middle_state, trajectory)); +} + +// There is no name in the middle bogie state +TEST_F(PlanarTrajectoryInterpolatorTest, NoMiddleBaseJointNames) { + auto middle_state = GenerateRobotState(2.0); + middle_state.multi_dof_joint_state.names.clear(); + + tmc_manipulation_types::TimedRobotTrajectory trajectory; + EXPECT_FALSE(planar_->Interpolate(GenerateInitialState(), GenerateGoalState(), 0.5, middle_state, trajectory)); +} + +// There is no posture in the middle bogie state +TEST_F(PlanarTrajectoryInterpolatorTest, NoMiddleBaseJointPoses) { + auto middle_state = GenerateRobotState(2.0); + middle_state.multi_dof_joint_state.poses.clear(); + + tmc_manipulation_types::TimedRobotTrajectory trajectory; + EXPECT_FALSE(planar_->Interpolate(GenerateInitialState(), GenerateGoalState(), 0.5, middle_state, trajectory)); +} + +// There is no speed in the intermediate bogie, but it does not have to be +TEST_F(PlanarTrajectoryInterpolatorTest, NoMiddleBaseTwists) { + auto middle_state = GenerateRobotState(2.0); + middle_state.multi_dof_joint_state.twist.clear(); + + tmc_manipulation_types::TimedRobotTrajectory trajectory; + EXPECT_TRUE(planar_->Interpolate(GenerateInitialState(), GenerateGoalState(), 0.5, middle_state, trajectory)); +} + +// Since then, EXPECTED and Actual have turned upside down, but it is troublesome to fix it, so I will not mess with it. + +class PlanarTrajectoryInterpolatorPlainTest : public ::testing::Test { + protected: + void SetUp() override; + + rclcpp::Node::SharedPtr node_; + ITrajectoryInterpolator::Ptr planar_; +}; + +void PlanarTrajectoryInterpolatorPlainTest::SetUp() { + node_ = rclcpp::Node::make_shared("test_node"); + + planar_ = std::make_shared(); + planar_->Initialize(node_, GenerateTestParameters()); +} + +void ValidatePlainInterpolation(const tmc_manipulation_types::TimedRobotTrajectory& trajectory, + const tmc_manipulation_types::RobotState& initial_state, + const std::vector& path_states, + const std::vector& time_from_starts) { + EXPECT_EQ(trajectory.joint_trajectory.joint_names, kJointNames); + ASSERT_EQ(trajectory.joint_trajectory.points.size(), 1 + path_states.size()); + EXPECT_EQ(trajectory.joint_trajectory.points[0].positions, initial_state.joint_state.position); + EXPECT_EQ(trajectory.joint_trajectory.points[0].velocities, initial_state.joint_state.velocity); + for (auto i = 0; i < path_states.size(); ++i) { + // Goal's Velocity is meaningless (not used), so don't check it + EXPECT_EQ(trajectory.joint_trajectory.points[i + 1].positions, path_states[i].joint_state.position); + EXPECT_EQ(trajectory.joint_trajectory.points[i + 1].time_from_start, time_from_starts[i]); + } + + EXPECT_EQ(trajectory.multi_dof_joint_trajectory.joint_names, kBaseNames); + ASSERT_EQ(trajectory.multi_dof_joint_trajectory.points.size(), 1 + path_states.size()); + Eq(trajectory.multi_dof_joint_trajectory.points[0].transforms[0], initial_state.multi_dof_joint_state.poses[0]); + EXPECT_EQ(trajectory.multi_dof_joint_trajectory.points[0].velocities[0], + initial_state.multi_dof_joint_state.twist[0]); + for (auto i = 0; i < path_states.size(); ++i) { + Eq(trajectory.multi_dof_joint_trajectory.points[i + 1].transforms[0], + path_states[i].multi_dof_joint_state.poses[0]); + EXPECT_EQ(trajectory.multi_dof_joint_trajectory.points[i + 1].time_from_start, time_from_starts[i]); + } +} + +// Orbit conversion without intermediate point, intermediate route +TEST_F(PlanarTrajectoryInterpolatorPlainTest, NoMiddleStateNoMiddlePath) { + const auto initial_state = GenerateInitialState(); + const auto goal_state = GenerateGoalState(); + + tmc_manipulation_types::TimedRobotTrajectory trajectory; + ASSERT_TRUE(planar_->Interpolate(initial_state, goal_state, 0.5, trajectory)); + ValidatePlainInterpolation(trajectory, initial_state, {goal_state}, {5.0}); +} + +// There is no intermediate point, the intermediate route is the orbit conversion of the sky +TEST_F(PlanarTrajectoryInterpolatorPlainTest, NoMiddleStateEmptyMiddlePath) { + const auto initial_state = GenerateInitialState(); + const auto goal_state = GenerateGoalState(); + + tmc_manipulation_types::TimedRobotTrajectory trajectory; + ASSERT_TRUE(planar_->Interpolate(initial_state, {}, goal_state, 0.5, trajectory)); + ValidatePlainInterpolation(trajectory, initial_state, {goal_state}, {5.0}); +} + +// Orbit conversion with intermediate points and intermediate routes +TEST_F(PlanarTrajectoryInterpolatorPlainTest, NoMiddleStateWithMiddlePath) { + const auto initial_state = GenerateInitialState(); + const auto goal_state = GenerateGoalState(); + const auto middle_state = GenerateRobotState(2.0); + + tmc_manipulation_types::TimedRobotTrajectory trajectory; + ASSERT_TRUE(planar_->Interpolate(initial_state, {middle_state}, goal_state, 0.5, trajectory)); + ValidatePlainInterpolation(trajectory, initial_state, {middle_state, goal_state}, {10.0, 15.0}); +} + +// There is an intermediate point, orbit conversion without an intermediate route +TEST_F(PlanarTrajectoryInterpolatorPlainTest, WithMiddleStateNoMiddlePath) { + const auto initial_state = GenerateInitialState(); + const auto goal_state = GenerateGoalState(); + const auto middle_state = GenerateRobotState(2.0); + + tmc_manipulation_types::TimedRobotTrajectory trajectory; + ASSERT_TRUE(planar_->Interpolate(initial_state, goal_state, 0.5, middle_state, trajectory)); + ValidatePlainInterpolation(trajectory, initial_state, {middle_state, goal_state}, {10.0, 15.0}); +} + +// There is an intermediate point, the intermediate route is the orbit conversion of the sky +TEST_F(PlanarTrajectoryInterpolatorPlainTest, WithMiddleStateEmptyMiddlePath) { + const auto initial_state = GenerateInitialState(); + const auto goal_state = GenerateGoalState(); + const auto middle_state = GenerateRobotState(2.0); + + tmc_manipulation_types::TimedRobotTrajectory trajectory; + ASSERT_TRUE(planar_->Interpolate(initial_state, {}, goal_state, 0.5, middle_state, trajectory)); + ValidatePlainInterpolation(trajectory, initial_state, {middle_state, goal_state}, {10.0, 15.0}); +} + +// Orbit conversion with intermediate points and intermediate routes +TEST_F(PlanarTrajectoryInterpolatorPlainTest, WithMiddleStateAndMiddlePath) { + const auto initial_state = GenerateInitialState(); + const auto goal_state = GenerateGoalState(); + const auto middle_state = GenerateRobotState(2.0); + const auto middle_path_state = GenerateRobotState(3.0); + + tmc_manipulation_types::TimedRobotTrajectory trajectory; + ASSERT_TRUE(planar_->Interpolate(initial_state, {middle_path_state}, goal_state, 0.5, middle_state, trajectory)); + ValidatePlainInterpolation(trajectory, initial_state, {middle_state, middle_path_state, goal_state}, + {10.0, 15.0, 25.0}); +} + +// There is no name in the intermediate route joint state +TEST_F(PlanarTrajectoryInterpolatorPlainTest, NoMiddlePathJointNames) { + auto middle_state = GenerateRobotState(2.0); + middle_state.joint_state.name.clear(); + + tmc_manipulation_types::TimedRobotTrajectory trajectory; + EXPECT_FALSE(planar_->Interpolate(GenerateInitialState(), {middle_state}, GenerateGoalState(), 0.5, trajectory)); + EXPECT_FALSE(planar_->Interpolate(GenerateInitialState(), {middle_state}, GenerateGoalState(), 0.5, + GenerateRobotState(2.0), trajectory)); +} + +// There is no position in the intermediate route joints +TEST_F(PlanarTrajectoryInterpolatorPlainTest, NoMiddlePathJointPositions) { + auto middle_state = GenerateRobotState(2.0); + middle_state.joint_state.position = Eigen::VectorXd(); + + tmc_manipulation_types::TimedRobotTrajectory trajectory; + EXPECT_FALSE(planar_->Interpolate(GenerateInitialState(), {middle_state}, GenerateGoalState(), 0.5, trajectory)); + EXPECT_FALSE(planar_->Interpolate(GenerateInitialState(), {middle_state}, GenerateGoalState(), 0.5, + GenerateRobotState(2.0), trajectory)); +} + +// There is no speed in the intermediate route joint, but it is not necessary +TEST_F(PlanarTrajectoryInterpolatorPlainTest, NoMiddlePathJointVelocities) { + auto middle_state = GenerateRobotState(2.0); + middle_state.joint_state.velocity = Eigen::VectorXd(); + + tmc_manipulation_types::TimedRobotTrajectory trajectory; + EXPECT_TRUE(planar_->Interpolate(GenerateInitialState(), {middle_state}, GenerateGoalState(), 0.5, trajectory)); + EXPECT_TRUE(planar_->Interpolate(GenerateInitialState(), {middle_state}, GenerateGoalState(), 0.5, + GenerateRobotState(2.0), trajectory)); +} + +// There is no name in the intermediate route trolle +TEST_F(PlanarTrajectoryInterpolatorTest, NoMiddlePathBaseJointNames) { + auto middle_state = GenerateRobotState(2.0); + middle_state.multi_dof_joint_state.names.clear(); + + tmc_manipulation_types::TimedRobotTrajectory trajectory; + EXPECT_FALSE(planar_->Interpolate(GenerateInitialState(), {middle_state}, GenerateGoalState(), 0.5, trajectory)); + EXPECT_FALSE(planar_->Interpolate(GenerateInitialState(), {middle_state}, GenerateGoalState(), 0.5, + GenerateRobotState(2.0), trajectory)); +} + +// There is no posture in the middle route trolle +TEST_F(PlanarTrajectoryInterpolatorTest, NoMiddlePathBaseJointPoses) { + auto middle_state = GenerateRobotState(2.0); + middle_state.multi_dof_joint_state.poses.clear(); + + tmc_manipulation_types::TimedRobotTrajectory trajectory; + EXPECT_FALSE(planar_->Interpolate(GenerateInitialState(), {middle_state}, GenerateGoalState(), 0.5, trajectory)); + EXPECT_FALSE(planar_->Interpolate(GenerateInitialState(), {middle_state}, GenerateGoalState(), 0.5, + GenerateRobotState(2.0), trajectory)); +} + +// There is no speed in the middle route trolle, but it does not have to be +TEST_F(PlanarTrajectoryInterpolatorTest, NoMiddlePathBaseTwists) { + auto middle_state = GenerateRobotState(2.0); + middle_state.multi_dof_joint_state.twist.clear(); + + tmc_manipulation_types::TimedRobotTrajectory trajectory; + EXPECT_TRUE(planar_->Interpolate(GenerateInitialState(), {middle_state}, GenerateGoalState(), 0.5, trajectory)); + EXPECT_TRUE(planar_->Interpolate(GenerateInitialState(), {middle_state}, GenerateGoalState(), 0.5, + GenerateRobotState(2.0), trajectory)); +} + +} // namespace tmc_simple_path_generator + +int main(int argc, char **argv) { + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/tmc_simple_path_generator/test/sample_goal_generator-test.cpp b/tmc_simple_path_generator/test/sample_goal_generator-test.cpp new file mode 100644 index 0000000..1452ab6 --- /dev/null +++ b/tmc_simple_path_generator/test/sample_goal_generator-test.cpp @@ -0,0 +1,574 @@ +/* +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. +*/ +/// @brief SampleGoalGenerator test + +#include +#include +#include +#include + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace { +constexpr double kEpsilon = 1e-3; + +const std::vector kJointNames = {"joint1", "joint2", "joint3", "joint4", "joint5", "joint6"}; +const std::vector kBaseNames = {"world_joint"}; +const char* const kEndFrame = "link7"; + + +void SetIkInitialRange(const rclcpp::Node::SharedPtr& node, double value) { + node->set_parameter(rclcpp::Parameter("ik_initial_range", value)); +} + +double GetJointPosition(const tmc_manipulation_types::JointState& joint_state, const std::string& joint_name) { + auto index = tmc_manipulation_types::GetJointIndex(joint_state.name, joint_name); + return joint_state.position[index]; +} + +Eigen::Affine3d CalculateEndFramePose( + const tmc_robot_kinematics_model::IRobotKinematicsModel::Ptr& robot, + const tmc_manipulation_types::RobotState& state) { + robot->SetRobotTransform(state.multi_dof_joint_state.poses[0]); + robot->SetNamedAngle(state.joint_state); + return robot->GetObjectTransform(kEndFrame); +} + +tmc_simple_path_generator::SamplingParameters::Ptr GenerateSamplingParameters() { + auto params = std::make_shared(); + params->joint_names = kJointNames; + params->joint_weights.resize(kJointNames.size()); + params->joint_weights << 5.0, 1.0, 0.5, 0.5, 0.5, 1.0; + params->base_names = {"world_joint"}; + params->base_weights = Eigen::Vector3d(5.0, 5.0, 1.0); + params->joint_limit_upper.resize(kJointNames.size(), 3.0); + params->joint_limit_lower.resize(kJointNames.size(), -3.0); + params->base_movement_type = tmc_manipulation_types::kPlanar; + return params; +} + +tmc_manipulation_types::RobotState GenerateInitialState() { + tmc_manipulation_types::RobotState state; + state.joint_state.name = kJointNames; + state.joint_state.position.resize(kJointNames.size()); + state.joint_state.position << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6; + state.multi_dof_joint_state.names = kBaseNames; + state.multi_dof_joint_state.poses = { + Eigen::Translation3d(-0.1, -0.2, 0.0) * Eigen::Quaterniond(Eigen::AngleAxisd(-0.3, Eigen::Vector3d::UnitZ()))}; + return state; +} + +tmc_manipulation_types::RobotState GenerateGoalState() { + tmc_manipulation_types::RobotState state; + state.joint_state.name = kJointNames; + state.joint_state.position.resize(kJointNames.size()); + state.joint_state.position << 1.1, 1.2, 1.3, 1.4, 1.5, 1.6; + state.multi_dof_joint_state.names = kBaseNames; + state.multi_dof_joint_state.poses = {Eigen::Affine3d::Identity()}; + return state; +} + +std::vector GenerateJointConstraints( + const tmc_manipulation_types::RobotState& state) { + return {std::make_shared(state, state, 0, 0)}; +} + +std::vector GenerateJointConstraints() { + return GenerateJointConstraints(GenerateGoalState()); +} + +std::vector GenerateLinkConstraints( + const tmc_robot_kinematics_model::IRobotKinematicsModel::Ptr& robot, + const tmc_manipulation_types::RobotState& state) { + tmc_manipulation_types::TaskSpaceRegion tsr; + tsr.origin_frame_id = "odom"; + tsr.end_frame_id = kEndFrame; + tsr.origin_to_tsr = CalculateEndFramePose(robot, state); + tsr.tsr_to_end = Eigen::Affine3d::Identity(); + tsr.min_bounds = tmc_manipulation_types::RegionValues::Zero(); + tsr.max_bounds = tmc_manipulation_types::RegionValues::Zero(); + // I don't use Priority, so put 0 appropriately + return {std::make_shared(tsr, 0)}; +} + +std::vector GenerateLinkConstraints( + const tmc_robot_kinematics_model::IRobotKinematicsModel::Ptr& robot) { + return GenerateLinkConstraints(robot, GenerateGoalState()); +} + +} // namespace + +namespace tmc_simple_path_generator { + +template +class SampleGoalGeneratorTest : public ::testing::Test { + protected: + void SetUp() override; + + rclcpp::Node::SharedPtr node_; + SampleGoalGeneratorBase::Ptr sample_goal_generator_; + + std::vector Sample( + const tmc_manipulation_types::RobotState& initial_state, + const std::vector& constraints, + uint32_t sampling_num); + + std::vector Sample( + const tmc_manipulation_types::RobotState& initial_state, + const std::vector& constraints, + const SamplingParameters::Ptr& params, + uint32_t sampling_num); + + tmc_robot_kinematics_model::IRobotKinematicsModel::Ptr robot_; +}; + +template +void SampleGoalGeneratorTest::SetUp() { + const auto robot_description = tmc_manipulation_tests::stanford_manipulator::GetUrdf(); + robot_ = std::make_shared(robot_description); + + node_ = rclcpp::Node::make_shared("test_node"); + node_->declare_parameter("robot_description_kinematics", robot_description); + sample_goal_generator_ = std::make_shared(); + // Because it is a test, the SEED is fixed + sample_goal_generator_->Initialize(node_, 0); + SetIkInitialRange(node_, 0.5); +} + +template +std::vector SampleGoalGeneratorTest::Sample( + const tmc_manipulation_types::RobotState& initial_state, + const std::vector& constraints, + uint32_t sampling_num) { + // AMD Ryzen 9 3950X was about 100ms even if Sampling_num was 100 ms, so it is said that at least 100ms will increase the sampling_num. + const auto end_time = std::chrono::system_clock::now() + std::chrono::duration(0.1 + 0.02 * sampling_num); + std::vector result; + while (result.size() < sampling_num && std::chrono::system_clock::now() < end_time) { + // SampleFromJointConstraints are prerequisite for Goal_state. + // It feels better to insert Initial_state clearly. + auto goal_state = initial_state; + // Since it is a prerequisite to be called multiple times, there is no success or failure once. + if (sample_goal_generator_->SampleFromJointConstraints(constraints, goal_state)) { + result.emplace_back(goal_state); + } + } + sample_goal_generator_->Terminate(); + return result; +} + +template +std::vector SampleGoalGeneratorTest::Sample( + const tmc_manipulation_types::RobotState& initial_state, + const std::vector& constraints, + const SamplingParameters::Ptr& params, + uint32_t sampling_num) { + const auto end_time = std::chrono::system_clock::now() + std::chrono::duration(0.1 + 0.02 * sampling_num); + std::vector result; + while (result.size() < sampling_num && std::chrono::system_clock::now() < end_time) { + tmc_manipulation_types::RobotState goal_state; + if (sample_goal_generator_->SampleFromLinkConstraints(initial_state, constraints, params, goal_state)) { + result.emplace_back(goal_state); + } + } + sample_goal_generator_->Terminate(); + return result; +} + +TYPED_TEST_SUITE_P(SampleGoalGeneratorTest); + +TYPED_TEST_P(SampleGoalGeneratorTest, SampleFromJointConstraints) { + const auto initial_state = GenerateInitialState(); + const auto joint_constraints = GenerateJointConstraints(); + const auto results = this->Sample(initial_state, joint_constraints, 1); + + ASSERT_EQ(results.size(), 1); + EXPECT_LT(joint_constraints[0]->CalcDisplacement(results[0]), kEpsilon); +} + +TYPED_TEST_P(SampleGoalGeneratorTest, RandomSampleJointGoals) { + const auto initial_state = GenerateInitialState(); + auto joint_constraints = GenerateJointConstraints(); + + auto state = GenerateGoalState(); + std::vector goal_position = {2.1, 2.2, 2.3, 2.4, 2.5, 2.6}; + state.joint_state.position = Eigen::Map(&goal_position[0], goal_position.size()); + joint_constraints.emplace_back(GenerateJointConstraints(state)[0]); + + const auto results = this->Sample(initial_state, joint_constraints, 100); + + ASSERT_EQ(results.size(), 100); + + uint32_t count = 0; + for (const auto goal_state : results) { + if (joint_constraints[0]->CalcDisplacement(goal_state) < kEpsilon) { + ++count; + } + } + // Since it is a binary distribution of 100 samples, 3σ is 15 + EXPECT_NEAR(count, 50, 15); +} + +TYPED_TEST_P(SampleGoalGeneratorTest, SampleFromLinkConstraints) { + const auto initial_state = GenerateInitialState(); + const auto link_constraints = GenerateLinkConstraints(this->robot_); + const auto params = GenerateSamplingParameters(); + const auto results = this->Sample(initial_state, link_constraints, params, 1); + + ASSERT_EQ(results.size(), 1); + EXPECT_LT(link_constraints[0]->CalcDisplacement(CalculateEndFramePose(this->robot_, results[0])), kEpsilon); +} + +TYPED_TEST_P(SampleGoalGeneratorTest, RandomSampleLinkGoals) { + const auto initial_state = GenerateInitialState(); + auto link_constraints = GenerateLinkConstraints(this->robot_); + + auto state = GenerateGoalState(); + state.multi_dof_joint_state.poses[0] *= Eigen::Translation3d(1.0, 0.0, 0.0); + link_constraints.emplace_back(GenerateLinkConstraints(this->robot_, state)[0]); + + const auto params = GenerateSamplingParameters(); + const auto results = this->Sample(initial_state, link_constraints, params, 100); + + ASSERT_EQ(results.size(), 100); + + uint32_t count = 0; + for (const auto goal_state : results) { + if (link_constraints[0]->CalcDisplacement(CalculateEndFramePose(this->robot_, goal_state)) < kEpsilon) { + ++count; + } + } + EXPECT_NEAR(count, 50, 15); +} + +TYPED_TEST_P(SampleGoalGeneratorTest, SampleFromLinkConstraintsWithoutBase) { + const auto initial_state = GenerateInitialState(); + + auto goal_state = initial_state; + goal_state.joint_state.position[0] += 1.0; + const auto link_constraints = GenerateLinkConstraints(this->robot_, goal_state); + + const auto params = GenerateSamplingParameters(); + auto removed_params = params->RemoveJoints({"world_joint"}); + const auto results = this->Sample(initial_state, link_constraints, removed_params, 1); + + ASSERT_EQ(results.size(), 1); + EXPECT_LT(link_constraints[0]->CalcDisplacement(CalculateEndFramePose(this->robot_, results[0])), kEpsilon); + + auto base_pose_initial = tmc_robot_local_planner_utils::Get2DPose(initial_state.multi_dof_joint_state.poses[0]); + auto base_pose_result = tmc_robot_local_planner_utils::Get2DPose(results[0].multi_dof_joint_state.poses[0]); + EXPECT_LT((base_pose_result - base_pose_initial).norm(), kEpsilon); +} + +TYPED_TEST_P(SampleGoalGeneratorTest, SampleFromLinkConstraintsWithIgnoreJoint) { + const auto initial_state = GenerateInitialState(); + + auto goal_state = initial_state; + goal_state.joint_state.position[1] += 1.0; + const auto link_constraints = GenerateLinkConstraints(this->robot_, goal_state); + + const auto params = GenerateSamplingParameters(); + auto removed_params = params->RemoveJoints({"joint1"}); + const auto results = this->Sample(initial_state, link_constraints, removed_params, 1); + + ASSERT_EQ(results.size(), 1); + EXPECT_LT(link_constraints[0]->CalcDisplacement(CalculateEndFramePose(this->robot_, results[0])), kEpsilon); + + EXPECT_NEAR(results[0].joint_state.position[0], initial_state.joint_state.position[0], kEpsilon); +} + +TYPED_TEST_P(SampleGoalGeneratorTest, SampleFromLinkConstraintsFromRandomState) { + // Generate Link_constraint that satisfies the position/posture by the initial value and the initial value + const auto initial_state = GenerateInitialState(); + const auto link_constraints = GenerateLinkConstraints(this->robot_, initial_state); + const auto params = GenerateSamplingParameters(); + + { + this->sample_goal_generator_->set_is_from_random_initial_state(false); + + const auto results = this->Sample(initial_state, link_constraints, params, 1); + + ASSERT_EQ(results.size(), 1); + EXPECT_LT(link_constraints[0]->CalcDisplacement(CalculateEndFramePose(this->robot_, results[0])), kEpsilon); + + // Since the goal is filled with the initial value, the same as Initial_state + for (auto i = 0; i < initial_state.joint_state.name.size(); ++i) { + const auto goal_position = GetJointPosition(results[0].joint_state, initial_state.joint_state.name[i]); + EXPECT_DOUBLE_EQ(initial_state.joint_state.position[i], goal_position); + } + EXPECT_DOUBLE_EQ(initial_state.multi_dof_joint_state.poses[0].translation().x(), + results[0].multi_dof_joint_state.poses[0].translation().x()); + EXPECT_DOUBLE_EQ(initial_state.multi_dof_joint_state.poses[0].translation().y(), + results[0].multi_dof_joint_state.poses[0].translation().y()); + EXPECT_DOUBLE_EQ(initial_state.multi_dof_joint_state.poses[0].rotation().eulerAngles(0, 1, 2)[2], + results[0].multi_dof_joint_state.poses[0].rotation().eulerAngles(0, 1, 2)[2]); + } + + { + this->sample_goal_generator_->set_is_from_random_initial_state(true); + + const auto results = this->Sample(initial_state, link_constraints, params, 1); + + ASSERT_EQ(results.size(), 1); + EXPECT_LT(link_constraints[0]->CalcDisplacement(CalculateEndFramePose(this->robot_, results[0])), kEpsilon); + + // You can add the bogie position, but it should be enough to judge only with the arm joint. + double diff = 0.0; + for (auto i = 0; i < initial_state.joint_state.name.size(); ++i) { + const auto goal_position = GetJointPosition(results[0].joint_state, initial_state.joint_state.name[i]); + diff += std::abs(goal_position - initial_state.joint_state.position[i]); + } + EXPECT_GT(diff, 0.0); + } +} + +TYPED_TEST_P(SampleGoalGeneratorTest, JointNamesOrderMismatch) { + const auto initial_state = GenerateInitialState(); + const auto link_constraints = GenerateLinkConstraints(this->robot_, initial_state); + auto params = GenerateSamplingParameters(); + params->joint_names = {"joint6", "joint1", "joint2", "joint3", "joint4", "joint5"}; + + this->sample_goal_generator_->set_is_from_random_initial_state(false); + const auto results = this->Sample(initial_state, link_constraints, params, 1); + + ASSERT_EQ(results.size(), 1); + EXPECT_LT(link_constraints[0]->CalcDisplacement(CalculateEndFramePose(this->robot_, results[0])), kEpsilon); + + // Since the goal is filled with the initial value, the same as Initial_state + for (auto i = 0; i < initial_state.joint_state.name.size(); ++i) { + const auto goal_position = GetJointPosition(results[0].joint_state, initial_state.joint_state.name[i]); + EXPECT_DOUBLE_EQ(initial_state.joint_state.position[i], goal_position); + } + EXPECT_DOUBLE_EQ(initial_state.multi_dof_joint_state.poses[0].translation().x(), + results[0].multi_dof_joint_state.poses[0].translation().x()); + EXPECT_DOUBLE_EQ(initial_state.multi_dof_joint_state.poses[0].translation().y(), + results[0].multi_dof_joint_state.poses[0].translation().y()); + EXPECT_DOUBLE_EQ(initial_state.multi_dof_joint_state.poses[0].rotation().eulerAngles(0, 1, 2)[2], + results[0].multi_dof_joint_state.poses[0].rotation().eulerAngles(0, 1, 2)[2]); +} + +TYPED_TEST_P(SampleGoalGeneratorTest, TerminateImmediately) { + const auto initial_state = GenerateInitialState(); + const auto link_constraints = GenerateLinkConstraints(this->robot_); + const auto params = GenerateSamplingParameters(); + + const auto end_time = std::chrono::system_clock::now() + std::chrono::duration(0.1); + while (std::chrono::system_clock::now() < end_time) { + auto goal_state = initial_state; + this->sample_goal_generator_->SampleFromLinkConstraints(initial_state, link_constraints, params, goal_state); + } + + const auto start = std::chrono::system_clock::now(); + this->sample_goal_generator_->Terminate(); + const auto end = std::chrono::system_clock::now(); + + // There is no particular basis for 5ms, the whole orbit candidate processing processing is at most 100ms, so the idea is that it is at least about 5ms. + EXPECT_LE(std::chrono::duration_cast(end - start).count(), 5); +} + +// Arthrangular restraint empty +TYPED_TEST_P(SampleGoalGeneratorTest, NoJointConstraints) { + const auto initial_state = GenerateInitialState(); + const auto joint_constraints = GenerateJointConstraints(); + const auto results = this->Sample(initial_state, {}, 1); + + EXPECT_TRUE(results.empty()); +} + +// When sampling the goal posture from joint restraint, there is no name in the initial joint state +TYPED_TEST_P(SampleGoalGeneratorTest, SampleFromJointConstraintsWithNoInitialJointNames) { + auto initial_state = GenerateInitialState(); + initial_state.joint_state.name.clear(); + const auto joint_constraints = GenerateJointConstraints(); + const auto results = this->Sample(initial_state, joint_constraints, 1); + + EXPECT_TRUE(results.empty()); +} + +// When sampling the goal posture from joint restraint, there is no initial joint state. +TYPED_TEST_P(SampleGoalGeneratorTest, SampleFromJointConstraintsWithNoInitialJointPositions) { + auto initial_state = GenerateInitialState(); + initial_state.joint_state.position = Eigen::VectorXd(); + const auto joint_constraints = GenerateJointConstraints(); + const auto results = this->Sample(initial_state, joint_constraints, 1); + + EXPECT_TRUE(results.empty()); +} + +// Posture restraint is empty +TYPED_TEST_P(SampleGoalGeneratorTest, NoLinkConstraints) { + const auto initial_state = GenerateInitialState(); + const auto params = GenerateSamplingParameters(); + const auto results = this->Sample(initial_state, {}, params, 1); + + EXPECT_TRUE(results.empty()); +} + +// When sampling the goal posture from the posture restraint, there is no name in the initial joint state +TYPED_TEST_P(SampleGoalGeneratorTest, SampleFromLinkConstraintsWithNoInitialJointNames) { + auto initial_state = GenerateInitialState(); + initial_state.joint_state.name.clear(); + const auto link_constraints = GenerateLinkConstraints(this->robot_); + const auto params = GenerateSamplingParameters(); + const auto results = this->Sample(initial_state, link_constraints, params, 1); + + EXPECT_TRUE(results.empty()); +} + +// When sampling the goal posture from the posture restraint, there is no initial joint status +TYPED_TEST_P(SampleGoalGeneratorTest, SampleFromLinkConstraintsWithNoInitialJointPositions) { + auto initial_state = GenerateInitialState(); + initial_state.joint_state.position = Eigen::VectorXd(); + const auto link_constraints = GenerateLinkConstraints(this->robot_); + const auto params = GenerateSamplingParameters(); + const auto results = this->Sample(initial_state, link_constraints, params, 1); + + EXPECT_TRUE(results.empty()); +} + +// There is no terminal frame +TYPED_TEST_P(SampleGoalGeneratorTest, InvalidEndFrame) { + auto initial_state = GenerateInitialState(); + + tmc_manipulation_types::TaskSpaceRegion tsr; + tsr.origin_frame_id = "odom"; + tsr.end_frame_id = "NoLink"; + tsr.origin_to_tsr = CalculateEndFramePose(this->robot_, GenerateGoalState()); + tsr.tsr_to_end = Eigen::Affine3d::Identity(); + tsr.min_bounds = tmc_manipulation_types::RegionValues::Zero(); + tsr.max_bounds = tmc_manipulation_types::RegionValues::Zero(); + + std::vector link_constraints = { + std::make_shared(tsr, 0)}; + + const auto params = GenerateSamplingParameters(); + const auto results = this->Sample(initial_state, link_constraints, params, 1); + + EXPECT_TRUE(results.empty()); +} + +// Reverse athletic is not required +TYPED_TEST_P(SampleGoalGeneratorTest, IKCannotSolve) { + const auto initial_state = GenerateInitialState(); + const auto link_constraints = GenerateLinkConstraints(this->robot_); + auto params = GenerateSamplingParameters(); + params->base_movement_type = tmc_manipulation_types::kNone; + const auto results = this->Sample(initial_state, link_constraints, params, 1); + + EXPECT_TRUE(results.empty()); +} + +REGISTER_TYPED_TEST_SUITE_P(SampleGoalGeneratorTest, + SampleFromJointConstraints, + RandomSampleJointGoals, + SampleFromLinkConstraints, + RandomSampleLinkGoals, + SampleFromLinkConstraintsWithoutBase, + SampleFromLinkConstraintsWithIgnoreJoint, + SampleFromLinkConstraintsFromRandomState, + JointNamesOrderMismatch, + TerminateImmediately, + NoJointConstraints, + SampleFromJointConstraintsWithNoInitialJointNames, + SampleFromJointConstraintsWithNoInitialJointPositions, + NoLinkConstraints, + SampleFromLinkConstraintsWithNoInitialJointNames, + SampleFromLinkConstraintsWithNoInitialJointPositions, + InvalidEndFrame, + IKCannotSolve); + +INSTANTIATE_TYPED_TEST_SUITE_P(SampleGoalGeneratorSingleThreadTest, + SampleGoalGeneratorTest, SampleGoalGenerator); +INSTANTIATE_TYPED_TEST_SUITE_P(SampleGoalGeneratorMultiThreadTest, + SampleGoalGeneratorTest, SampleGoalGeneratorMultiThread); + +TEST(SampleGoalGeneratorTest, CreateInstance) { + pluginlib::ClassLoader loader("tmc_simple_path_generator", + "tmc_simple_path_generator::SampleGoalGeneratorBase"); + { + const auto sampler = loader.createSharedInstance("tmc_simple_path_generator/SampleGoalGenerator"); + } + { + const auto sampler = loader.createSharedInstance("tmc_simple_path_generator/SampleGoalGeneratorMultiThread"); + } +} + +template +uint32_t Sample() { + const auto robot_description = tmc_manipulation_tests::stanford_manipulator::GetUrdf(); + auto robot = std::make_shared(robot_description); + + auto node = rclcpp::Node::make_shared("test_node"); + node->declare_parameter("robot_description_kinematics", robot_description); + node->declare_parameter("thread_pool_size", 8); + + auto sample_goal_generator = std::make_shared(); + sample_goal_generator->Initialize(node, 0); + sample_goal_generator->set_is_from_random_initial_state(true); + + const auto initial_state = GenerateInitialState(); + const auto link_constraints = GenerateLinkConstraints(robot); + const auto params = GenerateSamplingParameters(); + + uint32_t count = 0; + const auto end_time = std::chrono::system_clock::now() + std::chrono::duration(1.0); + while (std::chrono::system_clock::now() < end_time) { + tmc_manipulation_types::RobotState goal_state; + if (sample_goal_generator->SampleFromLinkConstraints(initial_state, link_constraints, params, goal_state)) { + ++count; + } + } + return count; +} + +TEST(SampleGoalGeneratorTest, CalculationSpeed) { + // Even if you think about various overhead, if the number of threads is 8, it should be able to be generated. + EXPECT_GT(Sample(), 2 * Sample()); +} + + +} // namespace tmc_simple_path_generator + +int main(int argc, char **argv) { + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/tmc_simple_path_generator/test/sample_middle_generator-test.cpp b/tmc_simple_path_generator/test/sample_middle_generator-test.cpp new file mode 100644 index 0000000..f908bce --- /dev/null +++ b/tmc_simple_path_generator/test/sample_middle_generator-test.cpp @@ -0,0 +1,278 @@ +/* +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. +*/ +#include + +#include + +#include + +namespace { +const std::vector kJointNames = {"arm", "hand", "shoulder"}; +const std::vector kBaseNames = {"world_joint"}; +constexpr double kEpsilon = 1.0e-6; + +tmc_simple_path_generator::SamplingParameters::Ptr GenerateTestParameters() { + auto params = std::make_shared(); + params->joint_names = kJointNames; + params->joint_weights = Eigen::Vector3d(5.0, 1.0, 0.5); + params->base_names = kBaseNames; + params->base_weights = Eigen::Vector3d(5.0, 5.0, 1.0); + params->joint_limit_upper = {0.1, 0.2, 0.3}; + params->joint_limit_lower = {0.0, 0.1, 0.2}; + return params; +} + +tmc_manipulation_types::RobotState GenerateRobotState() { + tmc_manipulation_types::RobotState state; + state.joint_state.name = kJointNames; + state.joint_state.position = Eigen::VectorXd::Zero(kJointNames.size()); + state.multi_dof_joint_state.names = kBaseNames; + state.multi_dof_joint_state.poses.push_back(Eigen::Affine3d::Identity()); + return state; +} + +void IsMinMaxNotEqual(std::vector> joint_positions) { + for (auto& values : joint_positions) { + auto minmax = std::minmax_element(values.begin(), values.end()); + EXPECT_NE(*minmax.first, *minmax.second); + } +} + +} // namespace + +namespace tmc_simple_path_generator { + +TEST(PlanerMiddleStateGeneratorPluginTest, CreateInstance) { + pluginlib::ClassLoader class_loader( + "tmc_simple_path_generator", "tmc_simple_path_generator::ISampleMiddleGenerator"); + auto interpolator = class_loader.createSharedInstance("tmc_simple_path_generator/PlanerMiddleStateGenerator"); +} + +class PlanerMiddleStateGeneratorTest : public ::testing::Test { + protected: + void SetUp() override; + + rclcpp::Node::SharedPtr node_; + ISampleMiddleGenerator::Ptr sampler_; +}; + +void PlanerMiddleStateGeneratorTest::SetUp() { + node_ = rclcpp::Node::make_shared("test_node"); + sampler_ = std::make_shared(); + sampler_->Initialize(node_, GenerateTestParameters()); +} + +// If the random width is 0, the arm is in its position, the bogie is between the initial state and the middle of the goal. +TEST_F(PlanerMiddleStateGeneratorTest, ComputeRandomMiddleStateZeroRange) { + node_->set_parameter(rclcpp::Parameter("middle_state_base_position_range", 0.0)); + node_->set_parameter(rclcpp::Parameter("middle_state_base_rotation_range", 0.0)); + rclcpp::spin_some(node_); + + auto params = GenerateTestParameters(); + params->joint_limit_upper = {1.0, 2.0, 3.0}; + params->joint_limit_lower = {1.0, 2.0, 3.0}; + + const auto initial_state = GenerateRobotState(); + auto goal_state = GenerateRobotState(); + // Joint_state.position is ignored, but set to confirm that it is ignored + goal_state.joint_state.position << 0.2, 0.4, 0.6; + goal_state.multi_dof_joint_state.poses[0] = + Eigen::Translation3d(0.8, 1.0, 0.0) * Eigen::AngleAxisd(1.2, Eigen::Vector3d::UnitZ()); + + const auto middle_state = sampler_->ComputeRandomMiddleState(initial_state, goal_state, params); + ASSERT_TRUE(middle_state.has_value()); + + EXPECT_EQ(middle_state.value().joint_state.name, kJointNames); + ASSERT_EQ(middle_state.value().joint_state.position.size(), 3); + EXPECT_NEAR(middle_state.value().joint_state.position[0], 1.0, kEpsilon); + EXPECT_NEAR(middle_state.value().joint_state.position[1], 2.0, kEpsilon); + EXPECT_NEAR(middle_state.value().joint_state.position[2], 3.0, kEpsilon); + + EXPECT_EQ(middle_state.value().multi_dof_joint_state.names, kBaseNames); + ASSERT_EQ(middle_state.value().multi_dof_joint_state.poses.size(), 1); + EXPECT_NEAR(middle_state.value().multi_dof_joint_state.poses[0].translation().x(), 0.4, kEpsilon); + EXPECT_NEAR(middle_state.value().multi_dof_joint_state.poses[0].translation().y(), 0.5, kEpsilon); + EXPECT_NEAR(middle_state.value().multi_dof_joint_state.poses[0].linear().eulerAngles(0, 1, 2)[2], 0.6, kEpsilon); +} + +// The arm is random between the lower limit, the bogie is randomly added between the initial state and the goal state. +TEST_F(PlanerMiddleStateGeneratorTest, ComputeRandomMiddleState) { + node_->set_parameter(rclcpp::Parameter("middle_state_base_position_range", 0.05)); + node_->set_parameter(rclcpp::Parameter("middle_state_base_rotation_range", 0.01)); + rclcpp::spin_some(node_); + + const auto initial_state = GenerateRobotState(); + auto goal_state = GenerateRobotState(); + goal_state.multi_dof_joint_state.poses[0] = Eigen::Translation3d(0.8, 1.0, 0.0) * Eigen::AngleAxisd::Identity(); + + // There is no particular basis for the number of times + std::vector> joint_positions(6, std::vector({})); + for (auto i = 0; i < 1000; ++i) { + const auto middle_state = sampler_->ComputeRandomMiddleState(initial_state, goal_state, GenerateTestParameters()); + ASSERT_TRUE(middle_state.has_value()); + + EXPECT_EQ(middle_state.value().joint_state.name, kJointNames); + ASSERT_EQ(middle_state.value().joint_state.position.size(), 3); + EXPECT_GE(middle_state.value().joint_state.position[0], 0.0); + EXPECT_LE(middle_state.value().joint_state.position[0], 0.1); + EXPECT_GE(middle_state.value().joint_state.position[1], 0.1); + EXPECT_LE(middle_state.value().joint_state.position[1], 0.2); + EXPECT_GE(middle_state.value().joint_state.position[2], 0.2); + EXPECT_LE(middle_state.value().joint_state.position[2], 0.3); + + EXPECT_EQ(middle_state.value().multi_dof_joint_state.names, kBaseNames); + ASSERT_EQ(middle_state.value().multi_dof_joint_state.poses.size(), 1); + EXPECT_NEAR(middle_state.value().multi_dof_joint_state.poses[0].translation().x(), 0.4, 0.05); + EXPECT_NEAR(middle_state.value().multi_dof_joint_state.poses[0].translation().y(), 0.5, 0.05); + EXPECT_NEAR(middle_state.value().multi_dof_joint_state.poses[0].linear().eulerAngles(0, 1, 2)[2], 0.0, 0.01); + + joint_positions[0].push_back(middle_state.value().joint_state.position[0]); + joint_positions[1].push_back(middle_state.value().joint_state.position[1]); + joint_positions[2].push_back(middle_state.value().joint_state.position[2]); + joint_positions[3].push_back(middle_state.value().multi_dof_joint_state.poses[0].translation().x()); + joint_positions[4].push_back(middle_state.value().multi_dof_joint_state.poses[0].translation().y()); + joint_positions[5].push_back(middle_state.value().multi_dof_joint_state.poses[0].linear().eulerAngles(0, 1, 2)[2]); + } + // Random tests are troublesome, so only confirm that min-max is not the same. + IsMinMaxNotEqual(joint_positions); +} + +// Remove the bogie from the target +TEST_F(PlanerMiddleStateGeneratorTest, IgnoreBase) { + node_->set_parameter(rclcpp::Parameter("middle_state_base_position_range", 0.05)); + node_->set_parameter(rclcpp::Parameter("middle_state_base_rotation_range", 0.01)); + rclcpp::spin_some(node_); + + auto params = GenerateTestParameters(); + params->base_names.clear(); + + const auto initial_state = GenerateRobotState(); + auto goal_state = GenerateRobotState(); + goal_state.multi_dof_joint_state.poses[0] = + Eigen::Translation3d(0.8, 1.0, 0.0) * Eigen::AngleAxisd(1.2, Eigen::Vector3d::UnitZ()); + + std::vector> joint_positions(3, std::vector({})); + for (auto i = 0; i < 10; ++i) { + const auto middle_state = sampler_->ComputeRandomMiddleState(initial_state, goal_state, params); + ASSERT_TRUE(middle_state.has_value()); + + EXPECT_EQ(middle_state.value().joint_state.name, kJointNames); + ASSERT_EQ(middle_state.value().joint_state.position.size(), 3); + + // Since it was removed from the target, the bogie is not random, but the intermediate value + EXPECT_EQ(middle_state.value().multi_dof_joint_state.names, kBaseNames); + ASSERT_EQ(middle_state.value().multi_dof_joint_state.poses.size(), 1); + EXPECT_NEAR(middle_state.value().multi_dof_joint_state.poses[0].translation().x(), 0.4, kEpsilon); + EXPECT_NEAR(middle_state.value().multi_dof_joint_state.poses[0].translation().y(), 0.5, kEpsilon); + EXPECT_NEAR(middle_state.value().multi_dof_joint_state.poses[0].linear().eulerAngles(0, 1, 2)[2], 0.6, kEpsilon); + + joint_positions[0].push_back(middle_state.value().joint_state.position[0]); + joint_positions[1].push_back(middle_state.value().joint_state.position[1]); + joint_positions[2].push_back(middle_state.value().joint_state.position[2]); + } + IsMinMaxNotEqual(joint_positions); +} + +// Remove joints from the target +TEST_F(PlanerMiddleStateGeneratorTest, IgnoreJoint) { + const auto params = GenerateTestParameters()->RemoveJoints({kJointNames[2]}); + + const auto initial_state = GenerateRobotState(); + auto goal_state = GenerateRobotState(); + goal_state.joint_state.position << 0.2, 0.4, 0.6; + + // Only Index2 removed from the target is not random but intermediate value + std::vector> joint_positions(5, std::vector({})); + for (auto i = 0; i < 10; ++i) { + const auto middle_state = sampler_->ComputeRandomMiddleState(initial_state, goal_state, params); + ASSERT_TRUE(middle_state.has_value()); + + EXPECT_EQ(middle_state.value().joint_state.name, kJointNames); + ASSERT_EQ(middle_state.value().joint_state.position.size(), 3); + EXPECT_NEAR(middle_state.value().joint_state.position[2], 0.3, kEpsilon); + + joint_positions[0].push_back(middle_state.value().joint_state.position[0]); + joint_positions[1].push_back(middle_state.value().joint_state.position[1]); + joint_positions[2].push_back(middle_state.value().multi_dof_joint_state.poses[0].translation().x()); + joint_positions[3].push_back(middle_state.value().multi_dof_joint_state.poses[0].translation().y()); + joint_positions[4].push_back(middle_state.value().multi_dof_joint_state.poses[0].linear().eulerAngles(0, 1, 2)[2]); + } + IsMinMaxNotEqual(joint_positions); +} + +// Initial_state's arm joint is insufficient +TEST_F(PlanerMiddleStateGeneratorTest, InvalidInitialArmJoint) { + auto initial_state = GenerateRobotState(); + initial_state.joint_state.name = {"arm", "hand"}; + initial_state.joint_state.position = Eigen::VectorXd::Zero(2); + + const auto middle_state = sampler_->ComputeRandomMiddleState( + initial_state, GenerateRobotState(), GenerateTestParameters()); + EXPECT_FALSE(middle_state.has_value()); +} + +// Initial_state's bogie joint is deficient +TEST_F(PlanerMiddleStateGeneratorTest, InvalidInitialBaseJoint) { + auto initial_state = GenerateRobotState(); + initial_state.multi_dof_joint_state.names.clear(); + initial_state.multi_dof_joint_state.poses.clear(); + + const auto middle_state = sampler_->ComputeRandomMiddleState( + initial_state, GenerateRobotState(), GenerateTestParameters()); + EXPECT_FALSE(middle_state.has_value()); +} + +// There is a shortage of Goal_state's arm joints +TEST_F(PlanerMiddleStateGeneratorTest, InvalidGoalArmJoint) { + auto goal_state = GenerateRobotState(); + goal_state.joint_state.name = {"arm", "hand"}; + goal_state.joint_state.position = Eigen::VectorXd::Zero(2); + + const auto middle_state = sampler_->ComputeRandomMiddleState( + GenerateRobotState(), goal_state, GenerateTestParameters()); + EXPECT_FALSE(middle_state.has_value()); +} + +// There is a shortage of Goal_state bogie joints +TEST_F(PlanerMiddleStateGeneratorTest, InvalidGoalBaseJoint) { + auto goal_state = GenerateRobotState(); + goal_state.multi_dof_joint_state.names.clear(); + goal_state.multi_dof_joint_state.poses.clear(); + + const auto middle_state = sampler_->ComputeRandomMiddleState( + GenerateRobotState(), goal_state, GenerateTestParameters()); + EXPECT_FALSE(middle_state.has_value()); +} + +} // namespace tmc_simple_path_generator + +int main(int argc, char **argv) { + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/tmc_simple_path_generator/test/simple_path_generator-test.cpp b/tmc_simple_path_generator/test/simple_path_generator-test.cpp new file mode 100644 index 0000000..57227f6 --- /dev/null +++ b/tmc_simple_path_generator/test/simple_path_generator-test.cpp @@ -0,0 +1,345 @@ +/* +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. +*/ +/// @brief SimplePathgenerator test + +#include +#include + +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + + +namespace { +// Robot joint name +const tmc_manipulation_types::NameSeq kJointNames = { + "joint1", "joint2", "joint3", "joint4", "joint5", "joint6"}; +// Robot bogie name +const tmc_manipulation_types::NameSeq kBaseNames = {"world_joint"}; +} // namespace + +namespace tmc_simple_path_generator { + +// Differences in behavior due to differences in parameters are performed by node testing. +TEST(SimplePathGeneratorPluginTest, Generate) { + pluginlib::ClassLoader loader( + "tmc_robot_local_planner", "tmc_robot_local_planner::IGenerator"); + auto generator = loader.createSharedInstance("tmc_simple_path_generator/SimplePathGeneratorPlugin"); + + auto node = rclcpp::Node::make_shared("plugins"); + node->declare_parameter("robot_description", tmc_manipulation_tests::stanford_manipulator::GetUrdf()); + node->declare_parameter("robot_description_kinematics", tmc_manipulation_tests::stanford_manipulator::GetUrdf()); + node->declare_parameter("base_movement_type", 1); + node->declare_parameter("joint_names", kJointNames); + node->declare_parameter("joint_weights", std::vector({5.0, 1.0, 0.5, 0.5, 0.5, 1.0})); + node->declare_parameter("base_names", kBaseNames); + node->declare_parameter("base_weights", std::vector({5.0, 5.0, 1.0})); + generator->Initialize(node); + + tmc_robot_kinematics_model::IRobotKinematicsModel::Ptr robot( + new tmc_robot_kinematics_model::PinocchioWrapper(tmc_manipulation_tests::stanford_manipulator::GetUrdf())); + tmc_manipulation_types::RobotState initial_state; + initial_state.joint_state = robot->GetNamedAngle(kJointNames); + initial_state.joint_state.velocity = Eigen::VectorXd::Zero(kJointNames.size()); + initial_state.multi_dof_joint_state.names = kBaseNames; + initial_state.multi_dof_joint_state.poses = {Eigen::Affine3d::Identity()}; + initial_state.multi_dof_joint_state.twist = {tmc_manipulation_types::Twist::Zero()}; + + tmc_robot_local_planner::Constraints constraints; + constraints.hard_joint_constraints = { + std::make_shared(initial_state, initial_state, 0)}; + + std::vector trajectories_out; + std::function func = []() -> bool{ return false; }; + // If the goal is the same as the present, it will fail, so move the current bogie position appropriately. + initial_state.multi_dof_joint_state.poses = {Eigen::Translation3d(1.0, 0.0, 0.0) * Eigen::AngleAxisd::Identity()}; + EXPECT_TRUE(generator->Generate(constraints, initial_state, 0.2, {}, func, trajectories_out)); +} + + +class SimplePathGeneratorTest : public ::testing::Test { + protected: + void SetUp() override; + void TearDown() override { + server_node_->set_parameter(rclcpp::Parameter("generator_timeout", 0.05)); + server_node_->set_parameter(rclcpp::Parameter("max_trajectory_num", 0)); + } + + void SpinSome() { + rclcpp::spin_some(client_node_); + rclcpp::spin_some(server_node_); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + template + void WaitForFutureComplete(const typename std::shared_future& future) { + while (future.wait_for(std::chrono::microseconds(1)) == std::future_status::timeout) { + SpinSome(); + } + } + + tmc_planning_msgs::action::GenerateRobotTrajectories::Result::SharedPtr SendGoalSync( + int8_t expected, std::vector ignore_joints = {}) { + tmc_planning_msgs::action::GenerateRobotTrajectories::Goal goal; + goal.constraints = constraints_; + goal.initial_state = initial_state_; + goal.normalized_velocity = normalized_velocity_; + goal.ignore_joints = ignore_joints; + + auto future_goal_handle = generate_client_->async_send_goal(goal); + WaitForFutureComplete(future_goal_handle); + + auto goal_handle = future_goal_handle.get(); + EXPECT_TRUE(goal_handle.get()); + + auto future_result = generate_client_->async_get_result(goal_handle); + WaitForFutureComplete(future_result); + + auto wrapped_result = future_result.get(); + EXPECT_EQ(static_cast(wrapped_result.code), expected); + EXPECT_TRUE(wrapped_result.result); + return wrapped_result.result; + } + + void SendRejectedGoal() { + tmc_planning_msgs::action::GenerateRobotTrajectories::Goal goal; + goal.constraints = constraints_; + goal.initial_state = initial_state_; + goal.normalized_velocity = normalized_velocity_; + + auto future_goal_handle = generate_client_->async_send_goal(goal); + WaitForFutureComplete(future_goal_handle); + + auto goal_handle = future_goal_handle.get(); + EXPECT_FALSE(goal_handle.get()); + } + + rclcpp::Node::SharedPtr client_node_; + std::shared_ptr server_node_; + + rclcpp_action::Client::SharedPtr generate_client_; + + // Restraint condition + tmc_planning_msgs::msg::Constraints constraints_; + // Initial state of robot + moveit_msgs::msg::RobotState initial_state_; + // Normalized speed + double normalized_velocity_; +}; + +void SimplePathGeneratorTest::SetUp() { + const auto robot_description = tmc_manipulation_tests::stanford_manipulator::GetUrdf(); + + rclcpp::NodeOptions options; + options.parameter_overrides() = { + rclcpp::Parameter("robot_description", robot_description), + rclcpp::Parameter("robot_description_kinematics", robot_description), + rclcpp::Parameter("base_movement_type", 1), + rclcpp::Parameter("interpolation_type", "tmc_simple_path_generator/PlanarTrajectoryInterpolatorPlain"), + rclcpp::Parameter("origin_frame", "odom"), + rclcpp::Parameter("timeout", 0.05), + rclcpp::Parameter("joint_names", kJointNames), + rclcpp::Parameter("joint_weights", std::vector({5.0, 1.0, 0.5, 0.5, 0.5, 1.0})), + rclcpp::Parameter("base_names", kBaseNames), + rclcpp::Parameter("base_weights", std::vector({5.0, 5.0, 1.0}))}; + server_node_ = std::make_shared(options); + server_node_->Initialize(); + + client_node_ = rclcpp::Node::make_shared("client"); + + auto tf_static_broadcaster = std::make_shared(client_node_); + geometry_msgs::msg::TransformStamped static_tf; + static_tf.header.frame_id = "map"; + static_tf.child_frame_id = "odom"; + static_tf.transform.translation.x = 1.0; + static_tf.transform.translation.y = 2.0; + static_tf.transform.rotation.w = 1.0; + tf_static_broadcaster->sendTransform(static_tf); + + generate_client_ = rclcpp_action::create_client( + client_node_, "generator/generate"); + while (!generate_client_->action_server_is_ready()) { + SpinSome(); + } + + tmc_robot_kinematics_model::IRobotKinematicsModel::Ptr robot( + new tmc_robot_kinematics_model::PinocchioWrapper(robot_description)); + robot->SetRobotTransform(Eigen::Affine3d::Identity()); + const auto initial_position_impl = robot->GetNamedAngle(kJointNames); + + sensor_msgs::msg::JointState initial_position; + tmc_manipulation_types_bridge::JointStateToJointStateMsg(initial_position_impl, initial_position); + initial_position.velocity = std::vector(initial_position.name.size(), 0.0); + + tmc_planning_msgs::msg::RangeJointConstraint rjc; + rjc.header.frame_id = "map"; + rjc.min.joint_state.name = kJointNames; + for (uint32_t i = 0; i < kJointNames.size(); ++i) { + rjc.min.joint_state.position.push_back(i * 0.1 + 0.2); + } + rjc.min.multi_dof_joint_state.joint_names = kBaseNames; + geometry_msgs::msg::Transform base_to_target_pos; + base_to_target_pos.translation.x = 1.0; + base_to_target_pos.translation.y = 1.0; + for (uint32_t i = 0; i < kBaseNames.size(); ++i) { + rjc.min.multi_dof_joint_state.transforms.push_back(base_to_target_pos); + } + rjc.max = rjc.min; + constraints_.hard_joint_constraints.push_back(rjc); + + initial_state_.joint_state = initial_position; + initial_state_.multi_dof_joint_state.joint_names = kBaseNames; + geometry_msgs::msg::Transform initial_pos; + initial_pos.translation.x = 1.0; + initial_pos.translation.y = 1.0; + initial_pos.rotation.w = 1.0; + geometry_msgs::msg::Twist initial_twist; + for (uint32_t i = 0; i < kBaseNames.size(); ++i) { + initial_state_.multi_dof_joint_state.transforms.push_back(initial_pos); + initial_state_.multi_dof_joint_state.twist.push_back(initial_twist); + } + + normalized_velocity_ = 0.5; +} + +TEST_F(SimplePathGeneratorTest, GenerateSucceeded) { + auto result = SendGoalSync(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED); + EXPECT_FALSE(result->robot_trajectories.empty()); + + // Check if random intermediate points are used + std::vector middle_pos_x; + for (const auto& trajectory : result->robot_trajectories) { + if (trajectory.joint_trajectory.points.size() == 3) { + middle_pos_x.push_back(trajectory.multi_dof_joint_trajectory.points[1].transforms[0].translation.x); + } + } + EXPECT_GT(middle_pos_x.size(), 2); + + std::sort(middle_pos_x.begin(), middle_pos_x.end()); + EXPECT_GT(middle_pos_x.back() - middle_pos_x.front(), 1.0e-3); +} + +TEST_F(SimplePathGeneratorTest, GenerateWithIgnoreJoint) { + tmc_planning_msgs::msg::TaskSpaceRegion tsr; + tsr.origin_to_tsr.position.z = 0.5; + tsr.origin_to_tsr.orientation.w = 1.0; + tsr.min_bounds = {0.0, 0.0, 0.0, -3.141592, -3.141592, -3.141592}; + tsr.max_bounds = {0.0, 0.0, 0.0, 3.141592, 3.141592, 3.141592}; + tsr.tsr_to_end.orientation.w = 1.0; + tsr.end_frame_id = "link7"; + tmc_planning_msgs::msg::TsrLinkConstraint tlc; + tlc.header.frame_id = "odom"; + tlc.tsr = tsr; + + constraints_.hard_link_constraints.push_back(tlc); + constraints_.hard_joint_constraints.clear(); + + auto result = SendGoalSync(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED); + auto robot_trajectories = result->robot_trajectories; + ASSERT_FALSE(robot_trajectories.empty()); + + auto initial_angle = initial_state_.joint_state.position[0]; + auto goal_angle = robot_trajectories[0].joint_trajectory.points.back().positions[0]; + EXPECT_GT(std::abs(initial_angle - goal_angle), 1e-3); + + + result = SendGoalSync(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, {"joint1"}); + robot_trajectories = result->robot_trajectories; + ASSERT_FALSE(robot_trajectories.empty()); + + goal_angle = robot_trajectories[0].joint_trajectory.points.back().positions[0]; + EXPECT_LT(std::abs(initial_angle - goal_angle), 1e-3); +} + +TEST_F(SimplePathGeneratorTest, InvalidInitialState) { + initial_state_.joint_state.velocity.clear(); + + SendRejectedGoal(); +} + +TEST_F(SimplePathGeneratorTest, FaildToTransform) { + constraints_.hard_joint_constraints[0].header.frame_id = "test"; + + (void)SendGoalSync(action_msgs::msg::GoalStatus::STATUS_ABORTED, {"joint1"}); +} + +TEST_F(SimplePathGeneratorTest, EmptyConstraint) { + constraints_.hard_joint_constraints.clear(); + + SendRejectedGoal(); +} + +TEST_F(SimplePathGeneratorTest, GeneratePreempted) { + server_node_->set_parameter(rclcpp::Parameter("generator_timeout", 1.0)); + + tmc_planning_msgs::action::GenerateRobotTrajectories::Goal goal; + goal.constraints = constraints_; + goal.initial_state = initial_state_; + goal.normalized_velocity = normalized_velocity_; + + auto future_goal_handle = generate_client_->async_send_goal(goal); + WaitForFutureComplete(future_goal_handle); + + auto goal_handle = future_goal_handle.get(); + EXPECT_TRUE(goal_handle.get()); + + auto future_cancel_response = generate_client_->async_cancel_goal(goal_handle); + WaitForFutureComplete(future_cancel_response); + + auto future_result = generate_client_->async_get_result(goal_handle); + WaitForFutureComplete(future_result); + + auto wrapped_result = future_result.get(); + EXPECT_EQ(static_cast(wrapped_result.code), action_msgs::msg::GoalStatus::STATUS_CANCELED); +} + +TEST_F(SimplePathGeneratorTest, MaxTrajectoryNum) { + auto result = SendGoalSync(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED); + EXPECT_GT(result->robot_trajectories.size(), 3); + + server_node_->set_parameter(rclcpp::Parameter("max_trajectory_num", 3)); + result = SendGoalSync(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED); + EXPECT_EQ(result->robot_trajectories.size(), 3); +} +} // namespace tmc_simple_path_generator + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + return RUN_ALL_TESTS(); +}