diff --git a/mock/cpp_mock_scenarios/CMakeLists.txt b/mock/cpp_mock_scenarios/CMakeLists.txt index 669465e6482..d43bfd534a7 100644 --- a/mock/cpp_mock_scenarios/CMakeLists.txt +++ b/mock/cpp_mock_scenarios/CMakeLists.txt @@ -42,6 +42,7 @@ option(BUILD_CPP_MOCK_SCENARIOS "Building the C++ scenarios" OFF) if(BUILD_CPP_MOCK_SCENARIOS) add_subdirectory(src/behavior_plugin) add_subdirectory(src/collision) + add_subdirectory(src/context_gamma_planner) add_subdirectory(src/crosswalk) add_subdirectory(src/follow_front_entity) add_subdirectory(src/follow_lane) diff --git a/mock/cpp_mock_scenarios/package.xml b/mock/cpp_mock_scenarios/package.xml index b87013c4ff7..ce9f7ac4209 100644 --- a/mock/cpp_mock_scenarios/package.xml +++ b/mock/cpp_mock_scenarios/package.xml @@ -20,6 +20,7 @@ openscenario_visualization scenario_test_runner behavior_tree_plugin + context_gamma_planner ament_lint_auto ament_cmake_clang_format diff --git a/mock/cpp_mock_scenarios/src/context_gamma_planner/CMakeLists.txt b/mock/cpp_mock_scenarios/src/context_gamma_planner/CMakeLists.txt new file mode 100644 index 00000000000..959cae23e22 --- /dev/null +++ b/mock/cpp_mock_scenarios/src/context_gamma_planner/CMakeLists.txt @@ -0,0 +1,252 @@ +ament_auto_add_executable(spawn + spawn.cpp +) +target_link_libraries(spawn + ${PROTOBUF_LIBRARY} + pthread + sodium + zmq +) + +ament_auto_add_executable(stop_at_crosswalk + stop_at_crosswalk.cpp +) +target_link_libraries(stop_at_crosswalk + ${PROTOBUF_LIBRARY} + pthread + sodium + zmq +) + +ament_auto_add_executable(parked_at_crosswalk + parked_at_crosswalk.cpp +) +target_link_libraries(parked_at_crosswalk + ${PROTOBUF_LIBRARY} + pthread + sodium + zmq +) + +ament_auto_add_executable(reverse_walk_at_road + reverse_walk_at_road.cpp +) +target_link_libraries(reverse_walk_at_road + ${PROTOBUF_LIBRARY} + pthread + sodium + zmq +) + +ament_auto_add_executable(pedestrian_follow_lane + pedestrian_follow_lane.cpp +) +target_link_libraries(pedestrian_follow_lane + ${PROTOBUF_LIBRARY} + pthread + sodium + zmq +) + +ament_auto_add_executable(stop_line_running + stop_line_running.cpp +) +target_link_libraries(stop_line_running + ${PROTOBUF_LIBRARY} + pthread + sodium + zmq +) + +ament_auto_add_executable(traffic_light_running + traffic_light_running.cpp +) +target_link_libraries(traffic_light_running + ${PROTOBUF_LIBRARY} + pthread + sodium + zmq +) + +ament_auto_add_executable(head_on_pedestrian +head_on_pedestrian.cpp +) +target_link_libraries(head_on_pedestrian + ${PROTOBUF_LIBRARY} + pthread + sodium + zmq +) + +ament_auto_add_executable(adaptive_cruise +adaptive_cruise.cpp +) +target_link_libraries(adaptive_cruise + ${PROTOBUF_LIBRARY} + pthread + sodium + zmq +) + +ament_auto_add_executable(follow_trajectory +follow_trajectory.cpp +) +target_link_libraries(follow_trajectory + ${PROTOBUF_LIBRARY} + pthread + sodium + zmq +) + +ament_auto_add_executable(follow_trajectory_closed +follow_trajectory_closed.cpp +) +target_link_libraries(follow_trajectory_closed + ${PROTOBUF_LIBRARY} + pthread + sodium + zmq +) + +ament_auto_add_executable(lane_change_time +lane_change_time.cpp +) +target_link_libraries(lane_change_time + ${PROTOBUF_LIBRARY} + pthread + sodium + zmq +) + +ament_auto_add_executable(lane_change_lateral +lane_change_lateral.cpp +) +target_link_libraries(lane_change_lateral + ${PROTOBUF_LIBRARY} + pthread + sodium + zmq +) + +ament_auto_add_executable(lane_change_longitudinal +lane_change_longitudinal.cpp +) +target_link_libraries(lane_change_longitudinal + ${PROTOBUF_LIBRARY} + pthread + sodium + zmq +) + +ament_auto_add_executable(follow_trajectory_dynamic_constraints +follow_trajectory_dynamic_constraints.cpp +) +target_link_libraries(follow_trajectory_dynamic_constraints + ${PROTOBUF_LIBRARY} + pthread + sodium + zmq +) + +ament_auto_add_executable(road_end +road_end.cpp +) +target_link_libraries(road_end + ${PROTOBUF_LIBRARY} + pthread + sodium + zmq +) + +if(${tf2_geometry_msgs_VERSION} VERSION_LESS 0.18.0) + target_compile_definitions(spawn PUBLIC + USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER + ) + target_compile_definitions(stop_at_crosswalk PUBLIC + USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER + ) + target_compile_definitions(parked_at_crosswalk PUBLIC + USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER + ) + target_compile_definitions(reverse_walk_at_road PUBLIC + USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER + ) + target_compile_definitions(pedestrian_follow_lane PUBLIC + USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER + ) + target_compile_definitions(stop_line_running PUBLIC + USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER + ) + target_compile_definitions(traffic_light_running PUBLIC + USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER + ) + target_compile_definitions(head_on_pedestrian PUBLIC + USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER + ) + target_compile_definitions(adaptive_cruise PUBLIC + USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER + ) + target_compile_definitions(follow_trajectory PUBLIC + USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER + ) + target_compile_definitions(follow_trajectory_closed PUBLIC + USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER + ) + target_compile_definitions(lane_change_time PUBLIC + USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER + ) + target_compile_definitions(lane_change_lateral PUBLIC + USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER + ) + target_compile_definitions(lane_change_longitudinal PUBLIC + USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER + ) + target_compile_definitions(follow_trajectory_dynamic_constraints PUBLIC + USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER + ) + target_compile_definitions(road_end PUBLIC + USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER + ) +endif() + +install(TARGETS + stop_at_crosswalk + parked_at_crosswalk + reverse_walk_at_road + spawn + pedestrian_follow_lane + stop_line_running + traffic_light_running + head_on_pedestrian + adaptive_cruise + follow_trajectory + follow_trajectory_closed + lane_change_time + lane_change_lateral + lane_change_longitudinal + follow_trajectory_dynamic_constraints + road_end + DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + include(./cmake/add_cpp_mock_scenario_test.cmake) + add_cpp_mock_scenario_test(${PROJECT_NAME} "spawn" "20.0") + add_cpp_mock_scenario_test(${PROJECT_NAME} "parked_at_crosswalk" "60.0") + add_cpp_mock_scenario_test(${PROJECT_NAME} "pedestrian_follow_lane" "60.0") + add_cpp_mock_scenario_test(${PROJECT_NAME} "reverse_walk_at_road" "120.0") + add_cpp_mock_scenario_test(${PROJECT_NAME} "stop_at_crosswalk" "40.0") + add_cpp_mock_scenario_test(${PROJECT_NAME} "stop_line_running" "200.0") + add_cpp_mock_scenario_test(${PROJECT_NAME} "traffic_light_running" "30.0") + add_cpp_mock_scenario_test(${PROJECT_NAME} "head_on_pedestrian" "60.0") + add_cpp_mock_scenario_test(${PROJECT_NAME} "adaptive_cruise" "120.0") + add_cpp_mock_scenario_test(${PROJECT_NAME} "follow_trajectory" "40.0") + add_cpp_mock_scenario_test(${PROJECT_NAME} "follow_trajectory_closed" "80.0") + add_cpp_mock_scenario_test(${PROJECT_NAME} "lane_change_time" "20.0") + add_cpp_mock_scenario_test(${PROJECT_NAME} "lane_change_lateral" "20.0") + add_cpp_mock_scenario_test(${PROJECT_NAME} "lane_change_longitudinal" "20.0") + add_cpp_mock_scenario_test(${PROJECT_NAME} "follow_trajectory_dynamic_constraints" "40.0") + add_cpp_mock_scenario_test(${PROJECT_NAME} "road_end" "30.0") +endif() diff --git a/mock/cpp_mock_scenarios/src/context_gamma_planner/adaptive_cruise.cpp b/mock/cpp_mock_scenarios/src/context_gamma_planner/adaptive_cruise.cpp new file mode 100644 index 00000000000..bebbbd16979 --- /dev/null +++ b/mock/cpp_mock_scenarios/src/context_gamma_planner/adaptive_cruise.cpp @@ -0,0 +1,131 @@ +// Copyright 2022 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include + +// headers in STL +#include +#include +#include + +class AdaptiveCruiseScenario : public cpp_mock_scenarios::CppScenarioNode +{ +public: + explicit AdaptiveCruiseScenario(const rclcpp::NodeOptions & option) + : cpp_mock_scenarios::CppScenarioNode( + "adaptive_cruise", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", + "lanelet2_map.osm", __FILE__, false, option) + { + start(); + } + +private: + bool is_stoped_ego_ = false; + bool is_stoped_bob_ = false; + void onUpdate() override + { + const auto t = api_.getCurrentTime(); + if (t >= 6) { + api_.requestSpeedChange("bob", 5, true); + } + if ( + static_cast(api_.getEntityStatus("ego").getLaneletPose()) + .lanelet_id == 34675 and + api_.getCurrentTwist("ego").linear.x < 0.4) { + is_stoped_ego_ = true; + } + if ( + static_cast(api_.getEntityStatus("bob").getLaneletPose()) + .lanelet_id == 34675 and + api_.getCurrentTwist("bob").linear.x < 0.4) { + is_stoped_bob_ = true; + } + if (is_stoped_ego_ and is_stoped_bob_) { + stop(cpp_mock_scenarios::Result::SUCCESS); + } + // LCOV_EXCL_STOP + if (t >= 120) { + stop(cpp_mock_scenarios::Result::FAILURE); + } + } + + void onInitialize() override + { + //Vehicle setting + api_.spawn( + "ego", + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34579, 3, 0, api_.getHdmapUtils()), + getVehicleParameters(), traffic_simulator::VehicleBehavior::contextGamma()); + api_.setEntityStatus( + "ego", + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34579, 3, 0, api_.getHdmapUtils()), + traffic_simulator::helper::constructActionStatus(10)); + api_.requestSpeedChange("ego", 5, true); + api_.requestAssignRoute( + "ego", std::vector{ + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34675, 0.0, 0, api_.getHdmapUtils()), + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34690, 0.0, 0, api_.getHdmapUtils()), + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34576, 0.0, 0, api_.getHdmapUtils()), + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34579, 0.0, 0, api_.getHdmapUtils()), + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34675, 0.0, 0, api_.getHdmapUtils())}); + + //Vehicle setting + api_.spawn( + "bob", + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34585, 3, 0, api_.getHdmapUtils()), + getVehicleParameters(), traffic_simulator::VehicleBehavior::contextGamma()); + api_.setEntityStatus( + "bob", + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34585, 3, 0, api_.getHdmapUtils()), + traffic_simulator::helper::constructActionStatus(10)); + api_.requestSpeedChange("bob", 0, true); + api_.requestAssignRoute( + "bob", std::vector{ + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34675, 0.0, 0, api_.getHdmapUtils()), + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34690, 0.0, 0, api_.getHdmapUtils()), + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34576, 0.0, 0, api_.getHdmapUtils()), + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34579, 0.0, 0, api_.getHdmapUtils()), + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34675, 0.0, 0, api_.getHdmapUtils())}); + } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + rclcpp::shutdown(); + return 0; +} diff --git a/mock/cpp_mock_scenarios/src/context_gamma_planner/follow_trajectory.cpp b/mock/cpp_mock_scenarios/src/context_gamma_planner/follow_trajectory.cpp new file mode 100644 index 00000000000..2dcfb4678f2 --- /dev/null +++ b/mock/cpp_mock_scenarios/src/context_gamma_planner/follow_trajectory.cpp @@ -0,0 +1,132 @@ +// Copyright 2022 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include + +// headers in STL +#include +#include +#include + +class FollowTrajectoryScenario : public cpp_mock_scenarios::CppScenarioNode +{ +public: + explicit FollowTrajectoryScenario(const rclcpp::NodeOptions & option) + : cpp_mock_scenarios::CppScenarioNode( + "follow_trajectory", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", + "lanelet2_map.osm", __FILE__, false, option) + { + start(); + } + +private: + traffic_simulator_msgs::msg::PolylineTrajectory follow_trajectory; + int vertex_num = 0; + Eigen::Vector3d toEigenVector3d(const geometry_msgs::msg::Point & pos) + { + return Eigen::Vector3d(pos.x, pos.y, pos.z); + } + void onUpdate() override + { + const auto t = api_.getCurrentTime(); + const double distance_threshold = 4.0; + const auto ego_pos = toEigenVector3d(api_.getEntityStatus("ego").getMapPose().position); + const auto reference_pos = + toEigenVector3d(follow_trajectory.shape.vertices.at(vertex_num).position.position); + + if ((ego_pos - reference_pos).norm() < distance_threshold) { + vertex_num++; + RCLCPP_INFO_STREAM(get_logger(), "passing vertex number: " << vertex_num); + } + + if (vertex_num >= static_cast(follow_trajectory.shape.vertices.size())) { + stop(cpp_mock_scenarios::Result::SUCCESS); + } + // LCOV_EXCL_STOP + if (t >= 40) { + stop(cpp_mock_scenarios::Result::FAILURE); + } + } + + void onInitialize() override + { + //Vehicle setting + api_.spawn( + "ego", + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34579, 0, 0, api_.getHdmapUtils()), + getVehicleParameters(), traffic_simulator::VehicleBehavior::contextGamma()); + api_.setEntityStatus( + "ego", + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34579, 0, 0, api_.getHdmapUtils()), + traffic_simulator::helper::constructActionStatus(10)); + api_.requestSpeedChange("ego", 7, true); + + auto toVertex = [](double time, const geometry_msgs::msg::Pose & pose) { + traffic_simulator_msgs::msg::Vertex vertex; + vertex.position = pose; + vertex.time = time; + return vertex; + }; + follow_trajectory.shape.vertices.push_back(toVertex( + 0.0, traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34579, 0.0, 0, 0, 0, 0, api_.getHdmapUtils())))); + follow_trajectory.shape.vertices.push_back(toVertex( + 10.0, traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34579, 10.0, 0, 0, 0, 0, api_.getHdmapUtils())))); + follow_trajectory.shape.vertices.push_back(toVertex( + 12.0, traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34600, 30.0, 0, 0, 0, 0, api_.getHdmapUtils())))); + follow_trajectory.shape.vertices.push_back(toVertex( + 13.0, traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34600, 20.0, 0, 0, 0, 0, api_.getHdmapUtils())))); + follow_trajectory.shape.vertices.push_back(toVertex( + 14.0, traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34579, 40.0, 0, 0, 0, 0, api_.getHdmapUtils())))); + follow_trajectory.shape.vertices.push_back(toVertex( + 25.0, traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34579, 50.0, 0, 0, 0, 0, api_.getHdmapUtils())))); + + follow_trajectory.initial_distance_offset = 0.0; + follow_trajectory.dynamic_constraints_ignorable = true; + follow_trajectory.base_time = 5.0; + follow_trajectory.closed = false; + std::shared_ptr follow_trajectory_ptr = + std::make_shared(follow_trajectory); + api_.requestFollowTrajectory("ego", follow_trajectory_ptr); + } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + rclcpp::shutdown(); + return 0; +} diff --git a/mock/cpp_mock_scenarios/src/context_gamma_planner/follow_trajectory_closed.cpp b/mock/cpp_mock_scenarios/src/context_gamma_planner/follow_trajectory_closed.cpp new file mode 100644 index 00000000000..94e9ebfc6e7 --- /dev/null +++ b/mock/cpp_mock_scenarios/src/context_gamma_planner/follow_trajectory_closed.cpp @@ -0,0 +1,167 @@ +// Copyright 2022 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include + +// headers in STL +#include +#include +#include + +class FollowTrajectoryClosedScenario : public cpp_mock_scenarios::CppScenarioNode +{ +public: + explicit FollowTrajectoryClosedScenario(const rclcpp::NodeOptions & option) + : cpp_mock_scenarios::CppScenarioNode( + "follow_trajectory_closed", + ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", "lanelet2_map.osm", + __FILE__, false, option) + { + start(); + } + +private: + traffic_simulator_msgs::msg::PolylineTrajectory follow_trajectory; + int vertex_num = 0; + int loop_count = 0; + Eigen::Vector3d toEigenVector3d(const geometry_msgs::msg::Point & pos) + { + return Eigen::Vector3d(pos.x, pos.y, pos.z); + } + void onUpdate() override + { + const auto t = api_.getCurrentTime(); + const double distance_threshold = 5.0; + const auto ego_pos = toEigenVector3d(api_.getEntityStatus("ego").getMapPose().position); + const auto reference_pos = + toEigenVector3d(follow_trajectory.shape.vertices.at(vertex_num).position.position); + if ((ego_pos - reference_pos).norm() < distance_threshold) { + if (vertex_num == static_cast(follow_trajectory.shape.vertices.size()) - 1) { + loop_count++; + vertex_num = 0; + RCLCPP_INFO_STREAM(get_logger(), "loop count: " << loop_count); + if (loop_count == 2) { + stop(cpp_mock_scenarios::Result::SUCCESS); + } + } else { + vertex_num++; + } + RCLCPP_INFO_STREAM(get_logger(), "passing vertex number: " << vertex_num); + } + // LCOV_EXCL_STOP + if (t >= 100) { + stop(cpp_mock_scenarios::Result::FAILURE); + } + } + + void onInitialize() override + { + //Vehicle setting + api_.spawn( + "ego", + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34579, 0, 0, api_.getHdmapUtils()), + getVehicleParameters(), traffic_simulator::VehicleBehavior::contextGamma()); + api_.setEntityStatus( + "ego", + (traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34579, 0, 0, api_.getHdmapUtils())), + traffic_simulator::helper::constructActionStatus(10)); + api_.requestSpeedChange("ego", 7, true); + + auto toVertex = [](double time, const geometry_msgs::msg::Pose & pose) { + traffic_simulator_msgs::msg::Vertex vertex; + vertex.position = pose; + vertex.time = time; + return vertex; + }; + follow_trajectory.shape.vertices.push_back(toVertex( + 0.0, traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34579, 0.0, 0, 0, 0, 0, api_.getHdmapUtils())))); + follow_trajectory.shape.vertices.push_back(toVertex( + 2.0, traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34579, 10.0, 0, 0, 0, 0, api_.getHdmapUtils())))); + follow_trajectory.shape.vertices.push_back(toVertex( + 4.0, traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34600, 30.0, 0, 0, 0, 0, api_.getHdmapUtils())))); + follow_trajectory.shape.vertices.push_back(toVertex( + 6.0, traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34600, 20.0, 0, 0, 0, 0, api_.getHdmapUtils())))); + follow_trajectory.shape.vertices.push_back(toVertex( + 8.0, traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34579, 40.0, 0, 0, 0, 0, api_.getHdmapUtils())))); + follow_trajectory.shape.vertices.push_back(toVertex( + 10.0, traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34579, 50.0, 0, 0, 0, 0, api_.getHdmapUtils())))); + + follow_trajectory.shape.vertices.push_back(toVertex( + 12.0, traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34600, 0.0, 0, 0, 0, 0, api_.getHdmapUtils())))); + follow_trajectory.shape.vertices.push_back(toVertex( + 14.0, traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34600, 10.0, 0, 0, 0, 0, api_.getHdmapUtils())))); + follow_trajectory.shape.vertices.push_back(toVertex( + 16.0, traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34579, 30.0, 0, 0, 0, 0, api_.getHdmapUtils())))); + follow_trajectory.shape.vertices.push_back(toVertex( + 18.0, traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34579, 20.0, 0, 0, 0, 0, api_.getHdmapUtils())))); + follow_trajectory.shape.vertices.push_back(toVertex( + 20.0, traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34600, 40.0, 0, 0, 0, 0, api_.getHdmapUtils())))); + follow_trajectory.shape.vertices.push_back(toVertex( + 22.0, traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34600, 50.0, 0, 0, 0, 0, api_.getHdmapUtils())))); + follow_trajectory.shape.vertices.push_back(toVertex( + 24.0, traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34579, 0.0, 0, 0, 0, 0, api_.getHdmapUtils())))); + + follow_trajectory.initial_distance_offset = 0.0; + follow_trajectory.dynamic_constraints_ignorable = true; + follow_trajectory.base_time = 5.0; + follow_trajectory.closed = true; + std::shared_ptr follow_trajectory_ptr = + std::make_shared(follow_trajectory); + api_.requestFollowTrajectory("ego", follow_trajectory_ptr); + } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + rclcpp::shutdown(); + return 0; +} diff --git a/mock/cpp_mock_scenarios/src/context_gamma_planner/follow_trajectory_dynamic_constraints.cpp b/mock/cpp_mock_scenarios/src/context_gamma_planner/follow_trajectory_dynamic_constraints.cpp new file mode 100644 index 00000000000..38e1094319a --- /dev/null +++ b/mock/cpp_mock_scenarios/src/context_gamma_planner/follow_trajectory_dynamic_constraints.cpp @@ -0,0 +1,139 @@ +// Copyright 2022 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include + +// headers in STL +#include +#include +#include + +class FollowTrajectoryDynamicConstraintsScenario : public cpp_mock_scenarios::CppScenarioNode +{ +public: + explicit FollowTrajectoryDynamicConstraintsScenario(const rclcpp::NodeOptions & option) + : cpp_mock_scenarios::CppScenarioNode( + "follow_trajectory_dynamic_constraints", + ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", "lanelet2_map.osm", + __FILE__, false, option) + { + start(); + } + +private: + traffic_simulator_msgs::msg::PolylineTrajectory follow_trajectory; + int vertex_num = 0; + Eigen::Vector3d toEigenVector3d(const geometry_msgs::msg::Point & pos) + { + return Eigen::Vector3d(pos.x, pos.y, pos.z); + } + void onUpdate() override + { + const double t_threshold = 3.0; + const auto t = api_.getCurrentTime(); + const auto reference_time = + follow_trajectory.base_time + follow_trajectory.shape.vertices.at(vertex_num).time; + + const double distance_threshold = 4.0; + const auto ego_pos = toEigenVector3d(api_.getEntityStatus("ego").getMapPose().position); + const auto reference_pos = + toEigenVector3d(follow_trajectory.shape.vertices.at(vertex_num).position.position); + + if ( + reference_time - t_threshold < t and t < reference_time + t_threshold and + (ego_pos - reference_pos).norm() < distance_threshold) { + vertex_num++; + RCLCPP_INFO_STREAM(get_logger(), "passing vertex number: " << vertex_num); + } + + if (vertex_num >= static_cast(follow_trajectory.shape.vertices.size()) - 1) { + stop(cpp_mock_scenarios::Result::SUCCESS); + } + // LCOV_EXCL_STOP + if (t >= 40) { + stop(cpp_mock_scenarios::Result::FAILURE); + } + } + + void onInitialize() override + { + //Vehicle setting + api_.spawn( + "ego", + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34579, 0, 0, api_.getHdmapUtils()), + getVehicleParameters(), traffic_simulator::VehicleBehavior::contextGamma()); + api_.setEntityStatus( + "ego", + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34579, 0, 0, api_.getHdmapUtils()), + traffic_simulator::helper::constructActionStatus(10)); + api_.requestSpeedChange("ego", 7, true); + + auto toVertex = [](double time, const geometry_msgs::msg::Pose & pose) { + traffic_simulator_msgs::msg::Vertex vertex; + vertex.position = pose; + vertex.time = time; + return vertex; + }; + follow_trajectory.shape.vertices.push_back(toVertex( + 0.0, traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34579, 0.0, 0, 0, 0, 0, api_.getHdmapUtils())))); + follow_trajectory.shape.vertices.push_back(toVertex( + 10.0, traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34579, 10.0, 0, 0, 0, 0, api_.getHdmapUtils())))); + follow_trajectory.shape.vertices.push_back(toVertex( + 12.0, traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34600, 30.0, 0, 0, 0, 0, api_.getHdmapUtils())))); + follow_trajectory.shape.vertices.push_back(toVertex( + 13.0, traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34600, 20.0, 0, 0, 0, 0, api_.getHdmapUtils())))); + follow_trajectory.shape.vertices.push_back(toVertex( + 14.0, traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34579, 40.0, 0, 0, 0, 0, api_.getHdmapUtils())))); + follow_trajectory.shape.vertices.push_back(toVertex( + 25.0, traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34579, 50.0, 0, 0, 0, 0, api_.getHdmapUtils())))); + + follow_trajectory.initial_distance_offset = 0.0; + follow_trajectory.dynamic_constraints_ignorable = false; + follow_trajectory.base_time = 5.0; + follow_trajectory.closed = false; + std::shared_ptr follow_trajectory_ptr = + std::make_shared(follow_trajectory); + api_.requestFollowTrajectory("ego", follow_trajectory_ptr); + } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + rclcpp::shutdown(); + return 0; +} diff --git a/mock/cpp_mock_scenarios/src/context_gamma_planner/head_on_pedestrian.cpp b/mock/cpp_mock_scenarios/src/context_gamma_planner/head_on_pedestrian.cpp new file mode 100644 index 00000000000..0d4c8ccc6cc --- /dev/null +++ b/mock/cpp_mock_scenarios/src/context_gamma_planner/head_on_pedestrian.cpp @@ -0,0 +1,203 @@ +// Copyright 2022 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include + +// headers in STL +#include +#include +#include + +class HeadOnPedestrianScenario : public cpp_mock_scenarios::CppScenarioNode +{ +public: + explicit HeadOnPedestrianScenario(const rclcpp::NodeOptions & option) + : cpp_mock_scenarios::CppScenarioNode( + "head_on_pedstrian", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", + "lanelet2_map.osm", __FILE__, false, option) + { + start(); + } + +private: + Eigen::Vector3d toEigenVector3d(const geometry_msgs::msg::Point & pos) + { + return Eigen::Vector3d(pos.x, pos.y, pos.z); + } + std::vector init_pos; + void onUpdate() override + { + const auto t = api_.getCurrentTime(); + float goal_threshold = 1.5; + if ( + (init_pos[0] - toEigenVector3d(api_.getEntityStatus("ego").getMapPose().position)).norm() < + goal_threshold) { + api_.requestSpeedChange("ego", 0, true); + } + if ( + (init_pos[1] - toEigenVector3d(api_.getEntityStatus("ego2").getMapPose().position)).norm() < + goal_threshold) { + api_.requestSpeedChange("ego2", 0, true); + } + if ( + (init_pos[2] - toEigenVector3d(api_.getEntityStatus("bob").getMapPose().position)).norm() < + goal_threshold) { + api_.requestSpeedChange("bob", 0, true); + } + if ( + (init_pos[3] - toEigenVector3d(api_.getEntityStatus("bob2").getMapPose().position)).norm() < + goal_threshold) { + api_.requestSpeedChange("bob2", 0, true); + } + if ( + (init_pos[0] - toEigenVector3d(api_.getEntityStatus("ego").getMapPose().position)).norm() < + goal_threshold and + (init_pos[1] - toEigenVector3d(api_.getEntityStatus("ego2").getMapPose().position)).norm() < + goal_threshold and + (init_pos[2] - toEigenVector3d(api_.getEntityStatus("bob").getMapPose().position)).norm() < + goal_threshold and + (init_pos[3] - toEigenVector3d(api_.getEntityStatus("bob2").getMapPose().position)).norm() < + goal_threshold) { + stop(cpp_mock_scenarios::Result::SUCCESS); + } + // LCOV_EXCL_STOP + if (t >= 60) { + stop(cpp_mock_scenarios::Result::FAILURE); + } + } + + void onInitialize() override + { + auto toVertex = [](double time, const geometry_msgs::msg::Pose & pose) { + traffic_simulator_msgs::msg::Vertex vertex; + vertex.position = pose; + vertex.time = time; + return vertex; + }; + auto toTrajectory = + [&](const geometry_msgs::msg::Pose & start_pose, const geometry_msgs::msg::Pose & goal_pose) { + traffic_simulator_msgs::msg::PolylineTrajectory follow_trajectory; + follow_trajectory.shape.vertices.push_back(toVertex(0.0, start_pose)); + follow_trajectory.shape.vertices.push_back(toVertex(10.0, goal_pose)); + follow_trajectory.initial_distance_offset = 0.0; + follow_trajectory.dynamic_constraints_ignorable = true; + follow_trajectory.base_time = 0.0; + follow_trajectory.closed = false; + return follow_trajectory; + }; + //Pedestrian1 setting + api_.spawn( + "bob", + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34378, 0.0, -1, api_.getHdmapUtils()), + getPedestrianParameters(), traffic_simulator::PedestrianBehavior::contextGamma()); + api_.requestSpeedChange( + "bob", 0.5, traffic_simulator::speed_change::Transition::LINEAR, + traffic_simulator::speed_change::Constraint( + traffic_simulator::speed_change::Constraint::Type::LONGITUDINAL_ACCELERATION, 1.0), + true); + std::shared_ptr follow_trajectory_ptr1 = + std::make_shared(toTrajectory( + traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34378, 0.0, -1, 0, 0, 0, api_.getHdmapUtils())), + traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34378, 7.5, 1, 0, 0, 0, api_.getHdmapUtils())))); + api_.requestFollowTrajectory("bob", follow_trajectory_ptr1); + //Pedestrian2 setting + api_.spawn( + "bob2", + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34378, 0.0, 1, api_.getHdmapUtils()), + getPedestrianParameters(), traffic_simulator::PedestrianBehavior::contextGamma()); + api_.requestSpeedChange( + "bob2", 0.5, traffic_simulator::speed_change::Transition::LINEAR, + traffic_simulator::speed_change::Constraint( + traffic_simulator::speed_change::Constraint::Type::LONGITUDINAL_ACCELERATION, 1.0), + true); + std::shared_ptr follow_trajectory_ptr2 = + std::make_shared(toTrajectory( + traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34378, 0.0, 1, 0, 0, 0, api_.getHdmapUtils())), + traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34378, 7.5, -1, 0, 0, 0, api_.getHdmapUtils())))); + api_.requestFollowTrajectory("bob2", follow_trajectory_ptr2); + + //Pedestrian3 setting + api_.spawn( + "ego", + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34378, 7.5, 1, api_.getHdmapUtils()), + getPedestrianParameters(), traffic_simulator::PedestrianBehavior::contextGamma()); + api_.requestSpeedChange( + "ego", 0.5, traffic_simulator::speed_change::Transition::LINEAR, + traffic_simulator::speed_change::Constraint( + traffic_simulator::speed_change::Constraint::Type::LONGITUDINAL_ACCELERATION, 1.0), + true); + std::shared_ptr follow_trajectory_ptr3 = + std::make_shared(toTrajectory( + traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34378, 7.5, 1, 0, 0, 0, api_.getHdmapUtils())), + traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34378, 0.0, -1, 0, 0, 0, api_.getHdmapUtils())))); + api_.requestFollowTrajectory("ego", follow_trajectory_ptr3); + + //Pedestrian4 setting + api_.spawn( + "ego2", + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34378, 7.5, -1, api_.getHdmapUtils()), + getPedestrianParameters(), traffic_simulator::PedestrianBehavior::contextGamma()); + api_.requestSpeedChange( + "ego2", 0.5, traffic_simulator::speed_change::Transition::LINEAR, + traffic_simulator::speed_change::Constraint( + traffic_simulator::speed_change::Constraint::Type::LONGITUDINAL_ACCELERATION, 1.0), + true); + std::shared_ptr follow_trajectory_ptr4 = + std::make_shared(toTrajectory( + traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34378, 7.5, -1, 0, 0, 0, api_.getHdmapUtils())), + traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34378, 0.0, 1, 0, 0, 0, api_.getHdmapUtils())))); + api_.requestFollowTrajectory("ego2", follow_trajectory_ptr4); + + init_pos.emplace_back(toEigenVector3d(api_.getEntityStatus("bob").getMapPose().position)); + init_pos.emplace_back(toEigenVector3d(api_.getEntityStatus("bob2").getMapPose().position)); + init_pos.emplace_back(toEigenVector3d(api_.getEntityStatus("ego").getMapPose().position)); + init_pos.emplace_back(toEigenVector3d(api_.getEntityStatus("ego2").getMapPose().position)); + } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + rclcpp::shutdown(); + return 0; +} diff --git a/mock/cpp_mock_scenarios/src/context_gamma_planner/lane_change_lateral.cpp b/mock/cpp_mock_scenarios/src/context_gamma_planner/lane_change_lateral.cpp new file mode 100644 index 00000000000..e314bf5ba01 --- /dev/null +++ b/mock/cpp_mock_scenarios/src/context_gamma_planner/lane_change_lateral.cpp @@ -0,0 +1,88 @@ +// Copyright 2022 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include + +// headers in STL +#include +#include +#include + +class LaneChangeLateralScenario : public cpp_mock_scenarios::CppScenarioNode +{ +public: + explicit LaneChangeLateralScenario(const rclcpp::NodeOptions & option) + : cpp_mock_scenarios::CppScenarioNode( + "lane_change_lateral", + ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", "lanelet2_map.osm", + __FILE__, false, option) + { + start(); + } + +private: + void onUpdate() override + { + const auto t = api_.getCurrentTime(); + if (1 < t && t < 1.1) { + api_.requestLaneChange( + "ego", + traffic_simulator::lane_change::RelativeTarget( + "ego", traffic_simulator::lane_change::Direction::RIGHT, 1, 0.0), + traffic_simulator::lane_change::TrajectoryShape::CUBIC, + traffic_simulator::lane_change::Constraint( + traffic_simulator::lane_change::Constraint::Type::LATERAL_VELOCITY, 1, + traffic_simulator::lane_change::Constraint::Policy::BEST_EFFORT)); + } + if ( + // api_.getEntityStatus("ego").getLaneletPose() and + static_cast(api_.getEntityStatus("ego").getLaneletPose()) + .lanelet_id == 34462 and + static_cast(api_.getEntityStatus("ego").getLaneletPose()).s >= + 28.0) { + stop(cpp_mock_scenarios::Result::SUCCESS); + } + // LCOV_EXCL_STOP + if (t >= 30) { + stop(cpp_mock_scenarios::Result::FAILURE); + } + } + + void onInitialize() override + { + //Vehicle setting + api_.spawn( + "ego", + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34513, 0, 0, api_.getHdmapUtils()), + getVehicleParameters(), traffic_simulator::VehicleBehavior::contextGamma()); + api_.requestSpeedChange("ego", 7, true); + } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + rclcpp::shutdown(); + return 0; +} diff --git a/mock/cpp_mock_scenarios/src/context_gamma_planner/lane_change_longitudinal.cpp b/mock/cpp_mock_scenarios/src/context_gamma_planner/lane_change_longitudinal.cpp new file mode 100644 index 00000000000..690e90c6fec --- /dev/null +++ b/mock/cpp_mock_scenarios/src/context_gamma_planner/lane_change_longitudinal.cpp @@ -0,0 +1,88 @@ +// Copyright 2022 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include + +// headers in STL +#include +#include +#include + +class LaneChangeLongitudinalScenario : public cpp_mock_scenarios::CppScenarioNode +{ +public: + explicit LaneChangeLongitudinalScenario(const rclcpp::NodeOptions & option) + : cpp_mock_scenarios::CppScenarioNode( + "lane_change_longitudinal", + ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", "lanelet2_map.osm", + __FILE__, false, option) + { + start(); + } + +private: + void onUpdate() override + { + const auto t = api_.getCurrentTime(); + if (1 < t && t < 1.1) { + api_.requestLaneChange( + "ego", + traffic_simulator::lane_change::RelativeTarget( + "ego", traffic_simulator::lane_change::Direction::RIGHT, 1, 0.0), + traffic_simulator::lane_change::TrajectoryShape::CUBIC, + traffic_simulator::lane_change::Constraint( + traffic_simulator::lane_change::Constraint::Type::LONGITUDINAL_DISTANCE, 20, + traffic_simulator::lane_change::Constraint::Policy::BEST_EFFORT)); + } + if ( + // api_.getEntityStatus("ego").getLaneletPose() and + static_cast(api_.getEntityStatus("ego").getLaneletPose()) + .lanelet_id == 34462 and + static_cast(api_.getEntityStatus("ego").getLaneletPose()).s >= + 28.0) { + stop(cpp_mock_scenarios::Result::SUCCESS); + } + // LCOV_EXCL_STOP + if (t >= 30) { + stop(cpp_mock_scenarios::Result::FAILURE); + } + } + + void onInitialize() override + { + //Vehicle setting + api_.spawn( + "ego", + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34513, 0, 0, api_.getHdmapUtils()), + getVehicleParameters(), traffic_simulator::VehicleBehavior::contextGamma()); + api_.requestSpeedChange("ego", 7, true); + } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + rclcpp::shutdown(); + return 0; +} diff --git a/mock/cpp_mock_scenarios/src/context_gamma_planner/lane_change_time.cpp b/mock/cpp_mock_scenarios/src/context_gamma_planner/lane_change_time.cpp new file mode 100644 index 00000000000..651ef6212b8 --- /dev/null +++ b/mock/cpp_mock_scenarios/src/context_gamma_planner/lane_change_time.cpp @@ -0,0 +1,92 @@ +// Copyright 2022 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include + +// headers in STL +#include +#include +#include + +class LaneChangeTimeScenario : public cpp_mock_scenarios::CppScenarioNode +{ +public: + explicit LaneChangeTimeScenario(const rclcpp::NodeOptions & option) + : cpp_mock_scenarios::CppScenarioNode( + "lane_change_time", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", + "lanelet2_map.osm", __FILE__, false, option) + { + start(); + } + +private: + void onUpdate() override + { + const auto t = api_.getCurrentTime(); + const double time_threshold = 1.0; + if (1 < t && t < 1.1) { + api_.requestLaneChange( + "ego", + traffic_simulator::lane_change::RelativeTarget( + "ego", traffic_simulator::lane_change::Direction::RIGHT, 1, 0.0), + traffic_simulator::lane_change::TrajectoryShape::CUBIC, + traffic_simulator::lane_change::Constraint( + traffic_simulator::lane_change::Constraint::Type::TIME, 4.0, + traffic_simulator::lane_change::Constraint::Policy::BEST_EFFORT)); + } + if ( + // api_.getEntityStatus("ego").getLaneletPose() and + static_cast(api_.getEntityStatus("ego").getLaneletPose()) + .lanelet_id == 34462 and + static_cast(api_.getEntityStatus("ego").getLaneletPose()).s >= + 23.0377) { + if (5.0 - time_threshold < t && t < 5.0 + time_threshold) { + stop(cpp_mock_scenarios::Result::SUCCESS); + } else { + stop(cpp_mock_scenarios::Result::FAILURE); + } + } + // LCOV_EXCL_STOP + if (t >= 20) { + stop(cpp_mock_scenarios::Result::FAILURE); + } + } + + void onInitialize() override + { + //Vehicle setting + api_.spawn( + "ego", + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34513, 0, 0, api_.getHdmapUtils()), + getVehicleParameters(), traffic_simulator::VehicleBehavior::contextGamma()); + api_.requestSpeedChange("ego", 7, true); + } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + rclcpp::shutdown(); + return 0; +} diff --git a/mock/cpp_mock_scenarios/src/context_gamma_planner/parked_at_crosswalk.cpp b/mock/cpp_mock_scenarios/src/context_gamma_planner/parked_at_crosswalk.cpp new file mode 100644 index 00000000000..7cb8076d701 --- /dev/null +++ b/mock/cpp_mock_scenarios/src/context_gamma_planner/parked_at_crosswalk.cpp @@ -0,0 +1,101 @@ +// Copyright 2022 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include + +// headers in STL +#include +#include +#include + +class ParkedAtCrosswalkScenario : public cpp_mock_scenarios::CppScenarioNode +{ +public: + explicit ParkedAtCrosswalkScenario(const rclcpp::NodeOptions & option) + : cpp_mock_scenarios::CppScenarioNode( + "parked_at_crosswalk", + ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", "lanelet2_map.osm", + __FILE__, false, option) + { + start(); + } + +private: + void onUpdate() override + { + const auto t = api_.getCurrentTime(); + // LCOV_EXCL_START + + if (api_.getEntityStatus("bob").getMapPose().position.y > 73744.0) { + stop(cpp_mock_scenarios::Result::SUCCESS); + } + // LCOV_EXCL_STOP + if (t >= 60) { + stop(cpp_mock_scenarios::Result::FAILURE); + } + } + + void onInitialize() override + { + api_.spawn( + "ego", + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34981, 0, 0, api_.getHdmapUtils()), + getVehicleParameters()); + api_.setEntityStatus( + "ego", + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34981, 0, 0, api_.getHdmapUtils()), + traffic_simulator::helper::constructActionStatus(5.7)); + api_.requestSpeedChange("ego", 0, true); + api_.requestAssignRoute( + "ego", std::vector{ + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34675, 0.0, 0.0, api_.getHdmapUtils()), + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34690, 0.0, 0.0, api_.getHdmapUtils()), + }); + + api_.spawn( + "bob", + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34378, 0.0, 0, api_.getHdmapUtils()), + getPedestrianParameters(), traffic_simulator::PedestrianBehavior::contextGamma()); + api_.requestSpeedChange( + "bob", 0.5, traffic_simulator::speed_change::Transition::LINEAR, + traffic_simulator::speed_change::Constraint( + traffic_simulator::speed_change::Constraint::Type::LONGITUDINAL_ACCELERATION, 1.0), + true); + const geometry_msgs::msg::Pose goal_pose = traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34378, 7.5, 0, 0, 0, 0, api_.getHdmapUtils())); + api_.requestAcquirePosition("bob", goal_pose); + } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + rclcpp::shutdown(); + return 0; +} diff --git a/mock/cpp_mock_scenarios/src/context_gamma_planner/pedestrian_follow_lane.cpp b/mock/cpp_mock_scenarios/src/context_gamma_planner/pedestrian_follow_lane.cpp new file mode 100644 index 00000000000..6a1e9c993df --- /dev/null +++ b/mock/cpp_mock_scenarios/src/context_gamma_planner/pedestrian_follow_lane.cpp @@ -0,0 +1,94 @@ +// Copyright 2022 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include + +// headers in STL +#include +#include +#include + +class PedestrianFollowLane : public cpp_mock_scenarios::CppScenarioNode +{ +public: + explicit PedestrianFollowLane(const rclcpp::NodeOptions & option) + : cpp_mock_scenarios::CppScenarioNode( + "pedestrian_follow_lane", + ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", "lanelet2_map.osm", + __FILE__, false, option) + { + start(); + } + +private: + int step_ = 0; + const std::vector checkpoint_ids_ = {35016, 35036, 34981}; + void onUpdate() override + { + const auto t = api_.getCurrentTime(); + if ( + static_cast(api_.getEntityStatus("ego").getLaneletPose()) + .lanelet_id == checkpoint_ids_.at(step_)) { + step_++; + } + if (step_ == static_cast(checkpoint_ids_.size())) { + stop(cpp_mock_scenarios::Result::SUCCESS); + } + + // LCOV_EXCL_STOP + if (t >= 100) { + stop(cpp_mock_scenarios::Result::FAILURE); + } + } + + void onInitialize() override + { + api_.spawn( + "ego", + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34426, 10.0, 0, api_.getHdmapUtils()), + getPedestrianParameters(), traffic_simulator::PedestrianBehavior::contextGamma()); + api_.requestSpeedChange( + "ego", 4.0, traffic_simulator::speed_change::Transition::LINEAR, + traffic_simulator::speed_change::Constraint( + traffic_simulator::speed_change::Constraint::Type::LONGITUDINAL_ACCELERATION, 1.0), + true); + api_.requestAssignRoute( + "ego", std::vector{ + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 35016, 0.0, 0, api_.getHdmapUtils()), + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 35026, 0.0, 0, api_.getHdmapUtils()), + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 35036, 0.0, 0, api_.getHdmapUtils()), + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34981, 1.0, 0, api_.getHdmapUtils())}); + } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + rclcpp::shutdown(); + return 0; +} diff --git a/mock/cpp_mock_scenarios/src/context_gamma_planner/reverse_walk_at_road.cpp b/mock/cpp_mock_scenarios/src/context_gamma_planner/reverse_walk_at_road.cpp new file mode 100644 index 00000000000..e763630ed08 --- /dev/null +++ b/mock/cpp_mock_scenarios/src/context_gamma_planner/reverse_walk_at_road.cpp @@ -0,0 +1,123 @@ +// Copyright 2022 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +class ReverseWalkAtRoadScenario : public cpp_mock_scenarios::CppScenarioNode +{ +public: + explicit ReverseWalkAtRoadScenario(const rclcpp::NodeOptions & option) + : cpp_mock_scenarios::CppScenarioNode( + "reverse_walk_at_road", + ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", "lanelet2_map.osm", + __FILE__, false, option) + { + start(); + } + +private: + const geometry_msgs::msg::Pose goal_pose_ = + traffic_simulator::pose::toMapPose(traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34976, 2, 0, 0, 0, 0, api_.getHdmapUtils())); + void onUpdate() override + { + const auto t = api_.getCurrentTime(); + if (6.5 < t && t < 7) { + api_.requestSpeedChange("ego", 0, true); + } + const Eigen::Vector3d target_pose{goal_pose_.position.x, goal_pose_.position.y, 0}; + const Eigen::Vector3d current_pose{ + api_.getEntityStatus("bob").getMapPose().position.x, + api_.getEntityStatus("bob").getMapPose().position.y, 0}; + if ((target_pose - current_pose).norm() < 2.0) { + stop(cpp_mock_scenarios::Result::SUCCESS); + } + // LCOV_EXCL_STOP + if (t >= 120) { + stop(cpp_mock_scenarios::Result::FAILURE); + } + } + + void onInitialize() override + { + api_.spawn( + "ego", + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34976, 0, 0, api_.getHdmapUtils()), + getVehicleParameters(), traffic_simulator::VehicleBehavior::contextGamma()); + api_.setEntityStatus( + "ego", + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34976, 0, 0, api_.getHdmapUtils()), + traffic_simulator::helper::constructActionStatus()); + api_.requestAssignRoute( + "ego", std::vector{ + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34591, 0.0, 0, api_.getHdmapUtils()), + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34690, 0.0, 0, api_.getHdmapUtils()), + }); + api_.requestSpeedChange("ego", 7, true); + + api_.spawn( + "bob", + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34981, 0.0, 0, api_.getHdmapUtils()), + getPedestrianParameters(), traffic_simulator::PedestrianBehavior::contextGamma()); + api_.requestSpeedChange( + "bob", 1, traffic_simulator::speed_change::Transition::LINEAR, + traffic_simulator::speed_change::Constraint( + traffic_simulator::speed_change::Constraint::Type::LONGITUDINAL_ACCELERATION, 1.0), + true); + + auto toVertex = [](double time, const geometry_msgs::msg::Pose & pose) { + traffic_simulator_msgs::msg::Vertex vertex; + vertex.position = pose; + vertex.time = time; + return vertex; + }; + traffic_simulator_msgs::msg::PolylineTrajectory follow_trajectory; + follow_trajectory.shape.vertices.push_back(toVertex( + 0.0, traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34981, 0, 0, 0, 0, 0, api_.getHdmapUtils())))); + follow_trajectory.shape.vertices.push_back(toVertex(30.0, goal_pose_)); + + follow_trajectory.initial_distance_offset = 0.0; + follow_trajectory.dynamic_constraints_ignorable = true; + follow_trajectory.base_time = 0.0; + follow_trajectory.closed = false; + std::shared_ptr follow_trajectory_ptr = + std::make_shared(follow_trajectory); + api_.requestFollowTrajectory("bob", follow_trajectory_ptr); + } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + rclcpp::shutdown(); + return 0; +} diff --git a/mock/cpp_mock_scenarios/src/context_gamma_planner/road_end.cpp b/mock/cpp_mock_scenarios/src/context_gamma_planner/road_end.cpp new file mode 100644 index 00000000000..e4e937a37ef --- /dev/null +++ b/mock/cpp_mock_scenarios/src/context_gamma_planner/road_end.cpp @@ -0,0 +1,82 @@ +// Copyright 2022 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include + +// headers in STL +#include +#include +#include + +class RoadEndScenario : public cpp_mock_scenarios::CppScenarioNode +{ +public: + explicit RoadEndScenario(const rclcpp::NodeOptions & option) + : cpp_mock_scenarios::CppScenarioNode( + "road_end", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", + "lanelet2_map.osm", __FILE__, false, option) + { + start(); + } + +private: + void onUpdate() override + { + const auto t = api_.getCurrentTime(); + if ( + api_.getEntityStatus("ego").getLaneletPose().s > 55.0 and + api_.getEntityStatus("ego").getLaneletPose().lanelet_id == 34468 and + api_.getEntityStatus("ego").getTwist().linear.x < 0.01) { + stop(cpp_mock_scenarios::Result::SUCCESS); + } + // LCOV_EXCL_STOP + if (t >= 30) { + stop(cpp_mock_scenarios::Result::FAILURE); + } + } + + void onInitialize() override + { + //Vehicle setting + api_.spawn( + "ego", + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34468, 30, 0, api_.getHdmapUtils()), + getVehicleParameters(), traffic_simulator::VehicleBehavior::contextGamma()); + api_.requestSpeedChange("ego", 5, true); + api_.requestAssignRoute( + "ego", std::vector{ + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34468, 0.0, 0, api_.getHdmapUtils()), + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34696, 0.0, 0, api_.getHdmapUtils()), + }); + } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + rclcpp::shutdown(); + return 0; +} diff --git a/mock/cpp_mock_scenarios/src/context_gamma_planner/spawn.cpp b/mock/cpp_mock_scenarios/src/context_gamma_planner/spawn.cpp new file mode 100644 index 00000000000..4dfe978c856 --- /dev/null +++ b/mock/cpp_mock_scenarios/src/context_gamma_planner/spawn.cpp @@ -0,0 +1,98 @@ +// Copyright 2015-2020 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include + +// headers in STL +#include +#include +#include + +class SpawnScenario : public cpp_mock_scenarios::CppScenarioNode +{ +public: + explicit SpawnScenario(const rclcpp::NodeOptions & option) + : cpp_mock_scenarios::CppScenarioNode( + "spawn", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", + "lanelet2_map.osm", __FILE__, false, option) + { + start(); + } + +private: + void onUpdate() override + { + const auto t = api_.getCurrentTime(); + + // LCOV_EXCL_STOP + if (t >= 5) { + stop(cpp_mock_scenarios::Result::SUCCESS); + } + } + + void onInitialize() override + { + //Vehicle setting + api_.spawn( + "ego", + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 120545, 0, 0, api_.getHdmapUtils()), + getVehicleParameters(), traffic_simulator::VehicleBehavior::contextGamma()); + api_.setEntityStatus( + "ego", + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 120545, 0, 0, api_.getHdmapUtils()), + traffic_simulator::helper::constructActionStatus(10)); + api_.requestSpeedChange("ego", 0, true); + api_.requestAssignRoute( + "ego", std::vector{ + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34675, 0.0, 0, api_.getHdmapUtils()), + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34690, 0.0, 0, api_.getHdmapUtils()), + }); + + //Pedestrian setting + api_.spawn( + "bob", + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34378, 0.0, 0, api_.getHdmapUtils()), + getPedestrianParameters(), traffic_simulator::PedestrianBehavior::contextGamma()); + api_.requestSpeedChange( + "bob", 0.0, traffic_simulator::speed_change::Transition::LINEAR, + traffic_simulator::speed_change::Constraint( + traffic_simulator::speed_change::Constraint::Type::LONGITUDINAL_ACCELERATION, 1.0), + true); + const geometry_msgs::msg::Pose goal_pose = traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34378, 7.5, 0, 0, 0, 0, api_.getHdmapUtils())); + api_.requestAcquirePosition("bob", goal_pose); + } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + rclcpp::shutdown(); + return 0; +} diff --git a/mock/cpp_mock_scenarios/src/context_gamma_planner/stop_at_crosswalk.cpp b/mock/cpp_mock_scenarios/src/context_gamma_planner/stop_at_crosswalk.cpp new file mode 100644 index 00000000000..722f3048ce7 --- /dev/null +++ b/mock/cpp_mock_scenarios/src/context_gamma_planner/stop_at_crosswalk.cpp @@ -0,0 +1,109 @@ +// Copyright 2022 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include + +// headers in STL +#include +#include +#include + +class StopAtCrosswalkScenario : public cpp_mock_scenarios::CppScenarioNode +{ +public: + explicit StopAtCrosswalkScenario(const rclcpp::NodeOptions & option) + : cpp_mock_scenarios::CppScenarioNode( + "stop_at_crosswalk", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", + "lanelet2_map.osm", __FILE__, false, option) + { + start(); + } + +private: + bool is_stoped_ = false; + void onUpdate() override + { + const auto t = api_.getCurrentTime(); + if ( + static_cast(api_.getEntityStatus("ego").getLaneletPose()) + .lanelet_id == 34981 && + api_.getCurrentTwist("ego").linear.x < 2.0) { + is_stoped_ = true; + } + if ( + static_cast(api_.getEntityStatus("ego").getLaneletPose()) + .lanelet_id == 34579 && + is_stoped_) { + stop(cpp_mock_scenarios::Result::SUCCESS); + } + // LCOV_EXCL_STOP + if (t >= 40) { + stop(cpp_mock_scenarios::Result::FAILURE); + } + } + + void onInitialize() override + { + //Vehicle setting + api_.spawn( + "ego", + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 120545, 0, 0, api_.getHdmapUtils()), + getVehicleParameters(), traffic_simulator::VehicleBehavior::contextGamma()); + api_.setEntityStatus( + "ego", + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 120545, 0, 0, api_.getHdmapUtils()), + traffic_simulator::helper::constructActionStatus(10)); + api_.requestSpeedChange("ego", 7, true); + api_.requestAssignRoute( + "ego", std::vector{ + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34675, 0.0, 0, api_.getHdmapUtils()), + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34690, 0.0, 0, api_.getHdmapUtils())}); + + //Pedestrian setting + api_.spawn( + "bob", + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34378, 0.0, 0, api_.getHdmapUtils()), + getPedestrianParameters(), traffic_simulator::PedestrianBehavior::contextGamma()); + api_.requestSpeedChange( + "bob", 0.5, traffic_simulator::speed_change::Transition::LINEAR, + traffic_simulator::speed_change::Constraint( + traffic_simulator::speed_change::Constraint::Type::LONGITUDINAL_ACCELERATION, 1.0), + true); + const geometry_msgs::msg::Pose goal_pose = traffic_simulator::pose::toMapPose( + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34378, 7.5, 0, 0, 0, 0, api_.getHdmapUtils())); + api_.requestAcquirePosition("bob", goal_pose); + } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + rclcpp::shutdown(); + return 0; +} diff --git a/mock/cpp_mock_scenarios/src/context_gamma_planner/stop_line_running.cpp b/mock/cpp_mock_scenarios/src/context_gamma_planner/stop_line_running.cpp new file mode 100644 index 00000000000..1928e72efa6 --- /dev/null +++ b/mock/cpp_mock_scenarios/src/context_gamma_planner/stop_line_running.cpp @@ -0,0 +1,104 @@ +// Copyright 2022 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include + +// headers in STL +#include +#include +#include + +class StopLineRunningScenario : public cpp_mock_scenarios::CppScenarioNode +{ +public: + explicit StopLineRunningScenario(const rclcpp::NodeOptions & option) + : cpp_mock_scenarios::CppScenarioNode( + "stop_line_running", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", + "lanelet2_map.osm", __FILE__, false, option) + { + start(); + } + +private: + bool is_stoped_ = false; + int stop_count_ = 0; + void onUpdate() override + { + const auto t = api_.getCurrentTime(); + if ( + static_cast(api_.getEntityStatus("ego").getLaneletPose()) + .lanelet_id == 34675 and + api_.getCurrentTwist("ego").linear.x < 0.2) { + is_stoped_ = true; + } + if ( + is_stoped_ and + static_cast(api_.getEntityStatus("ego").getLaneletPose()) + .lanelet_id == 34744) { + is_stoped_ = false; + stop_count_++; + } + if (stop_count_ == 2) { + stop(cpp_mock_scenarios::Result::SUCCESS); + } + // LCOV_EXCL_STOP + if (t >= 200) { + stop(cpp_mock_scenarios::Result::FAILURE); + } + } + + void onInitialize() override + { + //Vehicle setting + api_.spawn( + "ego", + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34579, 0, 0, api_.getHdmapUtils()), + getVehicleParameters(), traffic_simulator::VehicleBehavior::contextGamma()); + api_.setEntityStatus( + "ego", + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34579, 0, 0, api_.getHdmapUtils()), + traffic_simulator::helper::constructActionStatus(10)); + api_.requestSpeedChange("ego", 7, true); + api_.requestAssignRoute( + "ego", std::vector{ + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34675, 0.0, 0, api_.getHdmapUtils()), + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34690, 0.0, 0, api_.getHdmapUtils()), + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34576, 0.0, 0, api_.getHdmapUtils()), + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34579, 0.0, 0, api_.getHdmapUtils()), + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34675, 0.0, 0, api_.getHdmapUtils())}); + } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + rclcpp::shutdown(); + return 0; +} diff --git a/mock/cpp_mock_scenarios/src/context_gamma_planner/traffic_light_running.cpp b/mock/cpp_mock_scenarios/src/context_gamma_planner/traffic_light_running.cpp new file mode 100644 index 00000000000..1c517bbe646 --- /dev/null +++ b/mock/cpp_mock_scenarios/src/context_gamma_planner/traffic_light_running.cpp @@ -0,0 +1,113 @@ +// Copyright 2022 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include + +// headers in STL +#include +#include +#include + +class TrafficLightRunningScenario : public cpp_mock_scenarios::CppScenarioNode +{ +public: + explicit TrafficLightRunningScenario(const rclcpp::NodeOptions & option) + : cpp_mock_scenarios::CppScenarioNode( + "traffic_light_running", + ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", "lanelet2_map.osm", + __FILE__, false, option) + { + start(); + } + +private: + const traffic_simulator::TrafficLight::Bulb green_{ + traffic_simulator::TrafficLight::Color::green, + traffic_simulator::TrafficLight::Status::solid_on, + traffic_simulator::TrafficLight::Shape::circle}; + const traffic_simulator::TrafficLight::Bulb red_{ + traffic_simulator::TrafficLight::Color::red, traffic_simulator::TrafficLight::Status::solid_on, + traffic_simulator::TrafficLight::Shape::circle}; + void onUpdate() override + { + const auto t = api_.getCurrentTime(); + + // traffic light control + if (1.0 <= t && t <= 1.1) { + api_.getConventionalTrafficLight(34836).emplace(red_); + api_.getConventionalTrafficLight(34802).emplace(red_); + } + if (10.0 <= t && t <= 10.1) { + api_.getConventionalTrafficLight(34836).clear(); + api_.getConventionalTrafficLight(34802).clear(); + api_.getConventionalTrafficLight(34836).emplace(green_); + api_.getConventionalTrafficLight(34802).emplace(green_); + } + + if ( + t < 10.1 && + static_cast(api_.getEntityStatus("ego").getLaneletPose()) + .lanelet_id == 34624) { + stop(cpp_mock_scenarios::Result::FAILURE); + } + if ( + t >= 10.1 && + static_cast(api_.getEntityStatus("ego").getLaneletPose()) + .lanelet_id == 34624) { + stop(cpp_mock_scenarios::Result::SUCCESS); + } + // LCOV_EXCL_STOP + if (t >= 30) { + stop(cpp_mock_scenarios::Result::FAILURE); + } + } + + void onInitialize() override + { + //Vehicle setting + api_.spawn( + "ego", + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34408, 0, 0, api_.getHdmapUtils()), + getVehicleParameters(), traffic_simulator::VehicleBehavior::contextGamma()); + api_.setEntityStatus( + "ego", + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34408, 0, 0, api_.getHdmapUtils()), + traffic_simulator::helper::constructActionStatus(10)); + api_.requestSpeedChange("ego", 5, true); + api_.requestAssignRoute( + "ego", std::vector{ + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34630, 0.0, 0, api_.getHdmapUtils()), + traffic_simulator::helper::constructCanonicalizedLaneletPose( + 34696, 0.0, 0, api_.getHdmapUtils())}); + } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + rclcpp::shutdown(); + return 0; +} diff --git a/simulation/context_gamma_planner/context_gamma_planner/CMakeLists.txt b/simulation/context_gamma_planner/context_gamma_planner/CMakeLists.txt new file mode 100644 index 00000000000..48d48d5663d --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/CMakeLists.txt @@ -0,0 +1,70 @@ +cmake_minimum_required(VERSION 3.8) +project(context_gamma_planner) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake_auto REQUIRED) +include(FindProtobuf REQUIRED) +#find_package (glog 0.6.0 REQUIRED) + + +ament_auto_find_build_dependencies() + +ament_auto_add_library(context_gamma_planner SHARED + src/behavior/action_node_base.cpp + src/behavior/pedestrian/action_node.cpp + src/behavior/pedestrian/follow_lane_action.cpp + src/behavior/pedestrian/follow_polyline_trajectory_action.cpp + src/behavior/vehicle/action_node.cpp + src/behavior/vehicle/follow_lane_action.cpp + src/behavior/vehicle/follow_polyline_trajectory_action.cpp + src/behavior/vehicle/lane_change_action.cpp + src/constraints/constraint_activator_base.cpp + src/constraints/constraint_base.cpp + src/constraints/pedestrian/constraint_activator.cpp + src/constraints/road_edge_constraint.cpp + src/constraints/stop_line_constraint.cpp + src/constraints/traffic_light_constraint.cpp + src/constraints/vehicle/constraint_activator.cpp + src/pedestrian_plugin.cpp + src/planner/goal_planner_base.cpp + src/planner/follow_polyline_trajectory_planner_base.cpp + src/planner/pedestrian/follow_lane_planner.cpp + src/planner/pedestrian/follow_polyline_trajectory_planner.cpp + src/planner/vehicle/follow_lane_planner.cpp + src/planner/vehicle/follow_polyline_trajectory_planner.cpp + src/planner/vehicle/lane_change_planner.cpp + src/transition_events/logging_event.cpp + src/transition_events/reset_request_event.cpp + src/transition_events/transition_event.cpp + src/vehicle_plugin.cpp +) + +target_link_libraries(${PROJECT_NAME} + glog +) + +pluginlib_export_plugin_description_file(traffic_simulator plugins.xml) + +if(${tf2_geometry_msgs_VERSION} VERSION_LESS 0.18.0) + target_compile_definitions(context_gamma_planner PUBLIC + USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER + ) +endif() + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(test) +endif() + +install( + DIRECTORY config test/maps launch + DESTINATION share/${PROJECT_NAME} +) + +ament_auto_package() diff --git a/simulation/context_gamma_planner/context_gamma_planner/config/pedestrian_behavior.xml b/simulation/context_gamma_planner/context_gamma_planner/config/pedestrian_behavior.xml new file mode 100644 index 00000000000..d776bb412da --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/config/pedestrian_behavior.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/simulation/context_gamma_planner/context_gamma_planner/config/vehicle_behavior.xml b/simulation/context_gamma_planner/context_gamma_planner/config/vehicle_behavior.xml new file mode 100644 index 00000000000..9e83f4baa64 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/config/vehicle_behavior.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/behavior/action_node_base.hpp b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/behavior/action_node_base.hpp new file mode 100644 index 00000000000..a12733f2339 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/behavior/action_node_base.hpp @@ -0,0 +1,104 @@ +// Copyright 2015-2020 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTEXT_GAMMA_PLANNER__ACTION_NODE_HPP_ +#define CONTEXT_GAMMA_PLANNER__ACTION_NODE_HPP_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace context_gamma_planner +{ +class ActionNodeBase : public BT::ActionNodeBase +{ +public: + ActionNodeBase(const std::string & name, const BT::NodeConfiguration & config); + ~ActionNodeBase() override = default; + /// throws if the derived class return RUNNING. + auto executeTick() -> BT::NodeStatus override; + + /** + * @brief Halts the execution of the action node. + */ + void halt() override final { setStatus(BT::NodeStatus::IDLE); } + + /** + * @brief A list of ports for a behavior tree node. + */ + static BT::PortsList providedPorts() + { + return { + // clang-format off + BT::InputPort>("activator"), + BT::InputPort("request"), + BT::InputPort>("hdmap_utils"), + BT::InputPort>("canonicalized_entity_status"), + BT::InputPort("current_time"), + BT::InputPort("step_time"), + BT::InputPort("other_entity_status"), + BT::InputPort>("goal_poses"), + BT::InputPort>("route_lanelets"), + BT::InputPort>("target_speed"), + BT::OutputPort>("planning_speed"), + BT::OutputPort("next_goal"), + BT::OutputPort>("updated_status"), + BT::OutputPort("request"), + BT::InputPort("other_entity_status"), + BT::InputPort>("route_lanelets"), + BT::InputPort>("traffic_lights"), + BT::OutputPort>("hdmap_utils"), + BT::OutputPort>("obstacle"), + BT::OutputPort("waypoints")}; + // clang-format on + } + + /** + * @brief This function retrieves the values stored in the blackboard and performs any necessary operations. + */ + auto getBlackBoardValues() -> void; + +protected: + std::shared_ptr activator; + traffic_simulator::behavior::Request request; + std::shared_ptr hdmap_utils; + std::shared_ptr traffic_lights; + std::shared_ptr entity_status; + double current_time; + double step_time; + std::optional target_speed; + std::shared_ptr updated_status; + entity_behavior::EntityStatusDict other_entity_status; + std::vector route_lanelets; +}; +} // namespace context_gamma_planner + +#endif // CONTEXT_GAMMA_PLANNER__ACTION_NODE_HPP_ \ No newline at end of file diff --git a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/behavior/pedestrian/action_node.hpp b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/behavior/pedestrian/action_node.hpp new file mode 100644 index 00000000000..40414513b65 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/behavior/pedestrian/action_node.hpp @@ -0,0 +1,59 @@ +// Copyright 2015-2020 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTEXT_GAMMA_PLANNER__PEDESTRIAN__PEDESTRIAN_ACTION_NODE_HPP_ +#define CONTEXT_GAMMA_PLANNER__PEDESTRIAN__PEDESTRIAN_ACTION_NODE_HPP_ + +#include + +#include +#include +#include +#include +#include + +namespace context_gamma_planner +{ +namespace pedestrian +{ +class ActionNode : public ActionNodeBase +{ +public: + /** + * @brief Represents an action node in the behavior tree. + * @param name The name of the action node. + * @param config The configuration for the action node. + */ + ActionNode(const std::string & name, const BT::NodeConfiguration & config); + void getBlackBoardValues(); + + /** + * @brief A list of ports for a behavior tree node. + */ + static BT::PortsList providedPorts() + { + BT::PortsList ports = { + BT::InputPort("pedestrian_parameters")}; + BT::PortsList parent_ports = context_gamma_planner::ActionNodeBase::providedPorts(); + for (const auto & parent_port : parent_ports) { + ports.emplace(parent_port.first, parent_port.second); + } + return ports; + } + traffic_simulator_msgs::msg::PedestrianParameters pedestrian_parameters; +}; +} // namespace pedestrian +} // namespace context_gamma_planner + +#endif // CONTEXT_GAMMA_PLANNER__PEDESTRIAN__PEDESTRIAN_ACTION_NODE_HPP_ diff --git a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/behavior/pedestrian/follow_lane_action.hpp b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/behavior/pedestrian/follow_lane_action.hpp new file mode 100644 index 00000000000..9af4cb4dfad --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/behavior/pedestrian/follow_lane_action.hpp @@ -0,0 +1,65 @@ +// Copyright 2015-2020 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTEXT_GAMMA_PLANNER__BEHAVIOR__PEDESTRIAN__FOLLOW_LANE_ACTION_HPP_ +#define CONTEXT_GAMMA_PLANNER__BEHAVIOR__PEDESTRIAN__FOLLOW_LANE_ACTION_HPP_ + +#include +#include + +namespace context_gamma_planner +{ +namespace pedestrian +{ +class FollowLaneAction : public context_gamma_planner::pedestrian::ActionNode +{ +public: + /** + * @brief Represents an action node in the behavior tree. + * @param name The name of the action node. + * @param config The configuration for the action node. + */ + FollowLaneAction(const std::string & name, const BT::NodeConfiguration & config); + + /** + * @brief update function + * @return node status : RUNNING, SUCCESS, FAILURE, and IDLE. + */ + BT::NodeStatus tick() override; + + /** + * @brief This function retrieves the values stored in the blackboard and performs any necessary operations. + */ + void getBlackBoardValues(); + + /** + * @brief A list of ports for a behavior tree node. + */ + static BT::PortsList providedPorts() + { + BT::PortsList ports = {}; + BT::PortsList parent_ports = context_gamma_planner::pedestrian::ActionNode::providedPorts(); + for (const auto & parent_port : parent_ports) { + ports.emplace(parent_port.first, parent_port.second); + } + return ports; + } + +private: + FollowLanePlanner planner_; +}; +} // namespace pedestrian +} // namespace context_gamma_planner + +#endif // CONTEXT_GAMMA_PLANNER__BEHAVIOR__PEDESTRIAN__FOLLOW_LANE_ACTION_HPP_ diff --git a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/behavior/pedestrian/follow_polyline_trajectory_action.hpp b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/behavior/pedestrian/follow_polyline_trajectory_action.hpp new file mode 100644 index 00000000000..0e4d6dcbdd8 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/behavior/pedestrian/follow_polyline_trajectory_action.hpp @@ -0,0 +1,70 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTEXT_GAMMA_PLANNER__PEDESTRIAN__FOLLW_POLYLINE_TRAJECTORY_ACTION_HPP_ +#define CONTEXT_GAMMA_PLANNER__PEDESTRIAN__FOLLW_POLYLINE_TRAJECTORY_ACTION_HPP_ + +#include +#include +#include +#include + +namespace context_gamma_planner +{ +namespace pedestrian +{ +class FollowPolylineTrajectoryAction : public context_gamma_planner::pedestrian::ActionNode +{ +public: + /** + * @brief Represents an action node in the behavior tree. + * @param name The name of the action node. + * @param config The configuration for the action node. + */ + FollowPolylineTrajectoryAction(const std::string & name, const BT::NodeConfiguration & config); + + /** + * @brief update function + * @return node status : RUNNING, SUCCESS, FAILURE, and IDLE. + */ + BT::NodeStatus tick() override; + + /** + * @brief This function retrieves the values stored in the blackboard and performs any necessary operations. + */ + void getBlackBoardValues(); + + /** + * @brief A list of ports for a behavior tree node. + */ + static BT::PortsList providedPorts() + { + BT::PortsList ports = { + BT::InputPort>( + "polyline_trajectory")}; + BT::PortsList parent_ports = context_gamma_planner::pedestrian::ActionNode::providedPorts(); + for (const auto & parent_port : parent_ports) { + ports.emplace(parent_port.first, parent_port.second); + } + return ports; + } + +private: + std::shared_ptr polyline_trajectory_; + FollowPolylineTrajectoryPlanner planner_; +}; +} // namespace pedestrian +} // namespace context_gamma_planner + +#endif // CONTEXT_GAMMA_PLANNER__PEDESTRIAN__FOLLW_POLYLINE_TRAJECTORY_ACTION_HPP_ diff --git a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/behavior/vehicle/action_node.hpp b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/behavior/vehicle/action_node.hpp new file mode 100644 index 00000000000..2385642e295 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/behavior/vehicle/action_node.hpp @@ -0,0 +1,73 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTEXT_GAMMA_PLANNER__PEDESTRIAN__VEHICLE_ACTION_NODE_HPP_ +#define CONTEXT_GAMMA_PLANNER__PEDESTRIAN__VEHICLE_ACTION_NODE_HPP_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace context_gamma_planner +{ +namespace vehicle +{ +class ActionNode : public ActionNodeBase +{ +public: + /** + * @brief Represents an action node in the behavior tree. + * @param name The name of the action node. + * @param config The configuration for the action node. + */ + ActionNode(const std::string & name, const BT::NodeConfiguration & config); + ~ActionNode() override = default; + void getBlackBoardValues(); + + /** + * @brief A list of ports for a behavior tree node. + */ + static BT::PortsList providedPorts() + { + BT::PortsList ports = { + BT::InputPort("behavior_parameter"), + BT::InputPort("vehicle_parameters"), + BT::InputPort>("reference_trajectory")}; + BT::PortsList parent_ports = context_gamma_planner::ActionNodeBase::providedPorts(); + for (const auto & parent_port : parent_ports) { + ports.emplace(parent_port.first, parent_port.second); + } + return ports; + } + +protected: + traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter; + traffic_simulator_msgs::msg::VehicleParameters vehicle_parameters; + std::shared_ptr reference_trajectory; + std::unique_ptr trajectory; +}; +} // namespace vehicle +} // namespace context_gamma_planner +#endif // CONTEXT_GAMMA_PLANNER__PEDESTRIAN__VEHICLE_ACTION_NODE_HPP_ diff --git a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/behavior/vehicle/follow_lane_action.hpp b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/behavior/vehicle/follow_lane_action.hpp new file mode 100644 index 00000000000..9bcea9d3c6b --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/behavior/vehicle/follow_lane_action.hpp @@ -0,0 +1,67 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTEXT_GAMMA_PLANNER__VEHICLE__FOLLOW_LANE_ACTION_HPP_ +#define CONTEXT_GAMMA_PLANNER__VEHICLE__FOLLOW_LANE_ACTION_HPP_ + +#include +#include +#include +#include + +namespace context_gamma_planner +{ +namespace vehicle +{ +class FollowLaneAction : public context_gamma_planner::vehicle::ActionNode +{ +public: + /** + * @brief Represents an action node in the behavior tree. + * @param name The name of the action node. + * @param config The configuration for the action node. + */ + FollowLaneAction(const std::string & name, const BT::NodeConfiguration & config); + + /** + * @brief update function + * @return node status : RUNNING, SUCCESS, FAILURE, and IDLE. + */ + BT::NodeStatus tick() override; + + /** + * @brief This function retrieves the values stored in the blackboard and performs any necessary operations. + */ + void getBlackBoardValues(); + + /** + * @brief A list of ports for a behavior tree node. + */ + static BT::PortsList providedPorts() + { + BT::PortsList ports = {}; + BT::PortsList parent_ports = context_gamma_planner::vehicle::ActionNode::providedPorts(); + for (const auto & parent_port : parent_ports) { + ports.emplace(parent_port.first, parent_port.second); + } + return ports; + } + +private: + FollowLanePlanner planner_; +}; +} // namespace vehicle +} // namespace context_gamma_planner + +#endif // CONTEXT_GAMMA_PLANNER__VEHICLE__FOLLOW_LANE_ACTION_HPP_ diff --git a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/behavior/vehicle/follow_polyline_trajectory_action.hpp b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/behavior/vehicle/follow_polyline_trajectory_action.hpp new file mode 100644 index 00000000000..0e49ea4df65 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/behavior/vehicle/follow_polyline_trajectory_action.hpp @@ -0,0 +1,70 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTEXT_GAMMA_PLANNER__VEHICLE__FOLLW_POLYLINE_TRAJECTORY_ACTION_HPP_ +#define CONTEXT_GAMMA_PLANNER__VEHICLE__FOLLW_POLYLINE_TRAJECTORY_ACTION_HPP_ + +#include +#include +#include +#include + +namespace context_gamma_planner +{ +namespace vehicle +{ +class FollowPolylineTrajectoryAction : public context_gamma_planner::vehicle::ActionNode +{ +public: + /** + * @brief Represents an action node in the behavior tree. + * @param name The name of the action node. + * @param config The configuration for the action node. + */ + FollowPolylineTrajectoryAction(const std::string & name, const BT::NodeConfiguration & config); + + /** + * @brief update function + * @return node status : RUNNING, SUCCESS, FAILURE, and IDLE. + */ + BT::NodeStatus tick() override; + + /** + * @brief This function retrieves the values stored in the blackboard and performs any necessary operations. + */ + void getBlackBoardValues(); + + /** + * @brief A list of ports for a behavior tree node. + */ + static BT::PortsList providedPorts() + { + BT::PortsList ports = { + BT::InputPort>( + "polyline_trajectory")}; + BT::PortsList parent_ports = context_gamma_planner::vehicle::ActionNode::providedPorts(); + for (const auto & parent_port : parent_ports) { + ports.emplace(parent_port.first, parent_port.second); + } + return ports; + } + +private: + std::shared_ptr polyline_trajectory_; + FollowPolylineTrajectoryPlanner planner_; +}; +} // namespace vehicle +} // namespace context_gamma_planner + +#endif // CONTEXT_GAMMA_PLANNER__VEHICLE__FOLLW_POLYLINE_TRAJECTORY_ACTION_HPP_ diff --git a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/behavior/vehicle/lane_change_action.hpp b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/behavior/vehicle/lane_change_action.hpp new file mode 100644 index 00000000000..bcc41313867 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/behavior/vehicle/lane_change_action.hpp @@ -0,0 +1,69 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTEXT_GAMMA_PLANNER__VEHICLE__LANE_CHANGE_ACTION_HPP_ +#define CONTEXT_GAMMA_PLANNER__VEHICLE__LANE_CHANGE_ACTION_HPP_ + +#include +#include +#include +#include + +namespace context_gamma_planner +{ +namespace vehicle +{ +class LaneChangeAction : public context_gamma_planner::vehicle::ActionNode +{ +public: + /** + * @brief Represents an action node in the behavior tree. + * @param name The name of the action node. + * @param config The configuration for the action node. + */ + LaneChangeAction(const std::string & name, const BT::NodeConfiguration & config); + + /** + * @brief update function + * @return node status : RUNNING, SUCCESS, FAILURE, and IDLE. + */ + BT::NodeStatus tick() override; + + /** + * @brief This function retrieves the values stored in the blackboard and performs any necessary operations. + */ + void getBlackBoardValues(); + + /** + * @brief A list of ports for a behavior tree node. + */ + static BT::PortsList providedPorts() + { + BT::PortsList ports = { + BT::InputPort("lane_change_parameters")}; + BT::PortsList parent_ports = context_gamma_planner::vehicle::ActionNode::providedPorts(); + for (const auto & parent_port : parent_ports) { + ports.emplace(parent_port.first, parent_port.second); + } + return ports; + } + +private: + traffic_simulator::lane_change::Parameter lane_change_parameters_; + LaneChangePlanner planner_; +}; +} // namespace vehicle +} // namespace context_gamma_planner + +#endif // CONTEXT_GAMMA_PLANNER__VEHICLE__LANE_CHANGE_ACTION_HPP_ diff --git a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/constraints/constraint_activator_base.hpp b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/constraints/constraint_activator_base.hpp new file mode 100644 index 00000000000..22c29382e33 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/constraints/constraint_activator_base.hpp @@ -0,0 +1,111 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTEXT_GAMMA_PLANNER__CONSTRAINTS__CONSTRAINT_ACTIVATOR_HPP_ +#define CONTEXT_GAMMA_PLANNER__CONSTRAINTS__CONSTRAINT_ACTIVATOR_HPP_ + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace context_gamma_planner +{ +namespace constraints +{ +template +void activateAll(std::vector & constraints) +{ + for (auto & constraint : constraints) { + constraint.setState(State::ACTIVE); + } +} + +template +void deactivateAll(std::vector & constraints) +{ + for (auto & constraint : constraints) { + constraint.setState(State::INACTIVE); + } +} + +template +std::vector filter(std::vector & constraints, const State & state) +{ + std::vector filtered; + std::for_each(constraints.begin(), constraints.end(), [state, &filtered](T & x) mutable -> void { + if (x.getState() == state) { + filtered.emplace_back(x); + } + }); + return filtered; +} + +class ConstraintActivatorBase +{ +public: + ConstraintActivatorBase( + const std::shared_ptr hd_map_utils_ptr, + const std::shared_ptr traffic_lights_ptr); + const auto calculateRVOObstacles() -> std::vector >; + void deactivateAllConstraints(); + void appendRoadEdgeConstraint(const lanelet::Id lanelet_id, const State & state = State::ACTIVE); + void appendRoadEdgeConstraint( + const lanelet::Id lanelet_id, const RoadEdgeConstraint::Side & side, + const State & state = State::ACTIVE); + void appendRoadEdgeConstraint( + const lanelet::Ids & route_ids, const State & state = State::ACTIVE); + + virtual void appendLaneChangeConstraint( + const std::shared_ptr &, + const traffic_simulator::lane_change::Parameter &); + /// @param stop_velocity_threshold [m/s] Set a small value since a complete stop is not possible. + virtual void appendStopLineConstraint( + const lanelet::Ids &, const std::shared_ptr, + const std::shared_ptr &, const double = 4.0, + const double = 0.1); + virtual void appendTrafficLightConstraint(const lanelet::Ids &); + /// @todo This function needs to be divided into a route planning function and a part that adds constraints. + virtual void appendPedestrianRouteConstraint( + const geometry_msgs::msg::Pose &, const geometry_msgs::msg::Pose &, const lanelet::Ids &, + const std::vector &); + virtual void appendRoadEndConstraint(const lanelet::Ids &); + void appendPreviousRoadEdgeConstraint(const lanelet::Ids &); + +private: + void appendPolygonToRVOObstacles(const Polygon & polygon); + void appendPolygonsToRVOObstacles(const Polygons & polygon); + +protected: + std::vector queryRoadEdgeConstraint( + const geometry_msgs::msg::Point & p, double distance_threashold, const char subtype[]); + std::shared_ptr hd_map_utils_ptr_; + std::shared_ptr traffic_lights_ptr_; + std::vector lanelet_subtypes_; + std::vector traffic_light_constraints_; + std::vector lane_constraints_; + std::vector stop_line_constraints_; + std::vector > obstacles_; + lanelet::Ids route_ids_; + bool is_stoped_; +}; +} // namespace constraints +} // namespace context_gamma_planner + +#endif // CONTEXT_GAMMA_PLANNER__CONSTRAINTS__CONSTRAINT_ACTIVATOR_HPP_ diff --git a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/constraints/constraint_base.hpp b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/constraints/constraint_base.hpp new file mode 100644 index 00000000000..b0a6a4cccf8 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/constraints/constraint_base.hpp @@ -0,0 +1,86 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTEXT_GAMMA_PLANNER__CONSTRAINTS__CONSTRAINT_BASE_HPP_ +#define CONTEXT_GAMMA_PLANNER__CONSTRAINTS__CONSTRAINT_BASE_HPP_ + +#include +#include +#include +#include + +#define OVERRAY_CONSTRAINT "overlay" + +namespace context_gamma_planner +{ +namespace constraints +{ +typedef std::vector Polygon; +std::ostream & operator<<(std::ostream & os, const Polygon & polygon); + +typedef std::vector Polygons; +std::ostream & operator<<(std::ostream & os, const Polygons & polygons); + +enum class State { ACTIVE = 0, INACTIVE = 1 }; + +class ConstraintBase +{ +public: + ConstraintBase( + const std::shared_ptr & hdmap_utils, const std::string & type, + const Polygons & polygons); + ConstraintBase(const ConstraintBase & obj); + const Polygons & getPolygons() const; + const std::string type; + ConstraintBase operator+(ConstraintBase constraint); + ConstraintBase operator=(ConstraintBase constraint); + void setState(const State & state); + const State & getState() const; + +protected: + std::shared_ptr hdmap_utils_ptr_; + +private: + Polygons polygons_; + State state_; +}; + +std::ostream & operator<<(std::ostream & os, const ConstraintBase & constraint); + +template +void setStateByStopLineId( + std::vector & constraints, const lanelet::Id lanelet_id, + const State & state = State::ACTIVE) +{ + std::for_each(constraints.begin(), constraints.end(), [&](auto & constraint) { + if (constraint.stop_line_id == lanelet_id) { + constraint.setState(state); + } + }); +} + +template +void setStateByStopLineId( + std::vector & constraints, const std::vector & lanelet_ids, + const State & state = State::ACTIVE) +{ + std::for_each(lanelet_ids.begin(), lanelet_ids.end(), [&](const auto lanelet_id) { + setStateByStopLineId(constraints, lanelet_id, state); + }); +} + +} // namespace constraints +} // namespace context_gamma_planner + +#endif // CONTEXT_GAMMA_PLANNER__CONSTRAINTS__CONSTRAINT_BASE_HPP_ diff --git a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/constraints/pedestrian/constraint_activator.hpp b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/constraints/pedestrian/constraint_activator.hpp new file mode 100644 index 00000000000..c32d7102fd5 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/constraints/pedestrian/constraint_activator.hpp @@ -0,0 +1,43 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTEXT_GAMMA_PLANNER__PEDESTRIAN__CONSTRAINTS__CONSTRAINT_ACTIVATOR_HPP_ +#define CONTEXT_GAMMA_PLANNER__PEDESTRIAN__CONSTRAINTS__CONSTRAINT_ACTIVATOR_HPP_ + +#include + +#include +#include +#include +#include +#include + +namespace context_gamma_planner +{ +namespace pedestrian +{ +namespace constraints +{ +class ConstraintActivator : public context_gamma_planner::constraints::ConstraintActivatorBase +{ +public: + ConstraintActivator( + const std::shared_ptr hd_map_utils_ptr, + const std::shared_ptr traffic_lights_ptr); +}; +} // namespace constraints +} // namespace pedestrian +} // namespace context_gamma_planner + +#endif // CONTEXT_GAMMA_PLANNER__PEDESTRIAN__CONSTRAINTS__CONSTRAINT_ACTIVATOR_HPP_ diff --git a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/constraints/road_edge_constraint.hpp b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/constraints/road_edge_constraint.hpp new file mode 100644 index 00000000000..cc538066d33 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/constraints/road_edge_constraint.hpp @@ -0,0 +1,48 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTEXT_GAMMA_PLANNER__CONSTRAINTS__LANE_CONSTRAINT_HPP_ +#define CONTEXT_GAMMA_PLANNER__CONSTRAINTS__LANE_CONSTRAINT_HPP_ + +#include +#include + +#define LANE_CONSTRAINT "lane" + +namespace context_gamma_planner +{ +namespace constraints +{ +class RoadEdgeConstraint : public ConstraintBase +{ +public: + enum class Side { LEFT = 0, RIGHT = 1, FRONT = 2, BACK = 3 }; + RoadEdgeConstraint( + const std::shared_ptr & hdmap_utils, const lanelet::Id lanelet_id, + const Side & side); + const lanelet::Id lanelet_id; + const Side side; +}; + +void setState( + std::vector & constraints, const lanelet::Id lanelet_ids, + const RoadEdgeConstraint::Side & side, const State & state = State::ACTIVE); + +void setState( + std::vector & constraints, const std::vector & lanelet_ids, + const State & state = State::ACTIVE); +} // namespace constraints +} // namespace context_gamma_planner + +#endif // CONTEXT_GAMMA_PLANNER__CONSTRAINTS__LANE_CONSTRAINT_HPP_ diff --git a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/constraints/stop_line_constraint.hpp b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/constraints/stop_line_constraint.hpp new file mode 100644 index 00000000000..709e7191826 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/constraints/stop_line_constraint.hpp @@ -0,0 +1,37 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTEXT_GAMMA_PLANNER__CONSTRAINTS__STOP_LINE_CONSTRAINT_HPP_ +#define CONTEXT_GAMMA_PLANNER__CONSTRAINTS__STOP_LINE_CONSTRAINT_HPP_ + +#include +#include + +#define STOP_LINE_CONSTRAINT "stop_line" + +namespace context_gamma_planner +{ +namespace constraints +{ +class StopLineConstraint : public ConstraintBase +{ +public: + StopLineConstraint( + const std::shared_ptr & hdmap_utils, const lanelet::Id stop_line_id); + const lanelet::Id stop_line_id; +}; +} // namespace constraints +} // namespace context_gamma_planner + +#endif // CONTEXT_GAMMA_PLANNER__CONSTRAINTS__STOP_LINE_CONSTRAINT_HPP_ diff --git a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/constraints/traffic_light_constraint.hpp b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/constraints/traffic_light_constraint.hpp new file mode 100644 index 00000000000..bf63f0d3742 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/constraints/traffic_light_constraint.hpp @@ -0,0 +1,40 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTEXT_GAMMA_PLANNER__CONSTRAINTS__TRAFFIC_LIGHT_CONSTRAINT_HPP_ +#define CONTEXT_GAMMA_PLANNER__CONSTRAINTS__TRAFFIC_LIGHT_CONSTRAINT_HPP_ + +#include +#include +#include + +#define TRAFFIC_LIGHT_CONSTRAINT "traffic_light" + +namespace context_gamma_planner +{ +namespace constraints +{ +class TrafficLightConstraint : public ConstraintBase +{ +public: + TrafficLightConstraint( + const std::shared_ptr & hdmap_utils, + const lanelet::Id traffic_light_id, const lanelet::Id stop_line_id); + const lanelet::Id traffic_light_id; + const lanelet::Id stop_line_id; +}; +} // namespace constraints +} // namespace context_gamma_planner + +#endif // CONTEXT_GAMMA_PLANNER__CONSTRAINTS__TRAFFIC_LIGHT_CONSTRAINT_HPP_ diff --git a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/constraints/vehicle/constraint_activator.hpp b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/constraints/vehicle/constraint_activator.hpp new file mode 100644 index 00000000000..15d985516ce --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/constraints/vehicle/constraint_activator.hpp @@ -0,0 +1,55 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTEXT_GAMMA_PLANNER__VEHICLE__CONSTRAINTS__CONSTRAINT_ACTIVATOR_HPP_ +#define CONTEXT_GAMMA_PLANNER__VEHICLE__CONSTRAINTS__CONSTRAINT_ACTIVATOR_HPP_ + +#include + +#include +#include +#include +#include +#include + +namespace context_gamma_planner +{ +namespace vehicle +{ +namespace constraints +{ +class ConstraintActivator : public context_gamma_planner::constraints::ConstraintActivatorBase +{ +public: + ConstraintActivator( + const std::shared_ptr hd_map_utils_ptr, + const std::shared_ptr traffic_lights_ptr); + /// @param stop_velocity_threshold [m/s] Set a small value since a complete stop is not possible. + void appendStopLineConstraint( + const lanelet::Ids & route_ids, + const std::shared_ptr reference_trajectory, + const std::shared_ptr & entity_status, + const double stop_line_enable_threshold = 4.0, + const double stop_velocity_threshold = 0.1) override; + void appendTrafficLightConstraint(const lanelet::Ids & route_ids) override; + void appendLaneChangeConstraint( + const std::shared_ptr & entity_status, + const traffic_simulator::lane_change::Parameter & lane_change_parameter) override; + void appendRoadEndConstraint(const lanelet::Ids & route_ids) override; +}; +} // namespace constraints +} // namespace vehicle +} // namespace context_gamma_planner + +#endif // CONTEXT_GAMMA_PLANNER__VEHICLE__CONSTRAINTS__CONSTRAINT_ACTIVATOR_HPP_ diff --git a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/mock/catalogs.hpp b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/mock/catalogs.hpp new file mode 100644 index 00000000000..ad27141c7f6 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/mock/catalogs.hpp @@ -0,0 +1,97 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTEXT_GAMMA_PLANNER__MOCK__CATALOGS_HPP_ +#define CONTEXT_GAMMA_PLANNER__MOCK__CATALOGS_HPP_ + +#include +#include +#include +#include + +namespace context_gamma_planner +{ +namespace mock +{ +class Catalogs +{ +public: + /** + * @brief Get the default vehicle parameters. + * @return traffic_simulator_msgs::msg::VehicleParameters The vehicle parameters. + */ + auto getVehicleParameters() const -> traffic_simulator_msgs::msg::VehicleParameters + { + traffic_simulator_msgs::msg::VehicleParameters parameters; + parameters.name = "vehicle.volkswagen.t"; + parameters.subtype.value = traffic_simulator_msgs::msg::EntitySubtype::CAR; + parameters.performance.max_speed = 69.444; + parameters.performance.max_acceleration = 200; + parameters.performance.max_deceleration = 10.0; + parameters.bounding_box.center.x = 1.5; + parameters.bounding_box.center.y = 0.0; + parameters.bounding_box.center.z = 0.9; + parameters.bounding_box.dimensions.x = 4.5; + parameters.bounding_box.dimensions.y = 1.7; + parameters.bounding_box.dimensions.z = 1.8; + parameters.axles.front_axle.max_steering = 0.5; + parameters.axles.front_axle.wheel_diameter = 0.6; + parameters.axles.front_axle.track_width = 1.8; + parameters.axles.front_axle.position_x = 3.1; + parameters.axles.front_axle.position_z = 0.3; + parameters.axles.rear_axle.max_steering = 0.0; + parameters.axles.rear_axle.wheel_diameter = 0.6; + parameters.axles.rear_axle.track_width = 1.8; + parameters.axles.rear_axle.position_x = 0.0; + parameters.axles.rear_axle.position_z = 0.3; + return parameters; + } + + /** + * @brief Get the default pedestrian parameters. + * @return traffic_simulator_msgs::msg::PedestrianParameters The pedestrian parameters. + */ + auto getPedestrianParameters() const -> traffic_simulator_msgs::msg::PedestrianParameters + { + traffic_simulator_msgs::msg::PedestrianParameters parameters; + parameters.name = "pedestrian"; + parameters.subtype.value = traffic_simulator_msgs::msg::EntitySubtype::PEDESTRIAN; + parameters.bounding_box.center.x = 0.0; + parameters.bounding_box.center.y = 0.0; + parameters.bounding_box.center.z = 0.5; + parameters.bounding_box.dimensions.x = 1.0; + parameters.bounding_box.dimensions.y = 1.0; + parameters.bounding_box.dimensions.z = 2.0; + return parameters; + } + + /** + * @brief Get the default parameters of a miscellaneous object. + * @return traffic_simulator_msgs::msg::MiscObjectParameters The parameters of the miscellaneous object. + */ + auto getMiscObjectParameters() const -> traffic_simulator_msgs::msg::MiscObjectParameters + { + traffic_simulator_msgs::msg::MiscObjectParameters misc_object_param; + misc_object_param.bounding_box.dimensions.x = 1.0; + misc_object_param.bounding_box.dimensions.y = 1.0; + misc_object_param.bounding_box.dimensions.z = 1.0; + misc_object_param.subtype.value = traffic_simulator_msgs::msg::EntitySubtype::UNKNOWN; + misc_object_param.name = "obstacle"; + return misc_object_param; + } +}; +} // namespace mock +} // namespace context_gamma_planner + +#endif // CONTEXT_GAMMA_PLANNER__MOCK__CATALOGS_HPP_ \ No newline at end of file diff --git a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/pedestrian_plugin.hpp b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/pedestrian_plugin.hpp new file mode 100644 index 00000000000..d40cefc98e3 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/pedestrian_plugin.hpp @@ -0,0 +1,130 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTEXT_GAMMA_PLANNER__PEDESTRIAN_PLUGIN_BASE_HPP_ +#define CONTEXT_GAMMA_PLANNER__PEDESTRIAN_PLUGIN_BASE_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace context_gamma_planner +{ +class PedestrianPlugin : public entity_behavior::BehaviorPluginBase +{ +public: + void configure(const rclcpp::Logger & logger) override; + void update(double current_time, double step_time) override; + const std::string & getCurrentAction() const override; +#define DEFINE_GETTER_SETTER(NAME, TYPE) \ + TYPE get##NAME() override \ + { \ + TYPE value; \ + try { \ + value = tree_.rootBlackboard()->get(get##NAME##Key()); \ + } catch (const std::runtime_error & e) { \ + THROW_SIMULATION_ERROR("the value : ", get##NAME##Key(), " is empty.\n", e.what()); \ + } \ + return value; \ + } \ + void set##NAME(const TYPE & value) override \ + { \ + tree_.rootBlackboard()->set(get##NAME##Key(), value); \ + } + + // clang-format off + DEFINE_GETTER_SETTER(BehaviorParameter, traffic_simulator_msgs::msg::BehaviorParameter) + DEFINE_GETTER_SETTER(CanonicalizedEntityStatus, std::shared_ptr) + DEFINE_GETTER_SETTER(CurrentTime, double) + DEFINE_GETTER_SETTER(DebugMarker, std::vector) + DEFINE_GETTER_SETTER(DefaultMatchingDistanceForLaneletPoseCalculation, double) + DEFINE_GETTER_SETTER(PolylineTrajectory, std::shared_ptr) + DEFINE_GETTER_SETTER(HdMapUtils, std::shared_ptr) + DEFINE_GETTER_SETTER(LaneChangeParameters, traffic_simulator::lane_change::Parameter) + DEFINE_GETTER_SETTER(Obstacle, std::optional) + DEFINE_GETTER_SETTER(OtherEntityStatus, entity_behavior::EntityStatusDict) + DEFINE_GETTER_SETTER(PedestrianParameters, traffic_simulator_msgs::msg::PedestrianParameters) + DEFINE_GETTER_SETTER(Request, traffic_simulator::behavior::Request) + DEFINE_GETTER_SETTER(RouteLanelets, std::vector) + DEFINE_GETTER_SETTER(ReferenceTrajectory, std::shared_ptr) + DEFINE_GETTER_SETTER(StepTime, double) + DEFINE_GETTER_SETTER(TargetSpeed, std::optional) + DEFINE_GETTER_SETTER(TrafficLights, std::shared_ptr) + DEFINE_GETTER_SETTER(VehicleParameters, traffic_simulator_msgs::msg::VehicleParameters) + DEFINE_GETTER_SETTER(Waypoints, traffic_simulator_msgs::msg::WaypointsArray) + DEFINE_GETTER_SETTER(GoalPoses, std::vector) + // clang-format on + +#undef DEFINE_GETTER_SETTER + +// The blackboard value not defined in entity_behavior::BehaviorPluginBase is defined below. +#define DEFINE_GETTER_SETTER(NAME, KEY, TYPE) \ + auto get##NAME##Key() const -> const std::string & \ + { \ + static const std::string key = KEY; \ + return key; \ + } \ + TYPE get##NAME() \ + { \ + TYPE value; \ + try { \ + value = tree_.rootBlackboard()->get(get##NAME##Key()); \ + } catch (const std::runtime_error & e) { \ + THROW_SIMULATION_ERROR("the value : ", get##NAME##Key(), " is empty.\n", e.what()); \ + } \ + return value; \ + } \ + void set##NAME(const TYPE & value) { tree_.rootBlackboard()->set(get##NAME##Key(), value); } + // clang-format off + DEFINE_GETTER_SETTER(ConstraintActivator, "activator", std::shared_ptr) + DEFINE_GETTER_SETTER(NextGoal, "next_goal", geometry_msgs::msg::Point) + DEFINE_GETTER_SETTER(PlanningSpeed, "planning_speed", std::optional) + // clang-format on +#undef DEFINE_GETTER_SETTER + +private: + std::shared_ptr + activator_ptr_; + BT::NodeStatus tickOnce(double current_time, double step_time); + BT::BehaviorTreeFactory factory_; + BT::Tree tree_; + auto createBehaviorTree(const std::string & format_path) -> BT::Tree; + std::shared_ptr logging_event_ptr_; + std::shared_ptr reset_request_event_ptr_; + RVO::RVOSimulator rvo_simulator_; + std::shared_ptr rvo_ego_; + RVO::VisualizeMarker rvo_visualize_; + + void tryInitializeConstraintActivator(); + void tryInitializeEgoInRVO(); + void reflectEgoInRVO(const traffic_simulator::EntityStatus & ego_status); + void reflectNonEgoEntitiesInRVO(); + void updateNonEgoEntityInRVO( + std::shared_ptr agent, + const traffic_simulator::entity_status::CanonicalizedEntityStatus & entity_status); + void createNonEgoEntityInRVO( + const traffic_simulator::entity_status::CanonicalizedEntityStatus & entity_status); + void updateRVO(double step_time); + void updateSimulatorStatus(); + void visualize(); +}; +} // namespace context_gamma_planner + +#endif // CONTEXT_GAMMA_PLANNER__PEDESTRIAN_PLUGIN_BASE_HPP_ \ No newline at end of file diff --git a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/planner/follow_polyline_trajectory_planner_base.hpp b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/planner/follow_polyline_trajectory_planner_base.hpp new file mode 100644 index 00000000000..5a916c94559 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/planner/follow_polyline_trajectory_planner_base.hpp @@ -0,0 +1,55 @@ +// Copyright 2015-2020 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTEXT_GAMMA_PLANNER__FOLLOW_POLYLINE_TRAJECTORY_PLANNER_BASE_HPP_ +#define CONTEXT_GAMMA_PLANNER__FOLLOW_POLYLINE_TRAJECTORY_PLANNER_BASE_HPP_ + +#include +#include + +namespace context_gamma_planner +{ +class FollowPolylineTrajectoryPlannerBase : public GoalPlannerBase +{ +public: + FollowPolylineTrajectoryPlannerBase(const double goal_threshold); + /** + * @brief Sets the waypoints for the planner. + * @param trajectory The polyline trajectory containing the waypoints. + */ + void setWaypoints( + const std::shared_ptr trajectory); + /** + * @brief the next goal point for the vehicle. + * @return An optional containing the next goal point as a geometry_msgs::msg::Point, or an empty optional if the calculation fails. + */ + auto calculateNextGoalPoint() -> std::optional override; + /** + * @brief Get the target speed at a given time. + * @param current_time The current time. + * @return [m/s] The target speed at the given time. + */ + double getTargetSpeed(const double current_time) const; + /** + * @brief Clears the goal planner. + */ + void clear() override; + +private: + size_t getCurrentTrajectoryWaypointIndex() const; + std::shared_ptr trajectory_; +}; +} // namespace context_gamma_planner + +#endif // CONTEXT_GAMMA_PLANNE__FOLLOW_POLYLINE_TRAJECTORY_PLANNER_HPP_ diff --git a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/planner/goal_planner_base.hpp b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/planner/goal_planner_base.hpp new file mode 100644 index 00000000000..b96f0414f35 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/planner/goal_planner_base.hpp @@ -0,0 +1,95 @@ +// Copyright 2015-2020 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTEXT_GAMMA_PLANNER__GOAL_PLANNER_BASE_HPP_ +#define CONTEXT_GAMMA_PLANNER__GOAL_PLANNER_BASE_HPP_ + +#include +#include +#include +#include +#include +#include + +namespace context_gamma_planner +{ +class GoalPlannerBase +{ +public: + GoalPlannerBase(const double goal_threshold); + /** + * @brief Sets the entity status + * @param status entity status + */ + void setCurrentStatus( + const std::shared_ptr & status); + /** + * @brief Appends a vector of goal poses to the existing goal poses. + * @param goal_poses The vector of goal poses to be appended. + */ + void appendGoalPoses(const std::vector & goal_poses); + /** + * @brief Appends a vector of goal points to the existing goal points. + * @param goal_points The vector of goal points to be appended. + */ + void appendGoalPoints(const std::vector & goal_points); + /** + * @brief Sets the maximum speed. + * @param max_speed [m/s] The maximum speed. + */ + void setMaxSpeed(const double & max_speed); + /** + * @brief Get the maximum speed. + * @return [m/s] The maximum speed. + */ + double getMaxSpeed() const { return max_speed_; } + /** + * @brief Get the goal poses. + * @return std::vector The goal poses. + */ + std::vector getGoalPoses() const; + /** + * @brief Get the waypoints. + * @return traffic_simulator_msgs::msg::WaypointsArray The waypoints. + */ + traffic_simulator_msgs::msg::WaypointsArray getWaypoints() const; + /** + * @brief Get the next goal pose. + * @return geometry_msgs::msg::Pose The next goal pose. + */ + geometry_msgs::msg::Pose getNextGoalPose() const { return goal_poses_.front(); } + /** + * @brief Checks if the goal has been reached. + * @return true if the goal has been reached, false otherwise. + */ + bool isReachedGoal() const { return goal_poses_.empty(); } + /** + * @brief Calculates the next goal point from a point on the waypoint and returns a value. + * @return The next goal point. + */ + virtual auto calculateNextGoalPoint() -> std::optional = 0; + /** + * @brief Clears the goal planner. + */ + virtual void clear() = 0; + const double goal_threshold; + +protected: + std::optional status_; + std::deque goal_poses_; + double max_speed_ = 0.0; +}; +} // namespace context_gamma_planner + +#endif // CONTEXT_GAMMA_PLANNER__GOAL_PLANNER_BASE_HPP_ \ No newline at end of file diff --git a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/planner/pedestrian/follow_lane_planner.hpp b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/planner/pedestrian/follow_lane_planner.hpp new file mode 100644 index 00000000000..f3a28d7540f --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/planner/pedestrian/follow_lane_planner.hpp @@ -0,0 +1,44 @@ +// Copyright 2015-2020 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTEXT_GAMMA_PLANNER__PEDESTRIAN__FOLLOW_LANE_PLANNER_HPP_ +#define CONTEXT_GAMMA_PLANNER__PEDESTRIAN__FOLLOW_LANE_PLANNER_HPP_ + +#include + +namespace context_gamma_planner +{ +namespace pedestrian +{ +class FollowLanePlanner : public GoalPlannerBase +{ +public: + FollowLanePlanner(const double goal_threshold); + void setWaypoints( + const std::shared_ptr hdmap_utils, + const std::vector & route_ids); + /** + * @brief the next goal point for the vehicle. + * @return An optional containing the next goal point as a geometry_msgs::msg::Point, or an empty optional if the calculation fails. + */ + auto calculateNextGoalPoint() -> std::optional override; + /** + * @brief Clears the goal planner. + */ + void clear() override; +}; +} // namespace pedestrian +} // namespace context_gamma_planner + +#endif // CONTEXT_GAMMA_PLANNER__PEDESTRIAN__FOLLOW_LANE_PLANNER_HPP_ \ No newline at end of file diff --git a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/planner/pedestrian/follow_polyline_trajectory_planner.hpp b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/planner/pedestrian/follow_polyline_trajectory_planner.hpp new file mode 100644 index 00000000000..04331cc5c62 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/planner/pedestrian/follow_polyline_trajectory_planner.hpp @@ -0,0 +1,33 @@ +// Copyright 2015-2020 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTEXT_GAMMA_PLANNER__PEDESTRIAN__FOLLOW_POLYLINE_TRAJECTORY_PLANNER_HPP_ +#define CONTEXT_GAMMA_PLANNER__PEDESTRIAN__FOLLOW_POLYLINE_TRAJECTORY_PLANNER_HPP_ + +#include +#include + +namespace context_gamma_planner +{ +namespace pedestrian +{ +class FollowPolylineTrajectoryPlanner : public FollowPolylineTrajectoryPlannerBase +{ +public: + FollowPolylineTrajectoryPlanner(const double goal_threshold); +}; +} // namespace pedestrian +} // namespace context_gamma_planner + +#endif // CONTEXT_GAMMA_PLANNER__PEDESTRIAN__FOLLOW_POLYLINE_TRAJECTORY_PLANNER_HPP_ diff --git a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/planner/vehicle/follow_lane_planner.hpp b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/planner/vehicle/follow_lane_planner.hpp new file mode 100644 index 00000000000..ecfd1c5a39a --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/planner/vehicle/follow_lane_planner.hpp @@ -0,0 +1,53 @@ +// Copyright 2015-2020 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTEXT_GAMMA_PLANNER__VEHICLE__FOLLOW_LANE_PLANNER_HPP_ +#define CONTEXT_GAMMA_PLANNER__VEHICLE__FOLLOW_LANE_PLANNER_HPP_ + +#include + +namespace context_gamma_planner +{ +namespace vehicle +{ +class FollowLanePlanner : public GoalPlannerBase +{ +public: + FollowLanePlanner(const double goal_threshold); + /** + * @brief Sets the waypoints for the follow lane planner. + * @param hdmap_utils A shared pointer to the HD map utils object. + * @param route_ids The vector of route IDs. + */ + void setWaypoints( + const std::shared_ptr hdmap_utils, + const std::vector & route_ids); + /** + * @brief the next goal point for the vehicle. + * @return An optional containing the next goal point as a geometry_msgs::msg::Point, or an empty optional if the calculation fails. + */ + auto calculateNextGoalPoint() -> std::optional override; + /** + * @brief Clears the goal planner. + */ + void clear() override; + +private: + /// @todo Rename to more clear variable name. + /// std::optional pre_route_id_ = std::nullopt; +}; +} // namespace vehicle +} // namespace context_gamma_planner + +#endif // CONTEXT_GAMMA_PLANNER__VEHICLE__FOLLOW_LANE_PLANNER_HPP_ \ No newline at end of file diff --git a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/planner/vehicle/follow_polyline_trajectory_planner.hpp b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/planner/vehicle/follow_polyline_trajectory_planner.hpp new file mode 100644 index 00000000000..2c6328f035e --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/planner/vehicle/follow_polyline_trajectory_planner.hpp @@ -0,0 +1,33 @@ +// Copyright 2015-2020 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTEXT_GAMMA_PLANNER__VEHICLE__FOLLOW_POLYLINE_TRAJECTORY_PLANNER_HPP_ +#define CONTEXT_GAMMA_PLANNER__VEHICLE__FOLLOW_POLYLINE_TRAJECTORY_PLANNER_HPP_ + +#include +#include + +namespace context_gamma_planner +{ +namespace vehicle +{ +class FollowPolylineTrajectoryPlanner : public FollowPolylineTrajectoryPlannerBase +{ +public: + FollowPolylineTrajectoryPlanner(const double goal_threshold); +}; +} // namespace vehicle +} // namespace context_gamma_planner + +#endif // CONTEXT_GAMMA_PLANNER__VEHICLE__FOLLOW_POLYLINE_TRAJECTORY_PLANNER_HPP_ \ No newline at end of file diff --git a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/planner/vehicle/lane_change_planner.hpp b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/planner/vehicle/lane_change_planner.hpp new file mode 100644 index 00000000000..78672f03c12 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/planner/vehicle/lane_change_planner.hpp @@ -0,0 +1,60 @@ +// Copyright 2015-2020 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTEXT_GAMMA_PLANNER__VEHICLE__LANE_CHANGE_PLANNER_HPP_ +#define CONTEXT_GAMMA_PLANNER__VEHICLE__LANE_CHANGE_PLANNER_HPP_ + +#include + +namespace context_gamma_planner +{ +namespace vehicle +{ +class LaneChangePlanner : public GoalPlannerBase +{ +public: + LaneChangePlanner(const double goal_threshold); + /** + * @brief Sets the waypoints for the lane change planner. + * @param hdmap_utils A shared pointer to the HdMapUtils object. + * @param route_ids The IDs of the route waypoints. + * @param lane_change_parameter The parameter for the lane change. + */ + void setWaypoints( + const std::shared_ptr hdmap_utils, + const std::vector & route_ids, + const traffic_simulator::lane_change::Parameter & lane_change_parameter); + /** + * @brief the next goal point for the vehicle. + * @return An optional containing the next goal point as a geometry_msgs::msg::Point, or an empty optional if the calculation fails. + */ + auto calculateNextGoalPoint() -> std::optional override; + /** + * @brief Get the target speed at a given time. + * @param current_time The current time. + * @return [m/s] The target speed at the given time. + */ + double getTargetSpeed() const { return target_speed_; } + /** + * @brief Clears the goal planner. + */ + void clear() override; + +private: + double target_speed_ = 0.0; +}; +} // namespace vehicle +} // namespace context_gamma_planner + +#endif // CONTEXT_GAMMA_PLANNER__VEHICLE__LANE_CHANGE_PLANNER_HPP_ \ No newline at end of file diff --git a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/transition_events/logging_event.hpp b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/transition_events/logging_event.hpp new file mode 100644 index 00000000000..a22ef681531 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/transition_events/logging_event.hpp @@ -0,0 +1,39 @@ +// Copyright 2015-2020 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef context_gamma_planner__TRANSITION_EVENTS__LOGGING_EVENT_HPP_ +#define context_gamma_planner__TRANSITION_EVENTS__LOGGING_EVENT_HPP_ + +#include +#include +#include + +namespace context_gamma_planner +{ +class LoggingEvent : public TransitionEvent +{ +public: + LoggingEvent(BT::TreeNode * root_node, const rclcpp::Logger & logger); + const std::string & getCurrentAction() const; + +private: + void callback( + BT::Duration timestamp, const BT::TreeNode & node, BT::NodeStatus prev_status, + BT::NodeStatus status) override; + rclcpp::Logger ros_logger_; + std::string current_action_; +}; +} // namespace context_gamma_planner + +#endif // context_gamma_planner__TRANSITION_EVENTS__LOGGING_EVENT_HPP_ diff --git a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/transition_events/reset_request_event.hpp b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/transition_events/reset_request_event.hpp new file mode 100644 index 00000000000..10b7d34725a --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/transition_events/reset_request_event.hpp @@ -0,0 +1,44 @@ +// Copyright 2015-2020 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef context_gamma_planner__TRANSITION_EVENTS__RESET_REQUEST_EVENT_HPP_ +#define context_gamma_planner__TRANSITION_EVENTS__RESET_REQUEST_EVENT_HPP_ + +#include +#include +#include +#include + +namespace context_gamma_planner +{ +class ResetRequestEvent : public TransitionEvent +{ +public: + ResetRequestEvent( + BT::TreeNode * root_node, + std::function get_request_function, + std::function set_request_function); + const std::string & getCurrentAction() const; + +private: + void callback( + BT::Duration timestamp, const BT::TreeNode & node, BT::NodeStatus prev_status, + BT::NodeStatus status) override; + //const std::shared_ptr root_node_; + std::function get_request_function_; + std::function set_request_function_; +}; +} // namespace context_gamma_planner + +#endif // context_gamma_planner__TRANSITION_EVENTS__RESET_REQUEST_EVENT_HPP_ diff --git a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/transition_events/transition_event.hpp b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/transition_events/transition_event.hpp new file mode 100644 index 00000000000..0bd8fb15cc8 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/transition_events/transition_event.hpp @@ -0,0 +1,44 @@ +// Copyright 2015-2020 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef context_gamma_planner__TRANSITION_EVENTS__TRANSITION_EVENT_HPP_ +#define context_gamma_planner__TRANSITION_EVENTS__TRANSITION_EVENT_HPP_ + +#include + +#include +#include +#include + +namespace context_gamma_planner +{ +class TransitionEvent +{ +public: + TransitionEvent(BT::TreeNode * root_node); + +protected: + virtual void callback( + BT::Duration timestamp, const BT::TreeNode & node, BT::NodeStatus prev_status, + BT::NodeStatus status) = 0; + void updateCurrentAction(const BT::NodeStatus & status, const BT::TreeNode & node); + //std::shared_ptr root_node_; + BT::TimePoint first_timestamp_; + std::vector subscribers_; + BT::TimestampType type_; + std::string current_action_; +}; +} // namespace context_gamma_planner + +#endif // context_gamma_planner__TRANSITION_EVENTS__TRANSITION_EVENT_HPP_ diff --git a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/transition_events/transition_events.hpp b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/transition_events/transition_events.hpp new file mode 100644 index 00000000000..203ce7d453e --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/transition_events/transition_events.hpp @@ -0,0 +1,21 @@ +// Copyright 2015-2020 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef context_gamma_planner__TRANSITION_EVENTS__TRANSITION_EVENTS_HPP_ +#define context_gamma_planner__TRANSITION_EVENTS__TRANSITION_EVENTS_HPP_ + +#include "logging_event.hpp" +#include "reset_request_event.hpp" + +#endif // context_gamma_planner__TRANSITION_EVENTS__TRANSITION_EVENTS_HPP_ diff --git a/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/vehicle_plugin.hpp b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/vehicle_plugin.hpp new file mode 100644 index 00000000000..82c71dc5afc --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/include/context_gamma_planner/vehicle_plugin.hpp @@ -0,0 +1,128 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTEXT_GAMMA_PLANNER__VEHICLE_PLUGIN_BASE_HPP_ +#define CONTEXT_GAMMA_PLANNER__VEHICLE_PLUGIN_BASE_HPP_ + +#include +#include + +#include +#include +#include +#include +#include + +namespace context_gamma_planner +{ +class VehiclePlugin : public entity_behavior::BehaviorPluginBase +{ +public: + void configure(const rclcpp::Logger & logger) override; + void update(double current_time, double step_time) override; + const std::string & getCurrentAction() const override; +#define DEFINE_GETTER_SETTER(NAME, TYPE) \ + TYPE get##NAME() override \ + { \ + TYPE value; \ + try { \ + value = tree_.rootBlackboard()->get(get##NAME##Key()); \ + } catch (const std::runtime_error & e) { \ + THROW_SIMULATION_ERROR("the value : ", get##NAME##Key(), " is empty.\n", e.what()); \ + } \ + return value; \ + } \ + void set##NAME(const TYPE & value) override \ + { \ + tree_.rootBlackboard()->set(get##NAME##Key(), value); \ + } + + // clang-format off + DEFINE_GETTER_SETTER(BehaviorParameter, traffic_simulator_msgs::msg::BehaviorParameter) + DEFINE_GETTER_SETTER(CanonicalizedEntityStatus, std::shared_ptr) + DEFINE_GETTER_SETTER(CurrentTime, double) + DEFINE_GETTER_SETTER(DebugMarker, std::vector) + DEFINE_GETTER_SETTER(DefaultMatchingDistanceForLaneletPoseCalculation, double) + DEFINE_GETTER_SETTER(PolylineTrajectory, std::shared_ptr) + DEFINE_GETTER_SETTER(HdMapUtils, std::shared_ptr) + DEFINE_GETTER_SETTER(LaneChangeParameters, traffic_simulator::lane_change::Parameter) + DEFINE_GETTER_SETTER(Obstacle, std::optional) + DEFINE_GETTER_SETTER(OtherEntityStatus, entity_behavior::EntityStatusDict) + DEFINE_GETTER_SETTER(PedestrianParameters, traffic_simulator_msgs::msg::PedestrianParameters) + DEFINE_GETTER_SETTER(Request, traffic_simulator::behavior::Request) + DEFINE_GETTER_SETTER(RouteLanelets, std::vector) + DEFINE_GETTER_SETTER(ReferenceTrajectory, std::shared_ptr) + DEFINE_GETTER_SETTER(StepTime, double) + DEFINE_GETTER_SETTER(TargetSpeed, std::optional) + DEFINE_GETTER_SETTER(TrafficLights, std::shared_ptr) + DEFINE_GETTER_SETTER(VehicleParameters, traffic_simulator_msgs::msg::VehicleParameters) + DEFINE_GETTER_SETTER(Waypoints, traffic_simulator_msgs::msg::WaypointsArray) + DEFINE_GETTER_SETTER(GoalPoses, std::vector) + // clang-format on + +#undef DEFINE_GETTER_SETTER + +// The blackboard value not defined in entity_behavior::BehaviorPluginBase is defined below. +#define DEFINE_GETTER_SETTER(NAME, KEY, TYPE) \ + auto get##NAME##Key() const -> const std::string & \ + { \ + static const std::string key = KEY; \ + return key; \ + } \ + TYPE get##NAME() \ + { \ + TYPE value; \ + try { \ + value = tree_.rootBlackboard()->get(get##NAME##Key()); \ + } catch (const std::runtime_error & e) { \ + THROW_SIMULATION_ERROR("the value : ", get##NAME##Key(), " is empty.\n", e.what()); \ + } \ + return value; \ + } \ + void set##NAME(const TYPE & value) { tree_.rootBlackboard()->set(get##NAME##Key(), value); } + // clang-format off + DEFINE_GETTER_SETTER(ConstraintActivator, "activator", std::shared_ptr) + DEFINE_GETTER_SETTER(NextGoal, "next_goal", geometry_msgs::msg::Point) + DEFINE_GETTER_SETTER(PlanningSpeed, "planning_speed", std::optional) + // clang-format on +#undef DEFINE_GETTER_SETTER + +private: + std::shared_ptr activator_ptr_; + BT::NodeStatus tickOnce(double current_time, double step_time); + BT::BehaviorTreeFactory factory_; + BT::Tree tree_; + auto createBehaviorTree(const std::string & format_path) -> BT::Tree; + std::shared_ptr logging_event_ptr_; + std::shared_ptr reset_request_event_ptr_; + RVO::RVOSimulator rvo_simulator_; + std::shared_ptr rvo_ego_; + RVO::VisualizeMarker rvo_visualize_; + + void tryInitializeConstraintActivator(); + void tryInitializeEgoInRVO(); + void reflectEgoInRVO(const traffic_simulator::EntityStatus & ego_status); + void reflectNonEgoEntitiesInRVO(); + void updateNonEgoEntityInRVO( + std::shared_ptr agent, + const traffic_simulator::entity_status::CanonicalizedEntityStatus & entity_status); + void createNonEgoEntityInRVO( + const traffic_simulator::entity_status::CanonicalizedEntityStatus & entity_status); + void updateRVO(double step_time); + void updateSimulatorStatus(); + void visualize(); +}; +} // namespace context_gamma_planner + +#endif // CONTEXT_GAMMA_PLANNER__VEHICLE_PLUGIN_BASE_HPP_ \ No newline at end of file diff --git a/simulation/context_gamma_planner/context_gamma_planner/launch/test.launch.py b/simulation/context_gamma_planner/context_gamma_planner/launch/test.launch.py new file mode 100644 index 00000000000..3d0a452722f --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/launch/test.launch.py @@ -0,0 +1,163 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +"""Launch description for scenario runner moc.""" + +# Copyright (c) 2022 Tier IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from logging import shutdown +import os +import sys + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +import launch + +from launch.conditions import IfCondition +from launch.events import Shutdown +from launch.event_handlers import OnProcessExit, OnProcessIO + +from launch.actions import EmitEvent, RegisterEventHandler, LogInfo, TimerAction +from launch.actions.declare_launch_argument import DeclareLaunchArgument +from launch.substitutions.launch_configuration import LaunchConfiguration + +from launch_ros.actions import Node +from launch.actions import ExecuteProcess + +from typing import cast + +from pathlib import Path + + +class Color: + BLACK = "\033[30m" + RED = "\033[31m" + GREEN = "\033[32m" + YELLOW = "\033[33m" + BLUE = "\033[34m" + PURPLE = "\033[35m" + CYAN = "\033[36m" + WHITE = "\033[37m" + END = "\033[0m" + BOLD = "\038[1m" + UNDERLINE = "\033[4m" + INVISIBLE = "\033[08m" + REVERSE = "\033[07m" + + +def on_stderr_output(event: launch.Event) -> None: + lines = event.text.decode().splitlines() + if len(lines) == 1: + if lines[0] == "cpp_scenario:failure": + print(Color.RED + "Scenario Failed" + Color.END) + + +def on_stdout_output(event: launch.Event) -> None: + lines = event.text.decode().splitlines() + if len(lines) == 1: + if lines[0] == "cpp_scenario:success": + print(Color.GREEN + "Scenario Succeed" + Color.END) + + +def generate_launch_description(): + timeout = LaunchConfiguration("timeout", default=180.0) + scenario = LaunchConfiguration("scenario", default="spawn") + scenario_package = LaunchConfiguration("package", default="context_gamma_planner") + junit_path = LaunchConfiguration("junit_path", default="/tmp/output.xunit.xml") + launch_rviz = LaunchConfiguration("launch_rviz", default=True) + record = LaunchConfiguration("record", default=False) + scenario_node = Node( + package=scenario_package, + executable=scenario, + name=scenario, + output="screen", + arguments=[("__log_level:=info")], + parameters=[{"junit_path": junit_path, "timeout": timeout}], + ) + io_handler = OnProcessIO( + target_action=scenario_node, + on_stderr=on_stderr_output, + on_stdout=on_stdout_output, + ) + shutdown_handler = OnProcessExit( + target_action=scenario_node, on_exit=[EmitEvent(event=Shutdown())] + ) + description = LaunchDescription( + [ + DeclareLaunchArgument( + "scenario", default_value=scenario, description="Name of the scenario." + ), + DeclareLaunchArgument( + "package", + default_value=scenario_package, + description="Name of package your scenario exists", + ), + DeclareLaunchArgument( + "timeout", default_value=timeout, description="Timeout in seconds." + ), + DeclareLaunchArgument( + "junit_path", + default_value=junit_path, + description="Path of the junit output.", + ), + DeclareLaunchArgument( + "launch_rviz", + default_value=launch_rviz, + description="If true, launch with rviz.", + ), + DeclareLaunchArgument( + "record", + default_value=record, + description="If true, record rosbag data.", + ), + scenario_node, + RegisterEventHandler(event_handler=io_handler), + RegisterEventHandler(event_handler=shutdown_handler), + Node( + package="simple_sensor_simulator", + executable="simple_sensor_simulator_node", + name="simple_sensor_simulator_node", + output="log", + arguments=[("__log_level:=warn")], + ), + Node( + package="traffic_simulator", + executable="visualization_node", + name="visualizer", + output="screen", + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output={"stderr": "log", "stdout": "log"}, + condition=IfCondition(launch_rviz), + arguments=[ + "-d", + str( + Path(get_package_share_directory("context_gamma_planner")) + / "config/rviz/mock_test.rviz" + ), + ], + ), + ExecuteProcess( + cmd=['ros2', 'bag', 'record', '-a'], + output='screen', + condition=IfCondition(record), + ) + ] + ) + return description \ No newline at end of file diff --git a/simulation/context_gamma_planner/context_gamma_planner/package.xml b/simulation/context_gamma_planner/context_gamma_planner/package.xml new file mode 100644 index 00000000000..a3c9984d889 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/package.xml @@ -0,0 +1,33 @@ + + + + context_gamma_planner + 0.0.0 + TODO: Package description + masaya + TODO: License declaration + + ament_cmake_auto + ament_cmake_python + + rclcpp + pluginlib + traffic_simulator + behaviortree_cpp_v3 + customized_rvo2 + geometry + libgoogle-glog-dev + python3-xmlschema + rclpy + + ament_lint_auto + ament_cmake_clang_format + ament_cmake_copyright + ament_cmake_lint_cmake + ament_cmake_pep257 + ament_cmake_xmllint + + + ament_cmake + + diff --git a/simulation/context_gamma_planner/context_gamma_planner/plugins.xml b/simulation/context_gamma_planner/context_gamma_planner/plugins.xml new file mode 100644 index 00000000000..1eb5c07da87 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/plugins.xml @@ -0,0 +1,17 @@ + + + + + A context-gamma planner plugin for pedestrian NPC. + + + + + A context-gamma planner plugin for vehicle NPC. + + + diff --git a/simulation/context_gamma_planner/context_gamma_planner/src/behavior/action_node_base.cpp b/simulation/context_gamma_planner/context_gamma_planner/src/behavior/action_node_base.cpp new file mode 100644 index 00000000000..25eca4e728f --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/src/behavior/action_node_base.cpp @@ -0,0 +1,61 @@ +// Copyright 2015-2020 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +namespace context_gamma_planner +{ +ActionNodeBase::ActionNodeBase(const std::string & name, const BT::NodeConfiguration & config) +: BT::ActionNodeBase(name, config) +{ +} +auto ActionNodeBase::executeTick() -> BT::NodeStatus { return BT::ActionNodeBase::executeTick(); } +auto ActionNodeBase::getBlackBoardValues() -> void +{ + if (!getInput("request", request)) { + THROW_SIMULATION_ERROR("failed to get input request in ActionNode"); + } + if (!getInput("step_time", step_time)) { + THROW_SIMULATION_ERROR("failed to get input step_time in ActionNode"); + } + if (!getInput("current_time", current_time)) { + THROW_SIMULATION_ERROR("failed to get input current_time in ActionNode"); + } + + if (!getInput>("hdmap_utils", hdmap_utils)) { + THROW_SIMULATION_ERROR("failed to get input hdmap_utils in ActionNode"); + } + /* + if (!getInput>( + "traffic_light_manager", traffic_light_manager)) { + THROW_SIMULATION_ERROR("failed to get input traffic_light_manager in ActionNode"); + }*/ + if (!getInput>( + "canonicalized_entity_status", entity_status)) { + THROW_SIMULATION_ERROR("failed to get input canonicalized_entity_status in ActionNode"); + } + + if (!getInput>("target_speed", target_speed)) { + target_speed = std::nullopt; + } + + if (!getInput("other_entity_status", other_entity_status)) { + THROW_SIMULATION_ERROR("failed to get input other_entity_status in ActionNode"); + } + if (!getInput>("route_lanelets", route_lanelets)) { + THROW_SIMULATION_ERROR("failed to get input route_lanelets in ActionNode"); + } +} + +} // namespace context_gamma_planner \ No newline at end of file diff --git a/simulation/context_gamma_planner/context_gamma_planner/src/behavior/pedestrian/action_node.cpp b/simulation/context_gamma_planner/context_gamma_planner/src/behavior/pedestrian/action_node.cpp new file mode 100644 index 00000000000..0cf4e06ef33 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/src/behavior/pedestrian/action_node.cpp @@ -0,0 +1,42 @@ +// Copyright 2015-2020 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +namespace context_gamma_planner +{ +namespace pedestrian +{ +ActionNode::ActionNode(const std::string & name, const BT::NodeConfiguration & config) +: ActionNodeBase(name, config) +{ +} + +void ActionNode::getBlackBoardValues() +{ + ActionNodeBase::getBlackBoardValues(); + if (!getInput( + "pedestrian_parameters", pedestrian_parameters)) { + THROW_SIMULATION_ERROR("failed to get input pedestrian_parameters in pedestrian::ActionNode"); + } + getInput("activator", activator); + if (activator == nullptr) { + THROW_SIMULATION_ERROR("constraint activator is nullptr"); + } +} +} // namespace pedestrian +} // namespace context_gamma_planner diff --git a/simulation/context_gamma_planner/context_gamma_planner/src/behavior/pedestrian/follow_lane_action.cpp b/simulation/context_gamma_planner/context_gamma_planner/src/behavior/pedestrian/follow_lane_action.cpp new file mode 100644 index 00000000000..fac356a426d --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/src/behavior/pedestrian/follow_lane_action.cpp @@ -0,0 +1,68 @@ +// Copyright 2015-2020 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +namespace context_gamma_planner +{ +namespace pedestrian +{ +FollowLaneAction::FollowLaneAction(const std::string & name, const BT::NodeConfiguration & config) +: context_gamma_planner::pedestrian::ActionNode(name, config), planner_(3.0) +{ +} + +void FollowLaneAction::getBlackBoardValues() { ActionNode::getBlackBoardValues(); } + +BT::NodeStatus FollowLaneAction::tick() +{ + getBlackBoardValues(); + if ( + request != traffic_simulator::behavior::Request::NONE && + request != traffic_simulator::behavior::Request::FOLLOW_LANE) { + planner_.clear(); + return BT::NodeStatus::FAILURE; + } + planner_.setCurrentStatus(entity_status); + // update max speed + std::optional max_speed = std::nullopt; + if (target_speed) { + planner_.setMaxSpeed(target_speed.value()); + max_speed = planner_.getMaxSpeed(); + } + // update waypoints + planner_.setWaypoints(hdmap_utils, route_lanelets); + if (const auto next_goal = planner_.calculateNextGoalPoint()) { + setOutput("next_goal", next_goal.value()); + } else { + setOutput("next_goal", entity_status->getMapPose().position); + setOutput("waypoints", planner_.getWaypoints()); + setOutput("planning_speed", std::optional(0)); + return BT::NodeStatus::SUCCESS; + } + setOutput("waypoints", planner_.getWaypoints()); + setOutput("planning_speed", max_speed); + + // update constraints + // activator->appendPedestrianRouteConstraint( + // entity_status->getMapPose(), planner_.getNextGoalPose(), route_lanelets, + // planner_.getGoalPoses()); + // update constraints + activator->appendRoadEdgeConstraint(route_lanelets); + activator->appendPreviousRoadEdgeConstraint({entity_status->getLaneletPose().lanelet_id}); + + return BT::NodeStatus::RUNNING; +} +} // namespace pedestrian +} // namespace context_gamma_planner diff --git a/simulation/context_gamma_planner/context_gamma_planner/src/behavior/pedestrian/follow_polyline_trajectory_action.cpp b/simulation/context_gamma_planner/context_gamma_planner/src/behavior/pedestrian/follow_polyline_trajectory_action.cpp new file mode 100644 index 00000000000..832c5374f74 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/src/behavior/pedestrian/follow_polyline_trajectory_action.cpp @@ -0,0 +1,80 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +namespace context_gamma_planner +{ +namespace pedestrian +{ +FollowPolylineTrajectoryAction::FollowPolylineTrajectoryAction( + const std::string & name, const BT::NodeConfiguration & config) +: context_gamma_planner::pedestrian::ActionNode(name, config), + /// @note: The pedestrian has a small value of goal_threshold because of low speed + planner_(1.0) +{ +} + +void FollowPolylineTrajectoryAction::getBlackBoardValues() +{ + ActionNode::getBlackBoardValues(); + if (request == traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY) { + if (!getInput("polyline_trajectory", polyline_trajectory_)) { + THROW_SIMULATION_ERROR("failed to get input polyline_trajectory in ActionNode"); + } + } +} + +BT::NodeStatus FollowPolylineTrajectoryAction::tick() +{ + getBlackBoardValues(); + + if ( + request != traffic_simulator::behavior::Request::NONE && + request != traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY) { + planner_.clear(); + return BT::NodeStatus::FAILURE; + } + if (polyline_trajectory_->shape.vertices.empty()) { + THROW_SIMULATION_ERROR("polyline_trajectory is empty"); + } + planner_.setCurrentStatus(entity_status); + if (target_speed) { + planner_.setMaxSpeed(target_speed.value()); + } + if (planner_.getGoalPoses().empty()) { + planner_.setWaypoints(polyline_trajectory_); + } + if (const auto next_goal = planner_.calculateNextGoalPoint()) { + setOutput("next_goal", next_goal.value()); + } else { + return BT::NodeStatus::FAILURE; + } + setOutput("waypoints", planner_.getWaypoints()); + setOutput("planning_speed", std::optional(planner_.getTargetSpeed(current_time))); + + // update constraints + + if (planner_.isReachedGoal()) { + return BT::NodeStatus::SUCCESS; + } + + return BT::NodeStatus::RUNNING; +} +} // namespace pedestrian +} // namespace context_gamma_planner diff --git a/simulation/context_gamma_planner/context_gamma_planner/src/behavior/vehicle/action_node.cpp b/simulation/context_gamma_planner/context_gamma_planner/src/behavior/vehicle/action_node.cpp new file mode 100644 index 00000000000..ceba11e1286 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/src/behavior/vehicle/action_node.cpp @@ -0,0 +1,52 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include + +namespace context_gamma_planner +{ +namespace vehicle +{ +ActionNode::ActionNode(const std::string & name, const BT::NodeConfiguration & config) +: ActionNodeBase(name, config) +{ +} + +void ActionNode::getBlackBoardValues() +{ + ActionNodeBase::getBlackBoardValues(); + if (!getInput( + "behavior_parameter", behavior_parameter)) { + behavior_parameter = traffic_simulator_msgs::msg::BehaviorParameter(); + } + if (!getInput( + "vehicle_parameters", vehicle_parameters)) { + THROW_SIMULATION_ERROR("failed to get input vehicle_parameters in vehicle::ActionNode"); + } + if (!getInput>( + "reference_trajectory", reference_trajectory)) { + THROW_SIMULATION_ERROR("failed to get input reference_trajectory in vehicle::ActionNode"); + } + getInput("activator", activator); + if (activator == nullptr) { + THROW_SIMULATION_ERROR("constraint activator is nullptr"); + } +} +} // namespace vehicle +} // namespace context_gamma_planner diff --git a/simulation/context_gamma_planner/context_gamma_planner/src/behavior/vehicle/follow_lane_action.cpp b/simulation/context_gamma_planner/context_gamma_planner/src/behavior/vehicle/follow_lane_action.cpp new file mode 100644 index 00000000000..f962ac49cc0 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/src/behavior/vehicle/follow_lane_action.cpp @@ -0,0 +1,68 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +namespace context_gamma_planner +{ +namespace vehicle +{ +FollowLaneAction::FollowLaneAction(const std::string & name, const BT::NodeConfiguration & config) +: context_gamma_planner::vehicle::ActionNode(name, config), planner_(5.0) +{ +} + +void FollowLaneAction::getBlackBoardValues() { ActionNode::getBlackBoardValues(); } + +BT::NodeStatus FollowLaneAction::tick() +{ + getBlackBoardValues(); + if ( + request != traffic_simulator::behavior::Request::NONE && + request != traffic_simulator::behavior::Request::FOLLOW_LANE) { + planner_.clear(); + return BT::NodeStatus::FAILURE; + } + planner_.setCurrentStatus(entity_status); + // update max speed + std::optional max_speed = std::nullopt; + if (target_speed) { + planner_.setMaxSpeed(target_speed.value()); + max_speed = planner_.getMaxSpeed(); + } + // update waypoints + planner_.setWaypoints(hdmap_utils, route_lanelets); + /// @todo Goal threashold should be defined by adaptive. + if (const auto next_goal = planner_.calculateNextGoalPoint()) { + setOutput("next_goal", next_goal.value()); + } else { + return BT::NodeStatus::FAILURE; + } + setOutput("waypoints", planner_.getWaypoints()); + setOutput("planning_speed", max_speed); + + // update constraints + activator->appendRoadEdgeConstraint(route_lanelets); + activator->appendStopLineConstraint(route_lanelets, reference_trajectory, entity_status); + activator->appendTrafficLightConstraint(route_lanelets); + activator->appendRoadEndConstraint(route_lanelets); + activator->appendPreviousRoadEdgeConstraint(route_lanelets); + return BT::NodeStatus::RUNNING; +} +} // namespace vehicle +} // namespace context_gamma_planner diff --git a/simulation/context_gamma_planner/context_gamma_planner/src/behavior/vehicle/follow_polyline_trajectory_action.cpp b/simulation/context_gamma_planner/context_gamma_planner/src/behavior/vehicle/follow_polyline_trajectory_action.cpp new file mode 100644 index 00000000000..435eb9f34d6 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/src/behavior/vehicle/follow_polyline_trajectory_action.cpp @@ -0,0 +1,78 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +namespace context_gamma_planner +{ +namespace vehicle +{ +FollowPolylineTrajectoryAction::FollowPolylineTrajectoryAction( + const std::string & name, const BT::NodeConfiguration & config) +: context_gamma_planner::vehicle::ActionNode(name, config), planner_(5.0) +{ +} + +void FollowPolylineTrajectoryAction::getBlackBoardValues() +{ + ActionNode::getBlackBoardValues(); + if (request == traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY) { + if (!getInput("polyline_trajectory", polyline_trajectory_)) { + THROW_SIMULATION_ERROR("failed to get input polyline_trajectory in ActionNode"); + } + } +} + +BT::NodeStatus FollowPolylineTrajectoryAction::tick() +{ + getBlackBoardValues(); + + if ( + request != traffic_simulator::behavior::Request::NONE && + request != traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY) { + planner_.clear(); + return BT::NodeStatus::FAILURE; + } + if (polyline_trajectory_->shape.vertices.empty()) { + THROW_SIMULATION_ERROR("polyline_trajectory is empty"); + } + planner_.setCurrentStatus(entity_status); + if (target_speed) { + planner_.setMaxSpeed(target_speed.value()); + } + if (planner_.getGoalPoses().empty()) { + planner_.setWaypoints(polyline_trajectory_); + } + if (const auto next_goal = planner_.calculateNextGoalPoint()) { + setOutput("next_goal", next_goal.value()); + } else { + return BT::NodeStatus::FAILURE; + } + setOutput("waypoints", planner_.getWaypoints()); + setOutput("planning_speed", std::optional(planner_.getTargetSpeed(current_time))); + + // update constraints + + if (planner_.isReachedGoal()) { + return BT::NodeStatus::SUCCESS; + } + + return BT::NodeStatus::RUNNING; +} +} // namespace vehicle +} // namespace context_gamma_planner diff --git a/simulation/context_gamma_planner/context_gamma_planner/src/behavior/vehicle/lane_change_action.cpp b/simulation/context_gamma_planner/context_gamma_planner/src/behavior/vehicle/lane_change_action.cpp new file mode 100644 index 00000000000..e74517609b6 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/src/behavior/vehicle/lane_change_action.cpp @@ -0,0 +1,78 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +namespace context_gamma_planner +{ +namespace vehicle +{ +LaneChangeAction::LaneChangeAction(const std::string & name, const BT::NodeConfiguration & config) +: context_gamma_planner::vehicle::ActionNode(name, config), planner_(5.0) +{ +} + +void LaneChangeAction::getBlackBoardValues() +{ + ActionNode::getBlackBoardValues(); + if (request == traffic_simulator::behavior::Request::LANE_CHANGE) { + if (!getInput("lane_change_parameters", lane_change_parameters_)) { + THROW_SIMULATION_ERROR("failed to get input lane_change_parameters in ActionNode"); + } + } +} + +BT::NodeStatus LaneChangeAction::tick() +{ + getBlackBoardValues(); + + if ( + request != traffic_simulator::behavior::Request::NONE && + request != traffic_simulator::behavior::Request::LANE_CHANGE) { + planner_.clear(); + return BT::NodeStatus::FAILURE; + } + + if ( + lane_change_parameters_.constraint.policy == + traffic_simulator::lane_change::Constraint::Policy::FORCE) { + THROW_SIMULATION_ERROR("Force lane change is not supported in context gamma planner"); + } + planner_.setCurrentStatus(entity_status); + if (target_speed) { + planner_.setMaxSpeed(target_speed.value()); + } + if (planner_.getGoalPoses().empty()) { + planner_.setWaypoints(hdmap_utils, route_lanelets, lane_change_parameters_); + } + if (const auto next_goal = planner_.calculateNextGoalPoint()) { + setOutput("next_goal", next_goal.value()); + } else { + return BT::NodeStatus::SUCCESS; + } + setOutput("waypoints", planner_.getWaypoints()); + setOutput("planning_speed", std::optional(planner_.getTargetSpeed())); + // update constraints + activator->appendLaneChangeConstraint(entity_status, lane_change_parameters_); + if (planner_.isReachedGoal()) { + return BT::NodeStatus::SUCCESS; + } + return BT::NodeStatus::RUNNING; +} +} // namespace vehicle +} // namespace context_gamma_planner diff --git a/simulation/context_gamma_planner/context_gamma_planner/src/constraints/constraint_activator_base.cpp b/simulation/context_gamma_planner/context_gamma_planner/src/constraints/constraint_activator_base.cpp new file mode 100644 index 00000000000..5ba89c4fa70 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/src/constraints/constraint_activator_base.cpp @@ -0,0 +1,194 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +namespace context_gamma_planner +{ +namespace constraints +{ +ConstraintActivatorBase::ConstraintActivatorBase( + const std::shared_ptr hd_map_utils_ptr, + const std::shared_ptr traffic_lights_ptr) +: hd_map_utils_ptr_(hd_map_utils_ptr), + traffic_lights_ptr_(traffic_lights_ptr), + is_stoped_(false) +{ + for (const auto & id : hd_map_utils_ptr_->getLaneletIds()) { + lane_constraints_.emplace_back( + RoadEdgeConstraint(hd_map_utils_ptr, id, RoadEdgeConstraint::Side::RIGHT)); + lane_constraints_.emplace_back( + RoadEdgeConstraint(hd_map_utils_ptr, id, RoadEdgeConstraint::Side::LEFT)); + lane_constraints_.emplace_back( + RoadEdgeConstraint(hd_map_utils_ptr, id, RoadEdgeConstraint::Side::FRONT)); + lane_constraints_.emplace_back( + RoadEdgeConstraint(hd_map_utils_ptr, id, RoadEdgeConstraint::Side::BACK)); + } + + //crosswalk inactive + /* + lanelet::Ids crosswalk_conf_ids; + for(const auto id : hd_map_utils_ptr_->getLaneletIds()){ + auto conflict_ids = hd_map_utils_ptr_->getConflictingCrosswalkIds(lanelet::Ids{id}); + if (conflict_ids.size()) { + crosswalk_conf_ids.emplace_back(id); + } + } + appendRoadEdgeConstraint(crosswalk_conf_ids,State::INACTIVE); + lanelet::Ids crosswalk_ids=hd_map_utils_ptr_->filterLaneletIds(hd_map_utils_ptr_->getLaneletIds(),"crosswalk"); + appendRoadEdgeConstraint(crosswalk_ids,State::INACTIVE); + */ +} + +void ConstraintActivatorBase::deactivateAllConstraints() +{ + deactivateAll(lane_constraints_); + deactivateAll(traffic_light_constraints_); + deactivateAll(stop_line_constraints_); +} + +const auto ConstraintActivatorBase::calculateRVOObstacles() + -> std::vector > +{ + obstacles_.clear(); + for (const auto & constraint : filter(lane_constraints_, State::ACTIVE)) { + appendPolygonsToRVOObstacles(constraint.getPolygons()); + } + // std::cout << "Raw : " << static_cast(traffic_light_constraints_.size()) << std::endl; + // std::cout << "Filtered : " << static_cast(filter(traffic_light_constraints_, State::ACTIVE).size()) << std::endl; + for (const auto & constraint : filter(traffic_light_constraints_, State::ACTIVE)) { + appendPolygonsToRVOObstacles(constraint.getPolygons()); + } + for (const auto & constraint : filter(stop_line_constraints_, State::ACTIVE)) { + appendPolygonsToRVOObstacles(constraint.getPolygons()); + } + return obstacles_; +} + +std::vector ConstraintActivatorBase::queryRoadEdgeConstraint( + const geometry_msgs::msg::Point & p, double distance_threashold, const char subtype[]) +{ + const auto ids = hd_map_utils_ptr_->filterLaneletIds( + hd_map_utils_ptr_->getNearbyLaneletIds(p, distance_threashold), subtype); + deactivateAll(lane_constraints_); + setState(lane_constraints_, ids, State::ACTIVE); + return filter(lane_constraints_, State::ACTIVE); +} + +void ConstraintActivatorBase::appendPolygonToRVOObstacles(const Polygon & polygon) +{ + /// @todo Describe hard corded parameter `line_width`. + constexpr float line_width = 0.01; + for (size_t i = 1; i < polygon.size(); i++) { + std::vector obstacle; + RVO::Vector2 point, pre_point; + if (i == 0) { + point = RVO::Vector2(polygon.back().x, polygon.back().y); + pre_point = RVO::Vector2(polygon[i].x, polygon[i].y); + } else { + point = RVO::Vector2(polygon[i].x, polygon[i].y); + pre_point = RVO::Vector2(polygon[i - 1].x, polygon[i - 1].y); + } + const auto p = RVO::normalize(point - pre_point); + const auto rot_p = RVO::Vector2(p.y(), -p.x()); + obstacle.emplace_back(point); + obstacle.emplace_back(pre_point); + obstacle.emplace_back(pre_point + rot_p * line_width); + obstacle.emplace_back(point + rot_p * line_width); + //std::reverse(obstacle.begin(), obstacle.end()); + obstacles_.emplace_back(obstacle); + } +} + +void ConstraintActivatorBase::appendPolygonsToRVOObstacles(const Polygons & polygons) +{ + for (const auto & polygon : polygons) { + appendPolygonToRVOObstacles(polygon); + } +} + +void ConstraintActivatorBase::appendLaneChangeConstraint( + const std::shared_ptr &, + const traffic_simulator::lane_change::Parameter &) +{ + THROW_SIMULATION_ERROR( + "Currently, appendLaneChangeConstraint does not supported in this entity.", + "This message is mainly for developers.", + "If you are not developer, please notify to Masaya Kataoka (@hakuturu583)"); +} + +void ConstraintActivatorBase::appendStopLineConstraint( + const lanelet::Ids &, const std::shared_ptr, + const std::shared_ptr &, const double, const double) +{ + THROW_SIMULATION_ERROR( + "Currently, appendStopLineConstraint does not supported in this entity.", + "This message is mainly for developers.", + "If you are not developer, please notify to Masaya Kataoka (@hakuturu583)"); +} + +void ConstraintActivatorBase::appendTrafficLightConstraint(const lanelet::Ids &) +{ + THROW_SIMULATION_ERROR( + "Currently, appendTrafficLightConstraint does not supported in this entity.", + "This message is mainly for developers.", + "If you are not developer, please notify to Masaya Kataoka (@hakuturu583)"); +} + +void ConstraintActivatorBase::appendPedestrianRouteConstraint( + const geometry_msgs::msg::Pose &, const geometry_msgs::msg::Pose &, const lanelet::Ids &, + const std::vector &) +{ + THROW_SIMULATION_ERROR( + "Currently, appendPedestrianRouteConstraint does not supported in this entity.", + "This message is mainly for developers.", + "If you are not developer, please notify to Masaya Kataoka (@hakuturu583)"); +} + +void ConstraintActivatorBase::appendRoadEndConstraint(const lanelet::Ids &) +{ + THROW_SIMULATION_ERROR( + "Currently, appendRoadEndConstraint does not supported in this entity.", + "This message is mainly for developers.", + "If you are not developer, please notify to Masaya Kataoka (@hakuturu583)"); +} + +void ConstraintActivatorBase::appendRoadEdgeConstraint( + const lanelet::Id lanelet_id, const State & state) +{ + appendRoadEdgeConstraint(lanelet::Ids({lanelet_id}), state); +} + +void ConstraintActivatorBase::appendRoadEdgeConstraint( + const lanelet::Ids & lanelet_ids, const State & state) +{ + setState(lane_constraints_, lanelet_ids, state); +} + +void ConstraintActivatorBase::appendPreviousRoadEdgeConstraint(const lanelet::Ids & lanelet_ids) +{ + if (!hd_map_utils_ptr_->getPreviousLaneletIds(lanelet_ids.front()).empty()) { + appendRoadEdgeConstraint( + hd_map_utils_ptr_->getPreviousLaneletIds(lanelet_ids.front()).front(), State::ACTIVE); + } +} + +void ConstraintActivatorBase::appendRoadEdgeConstraint( + const lanelet::Id lanelet_id, const RoadEdgeConstraint::Side & side, const State & state) +{ + setState(lane_constraints_, lanelet_id, side, state); +} +} // namespace constraints +} // namespace context_gamma_planner diff --git a/simulation/context_gamma_planner/context_gamma_planner/src/constraints/constraint_base.cpp b/simulation/context_gamma_planner/context_gamma_planner/src/constraints/constraint_base.cpp new file mode 100644 index 00000000000..d2b4616b9fe --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/src/constraints/constraint_base.cpp @@ -0,0 +1,95 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +namespace context_gamma_planner +{ +namespace constraints +{ +ConstraintBase::ConstraintBase( + const std::shared_ptr & hdmap_utils, const std::string & type, + const Polygons & polygons) +: type(type), hdmap_utils_ptr_(hdmap_utils), polygons_(polygons) +{ + state_ = State::INACTIVE; +} + +ConstraintBase::ConstraintBase(const ConstraintBase & obj) +: type(obj.type), + hdmap_utils_ptr_(obj.hdmap_utils_ptr_), + polygons_(obj.getPolygons()), + state_(obj.state_) +{ +} + +std::ostream & operator<<(std::ostream & os, const Polygon & polygon) +{ + os << "================= polygon =================" << std::endl; + size_t index = 0; + for (const auto & point : polygon) { + os << "point[" << index << "], (x,y,z) = (" << point.x << "," << point.y << "," << point.z + << ")" << std::endl; + index++; + } + os << "================= polygon =================" << std::endl; + return os; +} + +std::ostream & operator<<(std::ostream & os, const Polygons & polygons) +{ + size_t index = 0; + for (const auto & polygon : polygons) { + os << "polygon [" << index << "]" << std::endl; + os << polygon; + index++; + } + return os; +} + +const Polygons & ConstraintBase::getPolygons() const { return polygons_; } + +ConstraintBase ConstraintBase::operator+(ConstraintBase constraint) +{ + if (state_ == State::ACTIVE && constraint.getState() == State::ACTIVE) { + std::copy( + constraint.polygons_.begin(), constraint.polygons_.end(), + std::back_inserter(this->polygons_)); + } else if (constraint.getState() == State::ACTIVE) { + return ConstraintBase(this->hdmap_utils_ptr_, OVERRAY_CONSTRAINT, constraint.getPolygons()); + } + return ConstraintBase(this->hdmap_utils_ptr_, OVERRAY_CONSTRAINT, this->polygons_); +} + +ConstraintBase ConstraintBase::operator=(ConstraintBase constraint) { return constraint; } + +void ConstraintBase::setState(const State & state) { state_ = state; } + +const State & ConstraintBase::getState() const { return state_; } + +std::ostream & operator<<(std::ostream & os, const ConstraintBase & constraint) +{ + os << "type : " << constraint.type << std::endl; + if (constraint.getState() == State::ACTIVE) { + os << "state : active " << std::endl; + } + if (constraint.getState() == State::INACTIVE) { + os << "state : inactive " << std::endl; + } + os << constraint.getPolygons(); + return os; +} +} // namespace constraints +} // namespace context_gamma_planner diff --git a/simulation/context_gamma_planner/context_gamma_planner/src/constraints/pedestrian/constraint_activator.cpp b/simulation/context_gamma_planner/context_gamma_planner/src/constraints/pedestrian/constraint_activator.cpp new file mode 100644 index 00000000000..419cf5492f8 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/src/constraints/pedestrian/constraint_activator.cpp @@ -0,0 +1,36 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +namespace context_gamma_planner +{ +namespace pedestrian +{ +namespace constraints +{ +using State = context_gamma_planner::constraints::State; + +ConstraintActivator::ConstraintActivator( + const std::shared_ptr hd_map_utils_ptr, + const std::shared_ptr traffic_lights_ptr) +: context_gamma_planner::constraints::ConstraintActivatorBase( + hd_map_utils_ptr, traffic_lights_ptr) +{ +} + +} // namespace constraints +} // namespace pedestrian +} // namespace context_gamma_planner diff --git a/simulation/context_gamma_planner/context_gamma_planner/src/constraints/road_edge_constraint.cpp b/simulation/context_gamma_planner/context_gamma_planner/src/constraints/road_edge_constraint.cpp new file mode 100644 index 00000000000..7985f08e1af --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/src/constraints/road_edge_constraint.cpp @@ -0,0 +1,83 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +namespace context_gamma_planner +{ +namespace constraints +{ +RoadEdgeConstraint::RoadEdgeConstraint( + const std::shared_ptr & hdmap_utils, const lanelet::Id lanelet_id, + const Side & side) +: ConstraintBase(hdmap_utils, LANE_CONSTRAINT, {[&]() { + switch (side) { + case Side::RIGHT: + return hdmap_utils->getRightBound(lanelet_id); + case Side::LEFT: + return hdmap_utils->getLeftBound(lanelet_id); + case Side::FRONT: { + context_gamma_planner::constraints::Polygon front_bound; + front_bound.push_back(hdmap_utils->getRightBound(lanelet_id).back()); + front_bound.push_back(hdmap_utils->getLeftBound(lanelet_id).back()); + return front_bound; + } + case Side::BACK: { + context_gamma_planner::constraints::Polygon back_bound; + back_bound.push_back(hdmap_utils->getRightBound(lanelet_id).front()); + back_bound.push_back(hdmap_utils->getLeftBound(lanelet_id).front()); + return back_bound; + } + } + THROW_SIMULATION_ERROR( + "Please check side of the lanelet, something completely wrong happend.", + "This message is mainly for developers.", + "If you are not developer, please notify to Masaya Kataoka (@hakuturu583)"); + }()}), + lanelet_id(lanelet_id), + side(side) +{ +} + +// void setState( +// std::vector & constraints, const std::vector & lanelet_ids, +// const RoadEdgeConstraint::Side & side, const State & state) +// { +// } + +void setState( + std::vector & constraints, const lanelet::Id lanelet_id, + const RoadEdgeConstraint::Side & side, const State & state) +{ + auto result = + std::find_if(constraints.begin(), constraints.end(), [lanelet_id, side](RoadEdgeConstraint x) { + return x.lanelet_id == lanelet_id && x.side == side; + }); + if (result != constraints.end()) { + result->setState(state); + } +} + +void setState( + std::vector & constraints, const std::vector & lanelet_ids, + const State & state) +{ + for (const auto & id : lanelet_ids) { + setState(constraints, id, RoadEdgeConstraint::Side::RIGHT, state); + setState(constraints, id, RoadEdgeConstraint::Side::LEFT, state); + } +} + +} // namespace constraints +} // namespace context_gamma_planner diff --git a/simulation/context_gamma_planner/context_gamma_planner/src/constraints/stop_line_constraint.cpp b/simulation/context_gamma_planner/context_gamma_planner/src/constraints/stop_line_constraint.cpp new file mode 100644 index 00000000000..10f8e8dcf4d --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/src/constraints/stop_line_constraint.cpp @@ -0,0 +1,29 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +namespace context_gamma_planner +{ +namespace constraints +{ +StopLineConstraint::StopLineConstraint( + const std::shared_ptr & hdmap_utils, const lanelet::Id stop_line_id) +: ConstraintBase( + hdmap_utils, STOP_LINE_CONSTRAINT, {hdmap_utils->getStopLinePolygon(stop_line_id)}), + stop_line_id(stop_line_id) +{ +} +} // namespace constraints +} // namespace context_gamma_planner diff --git a/simulation/context_gamma_planner/context_gamma_planner/src/constraints/traffic_light_constraint.cpp b/simulation/context_gamma_planner/context_gamma_planner/src/constraints/traffic_light_constraint.cpp new file mode 100644 index 00000000000..69db6d62a62 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/src/constraints/traffic_light_constraint.cpp @@ -0,0 +1,31 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +namespace context_gamma_planner +{ +namespace constraints +{ +TrafficLightConstraint::TrafficLightConstraint( + const std::shared_ptr & hdmap_utils, const lanelet::Id traffic_light_id, + const lanelet::Id stop_line_id) +: ConstraintBase( + hdmap_utils, TRAFFIC_LIGHT_CONSTRAINT, {hdmap_utils->getStopLinePolygon(stop_line_id)}), + traffic_light_id(traffic_light_id), + stop_line_id(stop_line_id) +{ +} +} // namespace constraints +} // namespace context_gamma_planner diff --git a/simulation/context_gamma_planner/context_gamma_planner/src/constraints/vehicle/constraint_activator.cpp b/simulation/context_gamma_planner/context_gamma_planner/src/constraints/vehicle/constraint_activator.cpp new file mode 100644 index 00000000000..77314da821c --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/src/constraints/vehicle/constraint_activator.cpp @@ -0,0 +1,147 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +namespace context_gamma_planner +{ +namespace vehicle +{ +namespace constraints +{ +using State = context_gamma_planner::constraints::State; +using Side = context_gamma_planner::constraints::RoadEdgeConstraint::Side; + +ConstraintActivator::ConstraintActivator( + const std::shared_ptr hd_map_utils_ptr, + const std::shared_ptr traffic_lights_ptr) +: context_gamma_planner::constraints::ConstraintActivatorBase( + hd_map_utils_ptr, traffic_lights_ptr) +{ + for (const auto & id : hd_map_utils_ptr_->getTrafficLightIds()) { + const auto stop_line_ids = hd_map_utils_ptr_->getTrafficLightStopLineIds(id); + for (const auto stop_line_id : stop_line_ids) { + traffic_light_constraints_.emplace_back( + context_gamma_planner::constraints::TrafficLightConstraint( + hd_map_utils_ptr_, id, stop_line_id)); + } + } + for (const auto & id : hd_map_utils_ptr_->getStopLineIds()) { + stop_line_constraints_.emplace_back( + context_gamma_planner::constraints::StopLineConstraint(hd_map_utils_ptr_, id)); + } +} + +void ConstraintActivator::appendStopLineConstraint( + const lanelet::Ids & route_ids, + const std::shared_ptr reference_trajectory, + const std::shared_ptr & entity_status, + const double stop_line_enable_threshold, const double stop_velocity_threshold) +{ + const double horizon = std::clamp(entity_status->getTwist().linear.x * 5.0, 20.0, 50.0); + const auto trajectory = std::make_unique( + reference_trajectory, entity_status->getLaneletPose().s, + entity_status->getLaneletPose().s + horizon); + const auto distance_to_stopline = + hd_map_utils_ptr_->getDistanceToStopLine(route_ids, *trajectory); + if (distance_to_stopline) { + //Enable stop line constraint while the car is moving + if (not is_stoped_) { + setStateByStopLineId( + stop_line_constraints_, hd_map_utils_ptr_->getStopLineIdsOnPath(route_ids), + context_gamma_planner::constraints::State::ACTIVE); + } + //Clear the stop line constraint when the car is paused + if ( + distance_to_stopline.value() < stop_line_enable_threshold and + entity_status->getTwist().linear.x < stop_velocity_threshold) { + is_stoped_ = true; + return; + } + //Allow the stop line to be reactivated once the car leaves the stop line + if (is_stoped_ and distance_to_stopline.value() > stop_line_enable_threshold) { + is_stoped_ = false; + } + } +} + +void ConstraintActivator::appendTrafficLightConstraint(const lanelet::Ids & route_ids) +{ + for (const auto & traffic_light_id : hd_map_utils_ptr_->getTrafficLightIdsOnPath(route_ids)) { + if (traffic_lights_ptr_->getTrafficLight(traffic_light_id) + .contains( + traffic_simulator::TrafficLight::Color::green, + traffic_simulator::TrafficLight::Status::solid_on, + traffic_simulator::TrafficLight::Shape::circle)) { + continue; + } + setStateByStopLineId( + traffic_light_constraints_, hd_map_utils_ptr_->getTrafficLightStopLineIds(traffic_light_id), + context_gamma_planner::constraints::State::ACTIVE); + } +} + +void ConstraintActivator::appendLaneChangeConstraint( + const std::shared_ptr & entity_status, + const traffic_simulator::lane_change::Parameter & lane_change_parameter) +{ + if (!entity_status->laneMatchingSucceed()) { + return; + } + std::optional id = entity_status->getLaneletPose().lanelet_id; + while (true) { + id = hd_map_utils_ptr_->getLaneChangeableLaneletId( + id.value(), traffic_simulator::lane_change::Direction::LEFT); + if (!id) { + break; + } + if (id.value() == lane_change_parameter.target.lanelet_id) { + appendRoadEdgeConstraint(entity_status->getLaneletPose().lanelet_id, Side::RIGHT); + appendRoadEdgeConstraint(lane_change_parameter.target.lanelet_id, Side::LEFT); + return; + } + } + id = entity_status->getLaneletPose().lanelet_id; + while (true) { + id = hd_map_utils_ptr_->getLaneChangeableLaneletId( + id.value(), traffic_simulator::lane_change::Direction::RIGHT); + if (!id) { + break; + } + if (id.value() == lane_change_parameter.target.lanelet_id) { + appendRoadEdgeConstraint(lane_change_parameter.target.lanelet_id, Side::RIGHT); + appendRoadEdgeConstraint(entity_status->getLaneletPose().lanelet_id, Side::LEFT); + return; + } + } + if (entity_status->getLaneletPose().lanelet_id == lane_change_parameter.target.lanelet_id) { + appendRoadEdgeConstraint(entity_status->getLaneletPose().lanelet_id); + return; + } +} + +void ConstraintActivator::appendRoadEndConstraint(const lanelet::Ids & route_ids) +{ + if (hd_map_utils_ptr_->getNextLaneletIds(route_ids.back()).empty()) { + appendRoadEdgeConstraint(route_ids.back(), Side::FRONT); + } + if (hd_map_utils_ptr_->getPreviousLaneletIds(route_ids.back()).empty()) { + appendRoadEdgeConstraint(route_ids.back(), Side::BACK); + } +} + +} // namespace constraints +} // namespace vehicle +} // namespace context_gamma_planner diff --git a/simulation/context_gamma_planner/context_gamma_planner/src/pedestrian_plugin.cpp b/simulation/context_gamma_planner/context_gamma_planner/src/pedestrian_plugin.cpp new file mode 100644 index 00000000000..b5136d6a7fd --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/src/pedestrian_plugin.cpp @@ -0,0 +1,265 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +namespace context_gamma_planner +{ +/** + * @brief Configures the PedestrianPlugin. + * @param logger The logger object used for logging. + */ +void PedestrianPlugin::configure(const rclcpp::Logger & logger) +{ + std::cout << "pedestrian plugin configure start" << std::endl; + std::string path = ament_index_cpp::get_package_share_directory("context_gamma_planner") + + "/config/pedestrian_behavior.xml"; + factory_.registerNodeType("FollowLane"); + factory_.registerNodeType( + "FollowPolylineTrajectory"); + tree_ = createBehaviorTree(path); + logging_event_ptr_ = + std::make_shared(tree_.rootNode(), logger); + reset_request_event_ptr_ = std::make_shared( + tree_.rootNode(), [&]() { return getRequest(); }, + [&](traffic_simulator::behavior::Request request) { return setRequest(request); }); + setRequest(traffic_simulator::behavior::Request::NONE); + traffic_simulator_msgs::msg::Obstacle obstacle_msg; + obstacle_msg.type = traffic_simulator_msgs::msg::Obstacle::ENTITY; + setObstacle(obstacle_msg); + std::cout << "configure end" << std::endl; +} + +/** + * @brief Creates a behavior tree from an XML file. + * @param format_path The path to the XML file. + * @return The created behavior tree. + */ +auto PedestrianPlugin::createBehaviorTree(const std::string & format_path) -> BT::Tree +{ + auto xml_doc = pugi::xml_document(); + xml_doc.load_file(format_path.c_str()); + + class XMLTreeWalker : public pugi::xml_tree_walker + { + public: + explicit XMLTreeWalker(const BT::TreeNodeManifest & manifest) : manifest_(manifest) {} + + private: + bool for_each(pugi::xml_node & node) final + { + if (node.name() == manifest_.registration_ID) { + for (const auto & [port, info] : manifest_.ports) { + node.append_attribute(port.c_str()) = std::string("{" + port + "}").c_str(); + } + } + return true; + } + + const BT::TreeNodeManifest & manifest_; + }; + + for (const auto & [id, manifest] : factory_.manifests()) { + if (factory_.builtinNodes().count(id) == 0) { + auto walker = XMLTreeWalker(manifest); + xml_doc.traverse(walker); + } + } + + auto xml_str = std::stringstream(); + xml_doc.save(xml_str); + return factory_.createTreeFromText(xml_str.str()); +} + +/** + * @brief Get the current action. + * @return const std::string& The current action. + */ +const std::string & PedestrianPlugin::getCurrentAction() const +{ + return logging_event_ptr_->getCurrentAction(); +} + +/** + * @brief Updates the pedestrian plugin. + * @param current_time The current time. + * @param step_time The time step. + */ +void PedestrianPlugin::update(double current_time, double step_time) +{ + tryInitializeConstraintActivator(); + tryInitializeEgoInRVO(); + + tickOnce(current_time, step_time); + while (getCurrentAction() == "root") { + tickOnce(current_time, step_time); + } + + if (getPlanningSpeed()) { + rvo_ego_->setMaxSpeed(getPlanningSpeed().value()); + } + + rvo_simulator_.deleteObstacles(); + rvo_simulator_.addGlobalObstacles(activator_ptr_->calculateRVOObstacles()); + rvo_simulator_.processObstacles(); + + reflectEgoInRVO( + static_cast(*this->getCanonicalizedEntityStatus())); + reflectNonEgoEntitiesInRVO(); + rvo_simulator_.deleteUnavailableAgent(); + updateRVO(step_time); + updateSimulatorStatus(); + visualize(); + + /// @note cleanup constraints + activator_ptr_->deactivateAllConstraints(); +} + +void PedestrianPlugin::tryInitializeConstraintActivator() +{ + if (activator_ptr_ == nullptr) { + activator_ptr_ = + std::make_shared( + getHdMapUtils(), getTrafficLights()); + setConstraintActivator(activator_ptr_); + } +} + +void PedestrianPlugin::tryInitializeEgoInRVO() +{ + if (rvo_ego_ == nullptr) { + std::cout << "set agent conf start" << std::endl; + /// @todo This value should be set by entity parameters. + const mock::Catalogs catalogs; + RVO::AgentConfig pedestrian_agent_config = + rvo_simulator_.createPedestrianConfig(catalogs.getPedestrianParameters()); + rvo_ego_ = std::make_shared( + getCanonicalizedEntityStatus()->getName(), getCanonicalizedEntityStatus()->getMapPose(), + pedestrian_agent_config); + rvo_simulator_.addAgent(rvo_ego_); + std::cout << "set agent conf end" << std::endl; + } +} + +/** + * @brief a single tick of the behavior tree. + * @param current_time The current time. + * @param step_time The time step. + * @return The status of the behavior tree node. + */ +BT::NodeStatus PedestrianPlugin::tickOnce(double current_time, double step_time) +{ + setCurrentTime(current_time); + setStepTime(step_time); + return tree_.rootNode()->executeTick(); +} + +void PedestrianPlugin::reflectEgoInRVO(const traffic_simulator::EntityStatus & ego_status) +{ + rvo_ego_->is_available_ = true; + auto ego_pos = ego_status.pose.position; + rvo_ego_->setPosition(RVO::Vector2(ego_pos.x, ego_pos.y)); +} + +void PedestrianPlugin::reflectNonEgoEntitiesInRVO() +{ + auto other_entity_status = getOtherEntityStatus(); + rvo_simulator_.markUnavailableAllAgent(); + rvo_ego_->is_available_ = true; + for (auto entity_status : other_entity_status) { + auto agent = rvo_simulator_.getAgent( + static_cast(entity_status.second).name); + if (agent) { + updateNonEgoEntityInRVO(agent, entity_status.second); + } else { + createNonEgoEntityInRVO(entity_status.second); + std::cout << "add new agent" << std::endl; + } + } +} + +void PedestrianPlugin::updateNonEgoEntityInRVO( + std::shared_ptr agent, + const traffic_simulator::entity_status::CanonicalizedEntityStatus & entity_status) +{ + agent->is_available_ = true; + agent->setPose(entity_status.getMapPose()); + agent->setVelocity( + RVO::Vector2(entity_status.getTwist().linear.x, entity_status.getTwist().linear.y)); + // we don't know other agent's navigation planning here. + // but, the agent velocity is reflecting its planner's decision in previous simulation step + // so, we set a waypoint in the direction of travel + agent->updateFrontWaypoint(agent->getPosition() + agent->getVelocity() * 10); +} + +void PedestrianPlugin::createNonEgoEntityInRVO( + const traffic_simulator::entity_status::CanonicalizedEntityStatus & canonicalized_entity_status) +{ + auto entity_status = static_cast(canonicalized_entity_status); + RVO::AgentConfig other_entity_config; + const mock::Catalogs catalogs; + /// @todo This value should be set by entity parameters. + if (entity_status.type.type == traffic_simulator_msgs::msg::EntityType::VEHICLE) { + other_entity_config = rvo_simulator_.createVehicleConfig(catalogs.getVehicleParameters()); + } else if (entity_status.type.type == traffic_simulator_msgs::msg::EntityType::PEDESTRIAN) { + other_entity_config = rvo_simulator_.createPedestrianConfig(catalogs.getPedestrianParameters()); + } + auto new_agent = std::make_shared( + entity_status.name, canonicalized_entity_status.getMapPose(), other_entity_config); + rvo_simulator_.addAgent(new_agent); +} + +void PedestrianPlugin::updateRVO(double step_time) +{ + auto next_goal = getNextGoal(); + rvo_ego_->updateFrontWaypoint(RVO::Vector2(next_goal.x, next_goal.y)); + rvo_simulator_.setTimeStep(step_time); + rvo_simulator_.update(rvo_ego_); +} + +void PedestrianPlugin::updateSimulatorStatus() +{ + auto ego_status = + static_cast(*this->getCanonicalizedEntityStatus()); + ego_status.pose = rvo_ego_->getPose(); + ego_status.action_status.twist.linear.x = rvo_ego_->getVelocity().x(); + ego_status.action_status.twist.linear.y = rvo_ego_->getVelocity().y(); + const auto lanelet_ego_pose = getHdMapUtils()->toLaneletPose(ego_status.pose, getRouteLanelets()); + if (lanelet_ego_pose) { + ego_status.lanelet_pose = lanelet_ego_pose.value(); + } + getCanonicalizedEntityStatus()->set( + ego_status, getDefaultMatchingDistanceForLaneletPoseCalculation(), getHdMapUtils()); +} + +void PedestrianPlugin::visualize() +{ + visualization_msgs::msg::MarkerArray marker_array; + rvo_visualize_.visualizeAllMarkers(marker_array, rvo_simulator_); + rvo_visualize_.visualizeNextGoal(marker_array, getNextGoal()); + for (auto && marker : marker_array.markers) { + marker.ns += "_" + getCanonicalizedEntityStatus()->getName(); + } + setDebugMarker(marker_array.markers); +} + +} // namespace context_gamma_planner + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(context_gamma_planner::PedestrianPlugin, entity_behavior::BehaviorPluginBase) diff --git a/simulation/context_gamma_planner/context_gamma_planner/src/planner/follow_polyline_trajectory_planner_base.cpp b/simulation/context_gamma_planner/context_gamma_planner/src/planner/follow_polyline_trajectory_planner_base.cpp new file mode 100644 index 00000000000..1ea32a1b915 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/src/planner/follow_polyline_trajectory_planner_base.cpp @@ -0,0 +1,122 @@ +// Copyright 2015-2020 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include + +namespace context_gamma_planner +{ + +FollowPolylineTrajectoryPlannerBase::FollowPolylineTrajectoryPlannerBase( + const double goal_threshold) +: GoalPlannerBase(goal_threshold) +{ +} + +void FollowPolylineTrajectoryPlannerBase::setWaypoints( + const std::shared_ptr trajectory) +{ + trajectory_ = trajectory; + goal_poses_.clear(); + if (trajectory->dynamic_constraints_ignorable) { + for (const auto & vertex : trajectory->shape.vertices) { + goal_poses_.emplace_back(vertex.position); + } + } else { + std::vector vertices_points; + for (const auto & vertex : trajectory->shape.vertices) { + vertices_points.emplace_back(vertex.position.position); + } + const math::geometry::CatmullRomSpline spline(vertices_points); + for (const auto & trajectory_point : spline.getTrajectory(0.0, spline.getLength(), 1.0)) { + geometry_msgs::msg::Pose pose; + pose.position = trajectory_point; + goal_poses_.emplace_back(pose); + } + } +} + +size_t FollowPolylineTrajectoryPlannerBase::getCurrentTrajectoryWaypointIndex() const +{ + size_t current_trajectory_waypoint_index = 0; + for (const auto & vertex : trajectory_->shape.vertices) { + if (math::geometry::getRelativePose(status_->getMapPose(), vertex.position).position.x >= 0) { + return current_trajectory_waypoint_index; + } + ++current_trajectory_waypoint_index; + } + return trajectory_->shape.vertices.size() - 1; +} + +auto FollowPolylineTrajectoryPlannerBase::calculateNextGoalPoint() + -> std::optional +{ + if (!status_) { + THROW_SIMULATION_ERROR( + "Current entity status should be set before you want to calculate goal pose."); + } + if (goal_poses_.empty()) { + THROW_SIMULATION_ERROR("goal poses is empty"); + } + if (!trajectory_) { + THROW_SIMULATION_ERROR( + "Trajectory does not specified. ", + "This message is not originally intended to be displayed, if you see it, " + "please contact the developer of traffic_simulator."); + } + while (!goal_poses_.empty()) { + if ( + math::geometry::getRelativePose(status_->getMapPose(), goal_poses_.front()).position.x >= 0 && + std::hypot( + (status_->getMapPose().position.x - goal_poses_.front().position.x), + (status_->getMapPose().position.y - goal_poses_.front().position.y)) <= goal_threshold) { + if (trajectory_->closed) { + goal_poses_.emplace_back(goal_poses_.front()); + } + goal_poses_.pop_front(); + } else { + return goal_poses_.front().position; + } + } + return std::nullopt; +} + +/// @todo Re-consider speed planning algorithm. +double FollowPolylineTrajectoryPlannerBase::getTargetSpeed(const double current_time) const +{ + if (trajectory_->base_time > current_time) { + return 0.0; + } + if (goal_poses_.empty()) { + THROW_SIMULATION_ERROR("goal poses is empty"); + } + double goal_time = trajectory_->base_time + + trajectory_->shape.vertices.at(getCurrentTrajectoryWaypointIndex()).time; + goal_time = std::fmod(goal_time, trajectory_->shape.vertices.back().time); + if (goal_time <= current_time) { + return max_speed_; + } + return std::hypot( + (status_->getMapPose().position.x - goal_poses_.front().position.x), + (status_->getMapPose().position.y - goal_poses_.front().position.y)) / + (goal_time - current_time); +} + +void FollowPolylineTrajectoryPlannerBase::clear() { goal_poses_.clear(); } + +} // namespace context_gamma_planner diff --git a/simulation/context_gamma_planner/context_gamma_planner/src/planner/goal_planner_base.cpp b/simulation/context_gamma_planner/context_gamma_planner/src/planner/goal_planner_base.cpp new file mode 100644 index 00000000000..fd2ba6e1786 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/src/planner/goal_planner_base.cpp @@ -0,0 +1,69 @@ +// Copyright 2015-2020 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include + +namespace context_gamma_planner +{ +// void GoalPlannerBase::setRouteLanelets(const std::vector & route) { route_ = route; } + +GoalPlannerBase::GoalPlannerBase(const double goal_threshold) : goal_threshold(goal_threshold) {} + +void GoalPlannerBase::setCurrentStatus( + const std::shared_ptr & status) +{ + status_ = *status; +} + +traffic_simulator_msgs::msg::WaypointsArray GoalPlannerBase::getWaypoints() const +{ + traffic_simulator_msgs::msg::WaypointsArray waypoints; + waypoints.waypoints.emplace_back(status_->getMapPose().position); + std::transform( + goal_poses_.begin(), goal_poses_.end(), std::back_inserter(waypoints.waypoints), + [](const auto & pose) { return pose.position; }); + return waypoints; +} + +void GoalPlannerBase::appendGoalPoses(const std::vector & goal_poses) +{ + goal_poses_.insert(goal_poses_.end(), goal_poses.begin(), goal_poses.end()); +} + +void GoalPlannerBase::appendGoalPoints(const std::vector & goal_points) +{ + for (const auto point : goal_points) { + geometry_msgs::msg::Pose pose; + pose.position = point; + goal_poses_.emplace_back(pose); + } +} + +void GoalPlannerBase::setMaxSpeed(const double & max_speed) { max_speed_ = max_speed; } + +std::vector GoalPlannerBase::getGoalPoses() const +{ + std::vector goal_poses; + std::transform( + goal_poses_.begin(), goal_poses_.end(), std::back_inserter(goal_poses), + [](const auto & pose) { return pose; }); + return goal_poses; +} + +} // namespace context_gamma_planner diff --git a/simulation/context_gamma_planner/context_gamma_planner/src/planner/pedestrian/follow_lane_planner.cpp b/simulation/context_gamma_planner/context_gamma_planner/src/planner/pedestrian/follow_lane_planner.cpp new file mode 100644 index 00000000000..f638020ff39 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/src/planner/pedestrian/follow_lane_planner.cpp @@ -0,0 +1,77 @@ +// Copyright 2015-2020 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include + +namespace context_gamma_planner +{ +namespace pedestrian +{ +FollowLanePlanner::FollowLanePlanner(const double goal_threshold) : GoalPlannerBase(goal_threshold) +{ +} + +void FollowLanePlanner::setWaypoints( + const std::shared_ptr hdmap_utils, + const std::vector & route_ids) +{ + goal_poses_.clear(); + for (const auto & point : hdmap_utils->getCenterPoints(route_ids)) { + geometry_msgs::msg::Pose pose; + pose.position = point; + goal_poses_.emplace_back(pose); + } +} + +auto FollowLanePlanner::calculateNextGoalPoint() -> std::optional +{ + if (!status_) { + THROW_SIMULATION_ERROR( + "Current entity status should be set before you want to calculate goal pose."); + } + if (goal_poses_.empty()) { + THROW_SIMULATION_ERROR("Goal poses should be set before you want to calculate goal pose."); + } + while (!goal_poses_.empty()) { + if ( + std::hypot( + (status_->getMapPose().position.x - goal_poses_.front().position.x), + (status_->getMapPose().position.y - goal_poses_.front().position.y)) >= goal_threshold) { + goal_poses_.pop_front(); + } else { + break; + } + } + while (goal_poses_.size() > 1) { + if ( + std::hypot( + (status_->getMapPose().position.x - goal_poses_.front().position.x), + (status_->getMapPose().position.y - goal_poses_.front().position.y)) < goal_threshold) { + goal_poses_.pop_front(); + } else { + return goal_poses_.front().position; + } + } + return goal_poses_.front().position; +} + +void FollowLanePlanner::clear() { goal_poses_.clear(); } + +} // namespace pedestrian +} // namespace context_gamma_planner diff --git a/simulation/context_gamma_planner/context_gamma_planner/src/planner/pedestrian/follow_polyline_trajectory_planner.cpp b/simulation/context_gamma_planner/context_gamma_planner/src/planner/pedestrian/follow_polyline_trajectory_planner.cpp new file mode 100644 index 00000000000..db49039df4e --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/src/planner/pedestrian/follow_polyline_trajectory_planner.cpp @@ -0,0 +1,32 @@ +// Copyright 2015-2020 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include + +namespace context_gamma_planner +{ +namespace pedestrian +{ +FollowPolylineTrajectoryPlanner::FollowPolylineTrajectoryPlanner(const double goal_threshold) +: FollowPolylineTrajectoryPlannerBase(goal_threshold) +{ +} + +} // namespace pedestrian +} // namespace context_gamma_planner diff --git a/simulation/context_gamma_planner/context_gamma_planner/src/planner/vehicle/follow_lane_planner.cpp b/simulation/context_gamma_planner/context_gamma_planner/src/planner/vehicle/follow_lane_planner.cpp new file mode 100644 index 00000000000..517ba4278bc --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/src/planner/vehicle/follow_lane_planner.cpp @@ -0,0 +1,98 @@ +// Copyright 2015-2020 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include + +namespace context_gamma_planner +{ +namespace vehicle +{ + +FollowLanePlanner::FollowLanePlanner(const double goal_threshold) : GoalPlannerBase(goal_threshold) +{ +} + +void FollowLanePlanner::setWaypoints( + const std::shared_ptr hdmap_utils, + const std::vector & route_ids) +{ + goal_poses_.clear(); + for (const auto & point : hdmap_utils->getCenterPoints(route_ids)) { + geometry_msgs::msg::Pose pose; + pose.position = point; + goal_poses_.emplace_back(pose); + } + /// @note This part is used in situations where Entity spawns from the middle of a lane, such as a lane change. + /// Determine that the algorithm needs to be modified. + // if (!pre_route_id_ or pre_route_id_.value() != route_ids.at(0)) { + // pre_route_id_ = route_ids.at(0); + // const auto lanelet_pose = + // hdmap_utils->toLaneletPose(status_->getMapPose(), route_ids.at(0), 10.0); + + // if (lanelet_pose) { + // while (hdmap_utils->toLaneletPose(goal_poses_.front(), route_ids.at(0), 50.0) and + // lanelet_pose.value().s > + // hdmap_utils->toLaneletPose(goal_poses_.front(), route_ids.at(0), 50.0).value().s) { + // goal_poses_.pop_front(); + // } + // } + // } + // pre_route_id_ = route_ids.at(0); +} + +auto FollowLanePlanner::calculateNextGoalPoint() -> std::optional +{ + if (!status_) { + THROW_SIMULATION_ERROR( + "Current entity status should be set before you want to calculate goal pose."); + } + if (goal_poses_.empty()) { + THROW_SIMULATION_ERROR("Goal poses should be set before you want to calculate goal pose."); + } + while (!goal_poses_.empty()) { + if ( + std::hypot( + (status_->getMapPose().position.x - goal_poses_.front().position.x), + (status_->getMapPose().position.y - goal_poses_.front().position.y)) >= goal_threshold) { + goal_poses_.pop_front(); + } else { + break; + } + } + while (goal_poses_.size() > 1) { + if ( + std::hypot( + (status_->getMapPose().position.x - goal_poses_.front().position.x), + (status_->getMapPose().position.y - goal_poses_.front().position.y)) < goal_threshold) { + goal_poses_.pop_front(); + } else { + return goal_poses_.front().position; + } + } + return goal_poses_.front().position; +} + +void FollowLanePlanner::clear() +{ + goal_poses_.clear(); + // pre_route_id_ = std::nullopt; +} + +} // namespace vehicle +} // namespace context_gamma_planner diff --git a/simulation/context_gamma_planner/context_gamma_planner/src/planner/vehicle/follow_polyline_trajectory_planner.cpp b/simulation/context_gamma_planner/context_gamma_planner/src/planner/vehicle/follow_polyline_trajectory_planner.cpp new file mode 100644 index 00000000000..d9a3ba197ca --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/src/planner/vehicle/follow_polyline_trajectory_planner.cpp @@ -0,0 +1,32 @@ +// Copyright 2015-2020 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include + +namespace context_gamma_planner +{ +namespace vehicle +{ +FollowPolylineTrajectoryPlanner::FollowPolylineTrajectoryPlanner(const double goal_threshold) +: FollowPolylineTrajectoryPlannerBase(goal_threshold) +{ +} + +} // namespace vehicle +} // namespace context_gamma_planner diff --git a/simulation/context_gamma_planner/context_gamma_planner/src/planner/vehicle/lane_change_planner.cpp b/simulation/context_gamma_planner/context_gamma_planner/src/planner/vehicle/lane_change_planner.cpp new file mode 100644 index 00000000000..1153d40e73b --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/src/planner/vehicle/lane_change_planner.cpp @@ -0,0 +1,136 @@ +// Copyright 2015-2020 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include + +namespace context_gamma_planner +{ +namespace vehicle +{ +LaneChangePlanner::LaneChangePlanner(const double goal_threshold) : GoalPlannerBase(goal_threshold) +{ +} + +void LaneChangePlanner::setWaypoints( + const std::shared_ptr hdmap_utils, + const std::vector & route_ids, + const traffic_simulator::lane_change::Parameter & lane_change_parameter) +{ + clear(); + // calc trajectory + const auto lanelet_pose = hdmap_utils->toLaneletPose(status_->getMapPose(), route_ids); + if (!lanelet_pose) { + THROW_SIMULATION_ERROR("Failed to get lanelet pose"); + } + std::optional> lanechange_trajectory; + traffic_simulator::LaneletPose along_pose, goal_pose; + switch (lane_change_parameter.constraint.type) { + case traffic_simulator::lane_change::Constraint::Type::NONE: + /** + @note Hard coded parameter, + 10.0 is a maximum_curvature_threshold (If the curvature of the trajectory is over 10.0, the trajectory was not selected.) + 20.0 is a target_trajectory_length (The one with the closest length to 20 m is selected from the candidate trajectories.) + 1.0 is a forward_distance_threshold (If the goal x position in the cartesian coordinate was under 1.0, the goal was rejected.) + */ + lanechange_trajectory = hdmap_utils->getLaneChangeTrajectory( + hdmap_utils->toMapPose(lanelet_pose.value()).pose, lane_change_parameter, 10.0, 20.0, 1.0); + along_pose = hdmap_utils->getAlongLaneletPose( + lanelet_pose.value(), + traffic_simulator::lane_change::Parameter::default_lanechange_distance); + break; + case traffic_simulator::lane_change::Constraint::Type::LATERAL_VELOCITY: + lanechange_trajectory = + hdmap_utils->getLaneChangeTrajectory(lanelet_pose.value(), lane_change_parameter); + along_pose = hdmap_utils->getAlongLaneletPose( + lanelet_pose.value(), + traffic_simulator::lane_change::Parameter::default_lanechange_distance); + break; + case traffic_simulator::lane_change::Constraint::Type::LONGITUDINAL_DISTANCE: + lanechange_trajectory = + hdmap_utils->getLaneChangeTrajectory(lanelet_pose.value(), lane_change_parameter); + along_pose = hdmap_utils->getAlongLaneletPose( + lanelet_pose.value(), lane_change_parameter.constraint.value); + break; + case traffic_simulator::lane_change::Constraint::Type::TIME: + lanechange_trajectory = + hdmap_utils->getLaneChangeTrajectory(lanelet_pose.value(), lane_change_parameter); + along_pose = hdmap_utils->getAlongLaneletPose( + lanelet_pose.value(), lane_change_parameter.constraint.value); + break; + } + if (!lanechange_trajectory) { + THROW_SIMULATION_ERROR("Failed to get lane change trajectory"); + } + for (const auto & lanechange_point : lanechange_trajectory.value().first.getTrajectory()) { + geometry_msgs::msg::Pose pose; + pose.position = lanechange_point; + goal_poses_.emplace_back(pose); + } + + // calc velocity + const auto curve_ = lanechange_trajectory->first; + goal_pose.lanelet_id = lane_change_parameter.target.lanelet_id; + goal_pose.s = lanechange_trajectory->second; + double offset = + std::fabs(math::geometry::getRelativePose( + hdmap_utils->toMapPose(along_pose).pose, hdmap_utils->toMapPose(goal_pose).pose) + .position.y); + switch (lane_change_parameter.constraint.type) { + case traffic_simulator::lane_change::Constraint::Type::NONE: + target_speed_ = max_speed_; + break; + case traffic_simulator::lane_change::Constraint::Type::LATERAL_VELOCITY: + target_speed_ = curve_.getLength() / (offset / lane_change_parameter.constraint.value); + break; + case traffic_simulator::lane_change::Constraint::Type::LONGITUDINAL_DISTANCE: + target_speed_ = max_speed_; + break; + case traffic_simulator::lane_change::Constraint::Type::TIME: + target_speed_ = curve_.getLength() / lane_change_parameter.constraint.value; + break; + } +} + +auto LaneChangePlanner::calculateNextGoalPoint() -> std::optional +{ + if (!status_) { + THROW_SIMULATION_ERROR( + "Current entity status should be set before you want to calculate goal pose."); + } + if (goal_poses_.empty()) { + THROW_SIMULATION_ERROR("goal poses is empty"); + } + + while (!goal_poses_.empty()) { + if ( + std::hypot( + (status_->getMapPose().position.x - goal_poses_.front().position.x), + (status_->getMapPose().position.y - goal_poses_.front().position.y)) <= goal_threshold) { + goal_poses_.pop_front(); + } else { + return goal_poses_.front().position; + } + } + return std::nullopt; +} + +void LaneChangePlanner::clear() { goal_poses_.clear(); } + +} // namespace vehicle +} // namespace context_gamma_planner diff --git a/simulation/context_gamma_planner/context_gamma_planner/src/transition_events/logging_event.cpp b/simulation/context_gamma_planner/context_gamma_planner/src/transition_events/logging_event.cpp new file mode 100644 index 00000000000..128dfc33866 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/src/transition_events/logging_event.cpp @@ -0,0 +1,37 @@ +// Copyright 2015-2020 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +namespace context_gamma_planner +{ +LoggingEvent::LoggingEvent(BT::TreeNode * root_node, const rclcpp::Logger & logger) +: TransitionEvent(root_node), ros_logger_(logger) +{ +} + +void LoggingEvent::callback( + BT::Duration /*timestamp*/, const BT::TreeNode & node, BT::NodeStatus prev_status, + BT::NodeStatus status) +{ + RCLCPP_INFO_STREAM( + ros_logger_, "Action " << node.name() << " changed status, " << BT::toStr(prev_status, true) + << " => " << BT::toStr(status, true)); + TransitionEvent::updateCurrentAction(status, node); +} + +const std::string & LoggingEvent::getCurrentAction() const { return current_action_; } +} // namespace context_gamma_planner diff --git a/simulation/context_gamma_planner/context_gamma_planner/src/transition_events/reset_request_event.cpp b/simulation/context_gamma_planner/context_gamma_planner/src/transition_events/reset_request_event.cpp new file mode 100644 index 00000000000..5aec77b37a9 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/src/transition_events/reset_request_event.cpp @@ -0,0 +1,43 @@ +// Copyright 2015-2020 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +namespace context_gamma_planner +{ +ResetRequestEvent::ResetRequestEvent( + BT::TreeNode * root_node, + std::function get_request_function, + std::function set_request_function) +: TransitionEvent(root_node), + //root_node_(root_node), + get_request_function_(get_request_function), + set_request_function_(set_request_function) +{ +} + +void ResetRequestEvent::callback( + BT::Duration /*timestamp*/, const BT::TreeNode & node, BT::NodeStatus /*prev_status*/, + BT::NodeStatus status) +{ + TransitionEvent::updateCurrentAction(status, node); + if (status == BT::NodeStatus::SUCCESS || status == BT::NodeStatus::FAILURE) { + if (getRequestString(get_request_function_()) == current_action_) { + set_request_function_(traffic_simulator::behavior::Request::NONE); + } + } +} +} // namespace context_gamma_planner diff --git a/simulation/context_gamma_planner/context_gamma_planner/src/transition_events/transition_event.cpp b/simulation/context_gamma_planner/context_gamma_planner/src/transition_events/transition_event.cpp new file mode 100644 index 00000000000..c0a9c8a9154 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/src/transition_events/transition_event.cpp @@ -0,0 +1,48 @@ +// Copyright 2015-2020 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +namespace context_gamma_planner +{ +TransitionEvent::TransitionEvent(BT::TreeNode * root_node) +//: root_node_(root_node) +{ + first_timestamp_ = std::chrono::high_resolution_clock::now(); + auto subscribeCallback = [this]( + BT::TimePoint timestamp, const BT::TreeNode & node, + BT::NodeStatus prev, BT::NodeStatus status) { + if (status != BT::NodeStatus::IDLE) { + if (type_ == BT::TimestampType::absolute) { + this->callback(timestamp.time_since_epoch(), node, prev, status); + } else { + this->callback(timestamp - first_timestamp_, node, prev, status); + } + } + }; + auto visitor = [this, subscribeCallback](BT::TreeNode * node) { + subscribers_.push_back(node->subscribeToStatusChange(std::move(subscribeCallback))); + }; + BT::applyRecursiveVisitor(root_node, visitor); +} + +void TransitionEvent::updateCurrentAction(const BT::NodeStatus & status, const BT::TreeNode & node) +{ + if (status != BT::NodeStatus::SUCCESS) { + current_action_ = node.name(); + } +} +} // namespace context_gamma_planner diff --git a/simulation/context_gamma_planner/context_gamma_planner/src/vehicle_plugin.cpp b/simulation/context_gamma_planner/context_gamma_planner/src/vehicle_plugin.cpp new file mode 100644 index 00000000000..75be91974ad --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/src/vehicle_plugin.cpp @@ -0,0 +1,281 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +namespace context_gamma_planner +{ +/** + * @brief Configures the VehiclePlugin. + * @details This function is responsible for configuring the VehiclePlugin. + * @param logger The logger object used for logging. + */ +void VehiclePlugin::configure(const rclcpp::Logger & logger) +{ + std::cout << "vehicle plugin configure start" << std::endl; + std::string path = ament_index_cpp::get_package_share_directory("context_gamma_planner") + + "/config/vehicle_behavior.xml"; + factory_.registerNodeType("FollowLane"); + factory_.registerNodeType("LaneChange"); + factory_.registerNodeType( + "FollowPolylineTrajectory"); + tree_ = createBehaviorTree(path); + logging_event_ptr_ = + std::make_shared(tree_.rootNode(), logger); + reset_request_event_ptr_ = std::make_shared( + tree_.rootNode(), [&]() { return getRequest(); }, + [&](traffic_simulator::behavior::Request request) { return setRequest(request); }); + setRequest(traffic_simulator::behavior::Request::NONE); + traffic_simulator_msgs::msg::Obstacle obstacle_msg; + obstacle_msg.type = traffic_simulator_msgs::msg::Obstacle::ENTITY; + setObstacle(std::nullopt); + std::cout << "configure end" << std::endl; +} + +/** + * @brief Creates a behavior tree from an XML file. + * @param format_path The path to the XML file. + * @return The created behavior tree. + */ +auto VehiclePlugin::createBehaviorTree(const std::string & format_path) -> BT::Tree +{ + auto xml_doc = pugi::xml_document(); + xml_doc.load_file(format_path.c_str()); + + class XMLTreeWalker : public pugi::xml_tree_walker + { + public: + explicit XMLTreeWalker(const BT::TreeNodeManifest & manifest) : manifest_(manifest) {} + + private: + bool for_each(pugi::xml_node & node) final + { + if (node.name() == manifest_.registration_ID) { + for (const auto & [port, info] : manifest_.ports) { + node.append_attribute(port.c_str()) = std::string("{" + port + "}").c_str(); + } + } + return true; + } + + const BT::TreeNodeManifest & manifest_; + }; + + for (const auto & [id, manifest] : factory_.manifests()) { + if (factory_.builtinNodes().count(id) == 0) { + auto walker = XMLTreeWalker(manifest); + xml_doc.traverse(walker); + } + } + + auto xml_str = std::stringstream(); + xml_doc.save(xml_str); + return factory_.createTreeFromText(xml_str.str()); +} + +/** + * @brief Get the current action of the vehicle. + * @return const std::string& The current action. + */ +const std::string & VehiclePlugin::getCurrentAction() const +{ + return logging_event_ptr_->getCurrentAction(); +} + +/** + * @brief Updates the vehicle plugin. + * @param current_time The current time. + * @param step_time The time step. + */ +void VehiclePlugin::update(double current_time, double step_time) +{ + tryInitializeConstraintActivator(); + tryInitializeEgoInRVO(); + + tickOnce(current_time, step_time); + while (getCurrentAction() == "root") { + tickOnce(current_time, step_time); + } + + if (getPlanningSpeed()) { + rvo_ego_->setMaxSpeed(getPlanningSpeed().value()); + } + + rvo_simulator_.deleteObstacles(); + rvo_simulator_.addGlobalObstacles(activator_ptr_->calculateRVOObstacles()); + rvo_simulator_.processObstacles(); + + auto ego_status = + static_cast(*this->getCanonicalizedEntityStatus()); + reflectEgoInRVO(ego_status); + reflectNonEgoEntitiesInRVO(); + rvo_simulator_.deleteUnavailableAgent(); + updateRVO(step_time); + updateSimulatorStatus(); + visualize(); + + /// @note cleanup constraints + activator_ptr_->deactivateAllConstraints(); +} + +void VehiclePlugin::tryInitializeConstraintActivator() +{ + if (activator_ptr_ == nullptr) { + activator_ptr_ = + std::make_shared( + getHdMapUtils(), getTrafficLights()); + setConstraintActivator(activator_ptr_); + } +} + +void VehiclePlugin::tryInitializeEgoInRVO() +{ + if (rvo_ego_ == nullptr) { + std::cout << "set agent conf start" << std::endl; + /// @todo This value should be set by entity parameters. + const mock::Catalogs catalogs; + RVO::AgentConfig vehicle_agent_config = + rvo_simulator_.createVehicleConfig(catalogs.getVehicleParameters()); + rvo_ego_ = std::make_shared( + getCanonicalizedEntityStatus()->getName(), getCanonicalizedEntityStatus()->getMapPose(), + vehicle_agent_config); + rvo_simulator_.addAgent(rvo_ego_); + std::cout << "set agent conf end" << std::endl; + } +} + +/** + * @brief a single tick of the behavior tree. + * @param current_time The current time. + * @param step_time The time step. + * @return The status of the behavior tree node. + */ +BT::NodeStatus VehiclePlugin::tickOnce(double current_time, double step_time) +{ + setCurrentTime(current_time); + setStepTime(step_time); + return tree_.rootNode()->executeTick(); +} + +void VehiclePlugin::reflectEgoInRVO(const traffic_simulator::EntityStatus & ego_status) +{ + rvo_ego_->is_available_ = true; + auto ego_pos = ego_status.pose.position; + rvo_ego_->setPosition(RVO::Vector2(ego_pos.x, ego_pos.y)); +} + +void VehiclePlugin::reflectNonEgoEntitiesInRVO() +{ + auto other_entity_status = getOtherEntityStatus(); + rvo_simulator_.markUnavailableAllAgent(); + rvo_ego_->is_available_ = true; + for (auto entity_status : other_entity_status) { + auto agent = rvo_simulator_.getAgent( + static_cast(entity_status.second).name); + if (agent) { + updateNonEgoEntityInRVO(agent, entity_status.second); + } else { + createNonEgoEntityInRVO(entity_status.second); + std::cout << "add new agent" << std::endl; + } + } +} + +void VehiclePlugin::updateNonEgoEntityInRVO( + std::shared_ptr agent, + const traffic_simulator::entity_status::CanonicalizedEntityStatus & canonicalized_entity_status) +{ + agent->is_available_ = true; + agent->setPose(canonicalized_entity_status.getMapPose()); + agent->setVelocity(RVO::Vector2( + canonicalized_entity_status.getTwist().linear.x, + canonicalized_entity_status.getTwist().linear.y)); + // we don't know other agent's navigation planning here. + // but, the agent velocity is reflecting its planner's decision in previous simulation step + // so, we set a waypoint in the direction of travel + agent->updateFrontWaypoint(agent->getPosition() + agent->getVelocity() * 10); +} + +void VehiclePlugin::createNonEgoEntityInRVO( + const traffic_simulator::entity_status::CanonicalizedEntityStatus & canonicalized_entity_status) +{ + auto entity_status = static_cast(canonicalized_entity_status); + RVO::AgentConfig other_entity_config; + const mock::Catalogs catalogs; + /// @todo This value should be set by entity parameters. + if (entity_status.type.type == traffic_simulator_msgs::msg::EntityType::VEHICLE) { + other_entity_config = rvo_simulator_.createVehicleConfig(catalogs.getVehicleParameters()); + } else if (entity_status.type.type == traffic_simulator_msgs::msg::EntityType::PEDESTRIAN) { + other_entity_config = rvo_simulator_.createPedestrianConfig(catalogs.getPedestrianParameters()); + } + auto new_agent = std::make_shared( + entity_status.name, canonicalized_entity_status.getMapPose(), other_entity_config); + rvo_simulator_.addAgent(new_agent); +} + +void VehiclePlugin::updateRVO(double step_time) +{ + auto next_goal = getNextGoal(); + rvo_ego_->updateFrontWaypoint(RVO::Vector2(next_goal.x, next_goal.y)); + rvo_simulator_.setTimeStep(step_time); + rvo_simulator_.update(rvo_ego_); +} + +void VehiclePlugin::updateSimulatorStatus() +{ + auto ego_status = + static_cast(*this->getCanonicalizedEntityStatus()); + ego_status.pose = rvo_ego_->getPose(); + ego_status.action_status.twist.linear.x = rvo_ego_->getVelocity().x(); + ego_status.action_status.twist.linear.y = rvo_ego_->getVelocity().y(); + + const auto candidate_on_route = + getHdMapUtils()->toLaneletPose(ego_status.pose, getRouteLanelets()); + const auto candidate_not_on_route = + getHdMapUtils()->toLaneletPose(ego_status.pose, getHdMapUtils()->getLaneletIds()); + if (candidate_on_route) { + ego_status.lanelet_pose_valid = true; + ego_status.lanelet_pose = candidate_on_route.value(); + } else if (candidate_not_on_route) { + ego_status.lanelet_pose_valid = true; + ego_status.lanelet_pose = candidate_not_on_route.value(); + } else { + ego_status.lanelet_pose_valid = false; + ego_status.lanelet_pose = traffic_simulator::LaneletPose(); + } + // std::cout << traffic_simulator_msgs::msg::to_yaml(ego_status) << std::endl; + getCanonicalizedEntityStatus()->set( + ego_status, getDefaultMatchingDistanceForLaneletPoseCalculation(), getHdMapUtils()); +} + +void VehiclePlugin::visualize() +{ + visualization_msgs::msg::MarkerArray marker_array; + rvo_visualize_.visualizeAllMarkers(marker_array, rvo_simulator_); + rvo_visualize_.visualizeNextGoal(marker_array, getNextGoal()); + for (auto && marker : marker_array.markers) { + marker.ns += "_" + getCanonicalizedEntityStatus()->getName(); + } + setDebugMarker(marker_array.markers); +} + +} // namespace context_gamma_planner + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(context_gamma_planner::VehiclePlugin, entity_behavior::BehaviorPluginBase) diff --git a/simulation/context_gamma_planner/context_gamma_planner/test/CMakeLists.txt b/simulation/context_gamma_planner/context_gamma_planner/test/CMakeLists.txt new file mode 100644 index 00000000000..0e154343f77 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/test/CMakeLists.txt @@ -0,0 +1,2 @@ +add_subdirectory(src/constraints) +add_subdirectory(src/plugins) \ No newline at end of file diff --git a/simulation/context_gamma_planner/context_gamma_planner/test/src/constraints/CMakeLists.txt b/simulation/context_gamma_planner/context_gamma_planner/test/src/constraints/CMakeLists.txt new file mode 100644 index 00000000000..d5b436e53c2 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/test/src/constraints/CMakeLists.txt @@ -0,0 +1,2 @@ +ament_add_gtest(test_constraint_activator_base test_constraint_activator_base.cpp) +target_link_libraries(test_constraint_activator_base context_gamma_planner) \ No newline at end of file diff --git a/simulation/context_gamma_planner/context_gamma_planner/test/src/constraints/test_constraint_activator_base.cpp b/simulation/context_gamma_planner/context_gamma_planner/test/src/constraints/test_constraint_activator_base.cpp new file mode 100644 index 00000000000..5e95e236672 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/test/src/constraints/test_constraint_activator_base.cpp @@ -0,0 +1,60 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include + +class ConstraintActivatorTest : public ::testing::Test +{ +protected: + static void SetUpTestCase() { rclcpp::init(0, nullptr); } + static void TearDownTestCase() { rclcpp::shutdown(); } + virtual void SetUp() + { + std::string path = ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", + "lanelet2_map.osm", geographic_msgs::msg::GeoPoint origin; + origin.latitude = 35.61836750154; + origin.longitude = 139.78066608243; + hdmap_utils_ptr = std::make_shared(path, origin); + const auto test_info = ::testing::UnitTest::GetInstance()->current_test_info(); + std::stringstream test_name; + test_name << test_info->test_case_name() << "_" << test_info->name(); + node = std::make_shared("node", test_name.str()); + manager = std::make_shared(hdmap_utils_ptr); + activator = std::make_unique( + hdmap_utils_ptr, manager); + } + virtual void TearDown() + { + // activator.release(); + node.reset(); + } + +public: + std::shared_ptr hdmap_utils_ptr; + std::unique_ptr activator; + std::shared_ptr manager; + rclcpp::Node::SharedPtr node; +}; + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/simulation/context_gamma_planner/context_gamma_planner/test/src/plugins/CMakeLists.txt b/simulation/context_gamma_planner/context_gamma_planner/test/src/plugins/CMakeLists.txt new file mode 100644 index 00000000000..af247f37973 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/test/src/plugins/CMakeLists.txt @@ -0,0 +1,2 @@ +ament_add_gtest(test_pedestrian_plugin test_pedestrian_plugin.cpp) +target_link_libraries(test_pedestrian_plugin context_gamma_planner) \ No newline at end of file diff --git a/simulation/context_gamma_planner/context_gamma_planner/test/src/plugins/test_pedestrian_plugin.cpp b/simulation/context_gamma_planner/context_gamma_planner/test/src/plugins/test_pedestrian_plugin.cpp new file mode 100644 index 00000000000..589d2c9f6f6 --- /dev/null +++ b/simulation/context_gamma_planner/context_gamma_planner/test/src/plugins/test_pedestrian_plugin.cpp @@ -0,0 +1,173 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +class PedestrianPluginTest : public ::testing::Test +{ +protected: + virtual void SetUp() + { + plugin = std::make_unique(); + plugin->configure(rclcpp::get_logger("gtest")); + } + virtual void TearDown() { plugin.release(); } + +public: + std::unique_ptr plugin; +}; + +TEST_F(PedestrianPluginTest, CurrentTime) +{ + plugin->setCurrentTime(3.4); + EXPECT_DOUBLE_EQ(plugin->getCurrentTime(), 3.4); +} + +TEST_F(PedestrianPluginTest, DebugMarker) +{ + std::vector markers; + visualization_msgs::msg::Marker marker; + marker.scale.x = 3.0; + marker.colors.emplace_back(std_msgs::msg::ColorRGBA()); + marker.action = marker.ADD; + marker.type = marker.TRIANGLE_LIST; + marker.text = "test"; + markers.emplace_back(marker); + plugin->setDebugMarker(markers); + EXPECT_EQ(plugin->getDebugMarker().size(), static_cast(1)); + EXPECT_EQ(plugin->getDebugMarker()[0], marker); +} + +TEST_F(PedestrianPluginTest, BehaviorParameter) +{ + traffic_simulator_msgs::msg::BehaviorParameter model; + model.dynamic_constraints.max_acceleration = 31.3; + model.dynamic_constraints.max_deceleration = 20.1; + model.lane_change_distance = 30.4; + model.see_around = false; + plugin->setBehaviorParameter(model); + EXPECT_EQ(plugin->getBehaviorParameter(), model); +} + +TEST_F(PedestrianPluginTest, HdMapUtils) +{ + std::string path = ament_index_cpp::get_package_share_directory("context_gamma_planner") + + "/maps/lanelet2_map.osm"; + geographic_msgs::msg::GeoPoint origin; + origin.latitude = 35.61836750154; + origin.longitude = 139.78066608243; + const auto hdmap_utils_ptr = std::make_shared(path, origin); + plugin->setHdMapUtils(hdmap_utils_ptr); + EXPECT_TRUE(plugin->getHdMapUtils()); +} + +TEST_F(PedestrianPluginTest, Obstacle) +{ + plugin->setObstacle(std::nullopt); + EXPECT_TRUE(plugin->getObstacle() == std::nullopt); + plugin->setObstacle(traffic_simulator_msgs::msg::Obstacle()); + EXPECT_EQ(plugin->getObstacle(), traffic_simulator_msgs::msg::Obstacle()); +} + +TEST_F(PedestrianPluginTest, PedestrianParameters) +{ + traffic_simulator_msgs::msg::PedestrianParameters param; + param.bounding_box.center.x = 20.0; + param.name = "ego"; + plugin->setPedestrianParameters(param); + EXPECT_EQ(plugin->getPedestrianParameters(), param); +} + +TEST_F(PedestrianPluginTest, Requests) +{ + plugin->setRequest(traffic_simulator::behavior::Request::WALK_STRAIGHT); + EXPECT_EQ(plugin->getRequest(), traffic_simulator::behavior::Request::WALK_STRAIGHT); +} + +TEST_F(PedestrianPluginTest, RouteLanelets) +{ + std::vector route = {1, 32, 31}; + plugin->setRouteLanelets(route); + EXPECT_EQ(plugin->getRouteLanelets()[0], 1); + EXPECT_EQ(plugin->getRouteLanelets()[1], 32); + EXPECT_EQ(plugin->getRouteLanelets()[2], 31); +} + +TEST_F(PedestrianPluginTest, TargetSpeed) +{ + plugin->setTargetSpeed(std::nullopt); + EXPECT_EQ(plugin->getTargetSpeed(), std::nullopt); + plugin->setTargetSpeed(15); + EXPECT_DOUBLE_EQ(plugin->getTargetSpeed().value(), 15); +} + +TEST_F(PedestrianPluginTest, StepTime) +{ + plugin->setStepTime(0.01); + EXPECT_DOUBLE_EQ(plugin->getStepTime(), 0.01); +} + +TEST_F(PedestrianPluginTest, LaneChangeParameters) +{ + plugin->setLaneChangeParameters(traffic_simulator::lane_change::Parameter()); + EXPECT_EQ( + plugin->getLaneChangeParameters().trajectory_shape, + traffic_simulator::lane_change::TrajectoryShape::CUBIC); +} + +/* +TEST_F(PedestrianPluginTest, TrafficLightManager) +{ + std::string path = ament_index_cpp::get_package_share_directory("context_gamma_planner") + + "/maps/lanelet2_map.osm"; + geographic_msgs::msg::GeoPoint origin; + origin.latitude = 35.61836750154; + origin.longitude = 139.78066608243; + const auto hdmap_utils_ptr = std::make_shared(path, origin); + const auto manager = std::make_shared( + hdmap_utils_ptr, nullptr, nullptr, nullptr); + plugin->setTrafficLightManager(manager); + EXPECT_EQ(plugin->getTrafficLightManager()->getIds().size(), static_cast(2)); +} +*/ + +TEST_F(PedestrianPluginTest, VehicleParameters) +{ + traffic_simulator_msgs::msg::VehicleParameters parameters; + parameters.bounding_box.center.x = 4.2; + plugin->setVehicleParameters(parameters); + EXPECT_EQ(plugin->getVehicleParameters(), parameters); + EXPECT_DOUBLE_EQ(plugin->getVehicleParameters().bounding_box.center.x, 4.2); +} + +TEST_F(PedestrianPluginTest, Waypoint) +{ + traffic_simulator_msgs::msg::WaypointsArray waypoints; + geometry_msgs::msg::Point p0; + geometry_msgs::msg::Point p1; + p1.x = 1; + waypoints.waypoints.emplace_back(p0); + waypoints.waypoints.emplace_back(p1); + plugin->setWaypoints(waypoints); + EXPECT_EQ(plugin->getWaypoints(), waypoints); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/simulation/context_gamma_planner/customized_rvo2/CMakeLists.txt b/simulation/context_gamma_planner/customized_rvo2/CMakeLists.txt new file mode 100644 index 00000000000..5c39aec8768 --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 3.5) +project(customized_rvo2) + +# find dependencies +find_package(ament_cmake_auto REQUIRED) +#find_package(OpenMP REQUIRED) +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fPIC") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC") +ament_auto_find_build_dependencies() + +set(RVO_SOURCES + src/Math.cpp + src/Obstacle.cpp + src/Agent.cpp + src/KdTree.cpp + src/RVOSimulator.cpp + src/VisualizeMarker.cpp + src/Plugins/ObstaclePlugin.cpp + src/Plugins/AgentPlugin.cpp) + +ament_auto_add_library(${PROJECT_NAME} SHARED ${RVO_SOURCES}) + +if(${tf2_geometry_msgs_VERSION} VERSION_LESS 0.18.0) + target_compile_definitions(${PROJECT_NAME} PUBLIC + USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER + ) +endif() + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(test_rvo2 test/src/test_rvo2.cpp) + target_link_libraries(test_rvo2 ${PROJECT_NAME}) +endif() + +ament_auto_package() diff --git a/simulation/context_gamma_planner/customized_rvo2/README.md b/simulation/context_gamma_planner/customized_rvo2/README.md new file mode 100644 index 00000000000..488967fe1ea --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2/README.md @@ -0,0 +1,36 @@ +## How to implement ORCA Plugin +### Choose Plugin type +There are 2 types of plugin as below: +- Obstacle ORCA Plugin +- Agent ORCA Plugin + +Usually use the Obstacle plugin. + +### Implement Plugin Class +- Make plugin class + - Inherit base class in `include/Plugins/OrcaPlugin.h` + - `OrcaAgentPluginBase` class + - `OrcaObstaclePluginBase` class + - Implement Constructor method + - Call the constructor method of the base class with plugin name. + - Implement `calcOrcaLines` method (override) + - Note that the arguments differ depending on the type of plugin + - Add the activation implementation at the beggining of the method as follows: +```c++ +std::vector HogePlugin::calcOrcaLines(...) +{ + std::vector orca_lines; + // skip if inactive + if(!is_active_){ + return orca_lines; + } + // plugin implementation +} +``` + +### Use custom plugins +- Make instance of the custom plugin as a shared pointer +- Add plugins to agent using methods below: + - `Agent::addAgentORCAPlugin` + - `Agent::addObstacleORCAPlugin` +- You can switch activation with `activate` or `deactivate` methods \ No newline at end of file diff --git a/simulation/context_gamma_planner/customized_rvo2/include/customized_rvo2/Agent.hpp b/simulation/context_gamma_planner/customized_rvo2/include/customized_rvo2/Agent.hpp new file mode 100644 index 00000000000..e4502619cf6 --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2/include/customized_rvo2/Agent.hpp @@ -0,0 +1,208 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CUSTOMIZED_RVO2__AGENT_HPP_ +#define CUSTOMIZED_RVO2__AGENT_HPP_ +#include +#include +#include +#include +#include +#include +#include +#include + +#include "customized_rvo2/KdTree.h" +#include "customized_rvo2/Plugins/OrcaPlugin.h" +#include "geometry_msgs/msg/pose.hpp" + +namespace RVO +{ +struct AgentConfig +{ + /// @brief Type of entity (VEHICLE or PEDESTRIAN) + uint8_t entity_type = traffic_simulator_msgs::msg::EntityType::VEHICLE; + /// @brief long radius + float radius = 2.0f; + /// @brief ellipticity = short radius / long radius (0 < ellipticity <= 1.0) + /// short radius = ellipticity * long radius + float ellipticity = 1.0f; + /// @brief vehicle base_link to circle center + float base_link_to_center = 0.0f; + /// @brief Agent's maximum speed [m/s] + float max_speed = 2.0f; + /// @brief Agent's maximum acceleration [m/s^2] + float max_acceleration = 100.0f; + /// @brief Agent's maximum deceleration [m/s^2] + float max_deceleration = 100.0f; + /// @brief Distance of obstacles to be considered in the calculation + float neighbor_dist = 15.0f; + /// @brief Number of obstacles considered in the calculation + size_t max_neighbors = 10; + /// @brief Time for dynamic obstacles to be included in the prediction calculation + float time_horizon = 5.0f; + /// @brief Time for static obstacles to be included in the prediction calculation + float time_horizon_obst = 5.0f; +}; + +struct AgentPose +{ + Vector2 position; + geometry_msgs::msg::Quaternion orientation; +}; + +class RVOSimulator; +class Agent +{ +public: + /** + * @brief Constructor for the Agent class. + * + * @param name The name of the agent. + * @param pos The initial position of the agent. + * @param config The configuration settings for the agent. + * @param orientation The initial orientation of the agent. + */ + explicit Agent( + const std::string & name, const RVO::Vector2 & pos, const AgentConfig & config = AgentConfig(), + double orientation = 0.0); + + /** + * @brief Constructor for the Agent class. + * + * @param name The name of the agent. + * @param pose The initial pose of the agent. + * @param config The configuration for the agent. + */ + explicit Agent( + const std::string & name, const geometry_msgs::msg::Pose & pose, + const AgentConfig & config = AgentConfig()); + + const std::string name; + using SharedPtr = std::shared_ptr; + + /** + * @brief Adds a list of waypoints to the agent's existing waypoints. + * + * @param wps The list of waypoints to be added. + */ + void addWaypoint(const RVO::Vector2 & wp) { waypoints_.emplace_back(wp); } + + /** + * @brief Updates the front waypoint of the agent. + * + * If the agent's waypoints list is empty, the given waypoint is added to the list. + * Otherwise, the first waypoint in the list is updated with the given waypoint. + * + * @param wp The new front waypoint. + */ + void updateFrontWaypoint(const RVO::Vector2 & wp); + void addWaypoints(const std::vector & wps); + + /** + * @brief Updates the agent's simple navigation based on the current waypoints. + * + * This function calculates the goal vector by subtracting the agent's current position from the first waypoint. + * If the squared magnitude of the goal vector is greater than 1.0, it is normalized. + * If there are more than one waypoints, the first waypoint is removed from the list. + * The preferred velocity is then calculated by multiplying the goal vector with the maximum speed. + */ + void updateSimpleNavigation(); + + /** + * @brief Updates the agent's state based on the given ORCA lines and time step. + * + * This function calculates the preferred velocity for the agent using the updateSimpleNavigation() function. + * It then calculates the ORCA lines for obstacles and other agents using the provided vectors. + * The ORCA lines are used to compute the new velocity for the agent. + * The agent's state is updated based on the new velocity and the given time step. + * + * @param orca_lines_obst The ORCA lines for obstacles. + * @param orca_lines_agent The ORCA lines for other agents. + * @param time_step_s The time step in seconds. + * @param stop_velocity_threshold [m/s] The threshold for stopping velocity. + * If the velocity is less than this value, the velocity is set to 0. + * The default value is small enough to prevent chattering. + */ + void update( + const std::vector & orca_lines_obst, const std::vector & orca_lines_agent, + double time_step_s, double stop_velocity_threshold = 0.1); + + /** + * @brief the new velocity for an agent based on the given agent lines, obstacle lines, and number of obstacle lines. + * + * @param agent_lines The vector of agent lines. + * @param obstacle_lines The vector of obstacle lines. + * @param numObstLines The number of obstacle lines. + * @return The computed velocity as a Vector2 object. + */ + Vector2 computeNewVelocity( + const std::vector & agent_lines, const std::vector & obstacle_lines, + int numObstLines) const; + + // getter setter + const Vector2 & getPrefVelocity() const; + const size_t & getId() const; + void setId(const size_t & id); + /// @brief Returns the base_link position of the agent. + Vector2 getPosition() const; + /// @brief set the base_link position of the agent. + void setPosition(const Vector2 & position); + const Vector2 & getCenterPosition() const; + const Vector2 & getVelocity() const; + void setVelocity(const Vector2 & velocity) { velocity_ = velocity; } + float getOrientationYaw() const; + const geometry_msgs::msg::Quaternion & getOrientation() const; + void setOrientation(const float & orientation); + void setOrientation(const geometry_msgs::msg::Quaternion & quat); + void setPose(const geometry_msgs::msg::Pose & pose); + geometry_msgs::msg::Pose getPose() const; + void setMaxSpeed(const float & max_speed); + const float & getMaxSpeed() const; + void setMaxAcceleration(const float & max_acceleration); + const float & getMaxAcceleration() const; + void setMaxDeceleration(const float & max_deceleration); + const float & getMaxDeceleration() const; + const AgentConfig & getAgentConfig() const; + std::vector getObstacleLines(); + void addIgnoreList(uint8_t entity_type) { ignore_list_.emplace_back(entity_type); }; + const auto & getIgnoreList() { return ignore_list_; } + + /// @brief A vector of RVO::Line objects representing obstacle ORCA lines.(static obstacles) + std::vector obst_orca_lines_; + + /// @brief A vector of RVO::Line objects representing agent ORCA lines.(dynamic obstacles) + std::vector agent_orca_lines_; + bool is_available_ = true; + + /** + * @brief Prints the status of the agent. + * + * This function prints the name, type, position, yaw angle, and velocity of the agent. + */ + void printStatus(); + +private: + std::deque waypoints_; + + AgentConfig config_; + AgentPose pose_; + Vector2 new_velocity_; + Vector2 pref_velocity_; + Vector2 velocity_; + size_t id_; + std::vector ignore_list_; +}; +} // namespace RVO +#endif // CUSTOMIZED_RVO2__AGENT_HPP_ diff --git a/simulation/context_gamma_planner/customized_rvo2/include/customized_rvo2/KdTree.h b/simulation/context_gamma_planner/customized_rvo2/include/customized_rvo2/KdTree.h new file mode 100644 index 00000000000..3b962da3ec2 --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2/include/customized_rvo2/KdTree.h @@ -0,0 +1,177 @@ +/* + * KdTree.h + * RVO2 Library + * + * Copyright 2008 University of North Carolina at Chapel Hill + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + * + * modified by Kotaro Yoshimoto + */ + +#ifndef CUSTOMIZED_RVO2__KDTREE_H_ +#define CUSTOMIZED_RVO2__KDTREE_H_ + +/** + * \file KdTree.h + * \brief Contains the KdTree class. + */ + +#include +#include +#include +#include + +#include "Math.h" +#include "Obstacle.h" + +namespace RVO +{ +class Agent; + +/** + * \brief Defines kd-trees for agents and static obstacles in the + * simulation. + */ + +class AgentKdTree +{ +public: + class AgentTreeNode + { + public: + size_t begin; + size_t end; + size_t left; + float max_x; + float max_y; + float min_x; + float min_y; + size_t right; + }; + AgentKdTree(); + + /** + * @brief This function clears the existing agent tree and rebuilds it based on the new list of agents. + * @param all_agents A reference to an unordered map containing all the agents. + */ + void rebuildAgentTree(std::unordered_map> & all_agents); + //void updateAgentTree(std::unordered_map> & all_agents); + + /** + * @brief builds the agent tree for the given range of agents. + * @param begin The index of the first agent in the range. + * @param end The index of the last agent in the range (exclusive). + * @param node The index of the current node in the agent tree. + */ + void buildAgentTreeRecursive(size_t begin, size_t end, size_t node); + + /** + * @brief Computes the neighboring agents within a specified range of a given position. + * @param position The position of the agent. + * @param range The range within which to search for neighboring agents. + * @return A vector of pairs, where each pair contains the distance to the agent and a shared pointer to the agent. + */ + std::vector>> computeAgentNeighbors( + const RVO::Vector2 position, const float range) const; + + /** + * @brief queries the agent tree to find agents within a specified range from a given position. + * @param position The position from which to query. + * @param range_sq The squared range within which to search for agents. + * @param node The index of the current node in the agent tree. + * @param ret A vector of pairs containing the squared range and shared pointers to the found agents. + */ + void queryAgentTreeRecursive( + RVO::Vector2 position, float & range_sq, size_t node, + std::vector>> & ret) const; + + std::vector> agents_; + std::vector agent_tree_; + + static const size_t MAX_LEAF_SIZE = 10; + + friend class Agent; +}; + +class ObstacleKdTree +{ +public: + class ObstacleTreeNode + { + public: + ObstacleTreeNode * left; + std::shared_ptr obstacle; + ObstacleTreeNode * right; + }; + + ObstacleKdTree(); + + /** + * @brief Builds the obstacle tree using the given list of obstacles. + * @param obstacles The list of obstacles to build the tree from. + */ + void buildObstacleTree(std::vector & obstacles); + + /** + * @brief Deletes the obstacle tree. + * @note The obstacle tree must be created before calling this function. + */ + void deleteObstacleTree(); + + /** + * @brief the obstacle neighbors within a specified range from a given position. + * @param position The position from which to compute the obstacle neighbors. + * @param range The range within which to search for obstacle neighbors. + * @return A vector of pairs, where each pair contains the distance to an obstacle neighbor and a shared pointer to the obstacle. + */ + std::vector> computeObstacleNeighbors( + RVO::Vector2 position, float & range) const; + +private: + ObstacleTreeNode * buildObstacleTreeRecursive( + std::vector & obstacles_node, + std::vector & obstacles_original); + void queryObstacleTreeRecursive( + RVO::Vector2 position, float & range_sq, const ObstacleTreeNode * node, + std::vector> & ret) const; + + void deleteObstacleTree(ObstacleTreeNode * node); + + bool queryVisibility(const Vector2 & q1, const Vector2 & q2, float radius) const; + + bool queryVisibilityRecursive( + const Vector2 & q1, const Vector2 & q2, float radius, const ObstacleTreeNode * node) const; + ObstacleTreeNode * obstacle_tree_; + static const size_t MAX_LEAF_SIZE = 10; + + friend class Agent; + bool delete_flag_ = false; +}; + +} // namespace RVO + +#endif // CUSTOMIZED_RVO2__KDTREE_H_ diff --git a/simulation/context_gamma_planner/customized_rvo2/include/customized_rvo2/Math.h b/simulation/context_gamma_planner/customized_rvo2/include/customized_rvo2/Math.h new file mode 100644 index 00000000000..2504548e61f --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2/include/customized_rvo2/Math.h @@ -0,0 +1,468 @@ +/* + * Math.h + * copied and modified from Vector2.h in RVO2 Library + * + * Copyright 2008 University of North Carolina at Chapel Hill + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + * + * modified by Kotaro Yoshimoto + */ + +#ifndef CUSTOMIZED_RVO2__MATH_H_ +#define CUSTOMIZED_RVO2__MATH_H_ + +/** + * \file Math.h + * \brief Contains the Vector2 class. + */ + +#include +#include +#include +#include +#include +#include + +/** + * \brief A sufficiently small positive number. + */ +const float RVO_EPSILON = 0.00001f; + +namespace RVO +{ +/** + * \brief Error value. + * + * A value equal to the largest unsigned integer that is returned in case + * of an error by functions in RVO::RVOSimulator. + */ +const size_t RVO_ERROR = std::numeric_limits::max(); + +/** + * \brief Defines a two-dimensional vector. + */ +class Vector2 +{ +public: + /** + * \brief Constructs and initializes a two-dimensional vector instance + * to (0.0, 0.0). + */ + inline Vector2() : x_(0.0f), y_(0.0f) {} + + /** + * \brief Constructs and initializes a two-dimensional vector from + * the specified xy-coordinates. + * \param x The x-coordinate of the two-dimensional + * vector. + * \param y The y-coordinate of the two-dimensional + * vector. + */ + inline Vector2(float x, float y) : x_(x), y_(y) {} + + /** + * \brief Returns the x-coordinate of this two-dimensional vector. + * \return The x-coordinate of the two-dimensional vector. + */ + inline float x() const { return x_; } + + /** + * \brief Returns the y-coordinate of this two-dimensional vector. + * \return The y-coordinate of the two-dimensional vector. + */ + inline float y() const { return y_; } + + /** + * \brief Computes the negation of this two-dimensional vector. + * \return The negation of this two-dimensional vector. + */ + inline Vector2 operator-() const { return Vector2(-x_, -y_); } + + /** + * \brief Computes the dot product of this two-dimensional vector with + * the specified two-dimensional vector. + * \param vector The two-dimensional vector with which the + * dot product should be computed. + * \return The dot product of this two-dimensional vector with a + * specified two-dimensional vector. + */ + inline float operator*(const Vector2 & vector) const { return x_ * vector.x() + y_ * vector.y(); } + + /** + * \brief Computes the scalar multiplication of this + * two-dimensional vector with the specified scalar value. + * \param s The scalar value with which the scalar + * multiplication should be computed. + * \return The scalar multiplication of this two-dimensional vector + * with a specified scalar value. + */ + inline Vector2 operator*(float s) const { return Vector2(x_ * s, y_ * s); } + + /** + * \brief Computes the scalar division of this two-dimensional vector + * with the specified scalar value. + * \param s The scalar value with which the scalar + * division should be computed. + * \return The scalar division of this two-dimensional vector with a + * specified scalar value. + */ + inline Vector2 operator/(float s) const + { + const float invS = 1.0f / s; + + return Vector2(x_ * invS, y_ * invS); + } + + /** + * \brief Computes the vector sum of this two-dimensional vector with + * the specified two-dimensional vector. + * \param vector The two-dimensional vector with which the + * vector sum should be computed. + * \return The vector sum of this two-dimensional vector with a + * specified two-dimensional vector. + */ + inline Vector2 operator+(const Vector2 & vector) const + { + return Vector2(x_ + vector.x(), y_ + vector.y()); + } + + /** + * \brief Computes the vector difference of this two-dimensional + * vector with the specified two-dimensional vector. + * \param vector The two-dimensional vector with which the + * vector difference should be computed. + * \return The vector difference of this two-dimensional vector with a + * specified two-dimensional vector. + */ + inline Vector2 operator-(const Vector2 & vector) const + { + return Vector2(x_ - vector.x(), y_ - vector.y()); + } + + /** + * \brief Tests this two-dimensional vector for equality with the + * specified two-dimensional vector. + * \param vector The two-dimensional vector with which to + * test for equality. + * \return True if the two-dimensional vectors are equal. + */ + inline bool operator==(const Vector2 & vector) const + { + return x_ == vector.x() && y_ == vector.y(); + } + + /** + * \brief Tests this two-dimensional vector for inequality with the + * specified two-dimensional vector. + * \param vector The two-dimensional vector with which to + * test for inequality. + * \return True if the two-dimensional vectors are not equal. + */ + inline bool operator!=(const Vector2 & vector) const + { + return x_ != vector.x() || y_ != vector.y(); + } + + /** + * \brief Sets the value of this two-dimensional vector to the scalar + * multiplication of itself with the specified scalar value. + * \param s The scalar value with which the scalar + * multiplication should be computed. + * \return A reference to this two-dimensional vector. + */ + inline Vector2 & operator*=(float s) + { + x_ *= s; + y_ *= s; + + return *this; + } + + /** + * \brief Sets the value of this two-dimensional vector to the scalar + * division of itself with the specified scalar value. + * \param s The scalar value with which the scalar + * division should be computed. + * \return A reference to this two-dimensional vector. + */ + inline Vector2 & operator/=(float s) + { + const float invS = 1.0f / s; + x_ *= invS; + y_ *= invS; + + return *this; + } + + /** + * \brief Sets the value of this two-dimensional vector to the vector + * sum of itself with the specified two-dimensional vector. + * \param vector The two-dimensional vector with which the + * vector sum should be computed. + * \return A reference to this two-dimensional vector. + */ + inline Vector2 & operator+=(const Vector2 & vector) + { + x_ += vector.x(); + y_ += vector.y(); + + return *this; + } + + /** + * \brief Sets the value of this two-dimensional vector to the vector + * difference of itself with the specified two-dimensional + * vector. + * \param vector The two-dimensional vector with which the + * vector difference should be computed. + * \return A reference to this two-dimensional vector. + */ + inline Vector2 & operator-=(const Vector2 & vector) + { + x_ -= vector.x(); + y_ -= vector.y(); + + return *this; + } + + /** + * \brief Computes vector rotation + * \param angle rotate angle [rad] + * \return compute vector + */ + inline Vector2 rotate(float angle) + { + return Vector2(cos(angle) * x_ - sin(angle) * y_, sin(angle) * x_ + cos(angle) * y_); + } + +private: + float x_; + float y_; +}; + +/** + * \relates Vector2 + * \brief Computes the scalar multiplication of the specified + * two-dimensional vector with the specified scalar value. + * \param s The scalar value with which the scalar + * multiplication should be computed. + * \param vector The two-dimensional vector with which the scalar + * multiplication should be computed. + * \return The scalar multiplication of the two-dimensional vector with the + * scalar value. + */ +inline Vector2 operator*(float s, const Vector2 & vector) +{ + return Vector2(s * vector.x(), s * vector.y()); +} + +/** + * \relates Vector2 + * \brief Inserts the specified two-dimensional vector into the specified + * output stream. + * \param os The output stream into which the two-dimensional + * vector should be inserted. + * \param vector The two-dimensional vector which to insert into + * the output stream. + * \return A reference to the output stream. + */ +inline std::ostream & operator<<(std::ostream & os, const Vector2 & vector) +{ + os << "(" << vector.x() << "," << vector.y() << ")"; + + return os; +} + +/** + * \relates Vector2 + * \brief Computes the length of a specified two-dimensional vector. + * \param vector The two-dimensional vector whose length is to be + * computed. + * \return The length of the two-dimensional vector. + */ +inline float abs(const Vector2 & vector) { return std::sqrt(vector * vector); } + +/** + * \relates Vector2 + * \brief Computes the squared length of a specified two-dimensional + * vector. + * \param vector The two-dimensional vector whose squared length + * is to be computed. + * \return The squared length of the two-dimensional vector. + */ +inline float absSq(const Vector2 & vector) { return vector * vector; } + +/** + * \relates Vector2 + * \brief Computes the determinant of a two-dimensional square matrix with + * rows consisting of the specified two-dimensional vectors. + * \param vector1 The top row of the two-dimensional square + * matrix. + * \param vector2 The bottom row of the two-dimensional square + * matrix. + * \return The determinant of the two-dimensional square matrix. + */ +inline float det(const Vector2 & vector1, const Vector2 & vector2) +{ + return vector1.x() * vector2.y() - vector1.y() * vector2.x(); +} + +/** + * \relates Vector2 + * \brief Computes the inner product of a two-dimensional square matrix with + * rows consisting of the specified two-dimensional vectors. + * \param vector1 The top row of the two-dimensional square + * matrix. + * \param vector2 The bottom row of the two-dimensional square + * matrix. + * \return The inner product of the two-dimensional square matrix. + */ +inline float dot(const Vector2 & vector1, const Vector2 & vector2) +{ + return vector1.x() * vector2.x() + vector1.y() * vector2.y(); +} + +/** + * \relates Vector2 + * \brief Computes the normalization of the specified two-dimensional + * vector. + * \param vector The two-dimensional vector whose normalization + * is to be computed. + * \return The normalization of the two-dimensional vector. + */ +inline Vector2 normalize(const Vector2 & vector) { return vector / abs(vector); } + +/** + * \brief Defines a directed line. + */ +class Line +{ +public: + /** + * \brief A point on the directed line. + */ + Vector2 point; + + /** + * \brief The direction of the directed line. + */ + Vector2 direction; +}; + +/** + * \brief Computes the squared distance from a line segment with the + * specified endpoints to a specified point. + * \param a The first endpoint of the line segment. + * \param b The second endpoint of the line segment. + * \param c The point to which the squared distance is to + * be calculated. + * \return The squared distance from the line segment to the point. + */ +inline float distSqPointLineSegment(const Vector2 & a, const Vector2 & b, const Vector2 & c) +{ + const float r = ((c - a) * (b - a)) / absSq(b - a); + + if (r < 0.0f) { + return absSq(c - a); + } else if (r > 1.0f) { + return absSq(c - b); + } else { + return absSq(c - (a + r * (b - a))); + } +} + +/** + * \brief Computes the signed distance from a line connecting the + * specified points to a specified point. + * \param a The first point on the line. + * \param b The second point on the line. + * \param c The point to which the signed distance is to + * be calculated. + * \return Positive when the point c lies to the left of the line ab. + */ +inline float leftOf(const Vector2 & a, const Vector2 & b, const Vector2 & c) +{ + return det(a - c, b - a); +} + +/** + * \brief Computes the square of a float. + * \param a The float to be squared. + * \return The square of the float. + */ +inline float sqr(float a) { return a * a; } + +/** + * \relates Agent + * \brief Solves a one-dimensional linear program on a specified line + * subject to linear constraints defined by lines and a circular + * constraint. + * \param lines Lines defining the linear constraints. + * \param line_no The specified line constraint. + * \param radius The radius of the circular constraint. + * \param opt_velocity The optimization velocity. + * \param direction_opt True if the direction should be optimized. + * \param result A reference to the result of the linear program. + * \return True if successful. + */ +bool linearProgram1( + const std::vector & lines, size_t line_no, float radius, const Vector2 & opt_velocity, + bool direction_opt, Vector2 & result); + +/** + * \relates Agent + * \brief Solves a two-dimensional linear program subject to linear + * constraints defined by lines and a circular constraint. + * \param lines Lines defining the linear constraints. + * \param radius The radius of the circular constraint. + * \param opt_velocity The optimization velocity. + * \param direction_opt True if the direction should be optimized. + * \param result A reference to the result of the linear program. + * \return The number of the line it fails on, and the number of lines if + * successful. + */ +size_t linearProgram2( + const std::vector & lines, float radius, const Vector2 & opt_velocity, bool direction_opt, + Vector2 & result); + +/** + * \relates Agent + * \brief Solves a two-dimensional linear program subject to linear + * constraints defined by agent_lines and a circular constraint. + * \param lines Lines defining the linear constraints. + * \param num_obst_lines Count of obstacle agent_lines. + * \param begin_line The line on which the 2-d linear program failed. + * \param radius The radius of the circular constraint. + * \param result A reference to the result of the linear program. + */ +void linearProgram3( + const std::vector & lines, size_t num_obst_lines, size_t begin_line, float radius, + Vector2 & result); + +} // namespace RVO + +#endif // CUSTOMIZED_RVO2__MATH_H_ diff --git a/simulation/context_gamma_planner/customized_rvo2/include/customized_rvo2/Obstacle.h b/simulation/context_gamma_planner/customized_rvo2/include/customized_rvo2/Obstacle.h new file mode 100644 index 00000000000..4e632791abe --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2/include/customized_rvo2/Obstacle.h @@ -0,0 +1,72 @@ +/* + * Obstacle.h + * RVO2 Library + * + * Copyright 2008 University of North Carolina at Chapel Hill + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + * + * modified by Kotaro Yoshimoto + */ + +#ifndef CUSTOMIZED_RVO2__OBSTACLE_H_ +#define CUSTOMIZED_RVO2__OBSTACLE_H_ + +/** + * \file Obstacle.h + * \brief Contains the Obstacle class. + */ + +#include + +#include "Math.h" + +namespace RVO +{ +/** + * \brief Defines static obstacles in the simulation. + */ +class Obstacle +{ +public: + using SharedPtr = std::shared_ptr; + /** + * \brief Constructs a static obstacle instance. + */ + Obstacle(); + Vector2 point_; + Obstacle::SharedPtr next_obstacle_ = nullptr; + Obstacle::SharedPtr prev_obstacle_ = nullptr; + Vector2 unit_dir_; + bool is_convex_; + size_t id_; + + friend class AgentKdTree; + friend class ObstacleKdTree; +}; +} // namespace RVO + +#endif // CUSTOMIZED_RVO2__OBSTACLE_H_ diff --git a/simulation/context_gamma_planner/customized_rvo2/include/customized_rvo2/Plugins/AgentPlugin.h b/simulation/context_gamma_planner/customized_rvo2/include/customized_rvo2/Plugins/AgentPlugin.h new file mode 100644 index 00000000000..30bcdfad8ea --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2/include/customized_rvo2/Plugins/AgentPlugin.h @@ -0,0 +1,31 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CUSTOMIZED_RVO2__PLUGINS__AGENT_PLUGIN_H +#define CUSTOMIZED_RVO2__PLUGINS__AGENT_PLUGIN_H + +#include "OrcaPlugin.h" + +class AgentPlugin : public OrcaAgentPluginBase +{ +public: + AgentPlugin() : OrcaAgentPluginBase("AgentPlugin") {} + std::vector calcOrcaLines( + std::shared_ptr agent, std::shared_ptr agent_kdtree, + double time_step_s) override; + using AgentNeighborList = std::vector>>; + void filterAgentNeighbors(std::shared_ptr agent, AgentNeighborList & neighbor_list); +}; + +#endif // CUSTOMIZED_RVO2__PLUGINS__AGENT_PLUGIN_H diff --git a/simulation/context_gamma_planner/customized_rvo2/include/customized_rvo2/Plugins/ObstaclePlugin.h b/simulation/context_gamma_planner/customized_rvo2/include/customized_rvo2/Plugins/ObstaclePlugin.h new file mode 100644 index 00000000000..51609874682 --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2/include/customized_rvo2/Plugins/ObstaclePlugin.h @@ -0,0 +1,38 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CUSTOMIZED_RVO2__PLUGINS__GLOBAL_OBSTACLE_PLUGIN_H +#define CUSTOMIZED_RVO2__PLUGINS__GLOBAL_OBSTACLE_PLUGIN_H + +#include "customized_rvo2/KdTree.h" +#include "customized_rvo2/Obstacle.h" +#include "customized_rvo2/Plugins/OrcaPlugin.h" + +class ObstaclePlugin : public OrcaObstaclePluginBase +{ +public: + using ObstacleNeighborList = std::vector>; + ObstaclePlugin() : OrcaObstaclePluginBase("ObstaclePlugin") {} + std::vector calcOrcaLines(std::shared_ptr agent) override; + void processObstacles() { obstacle_kdtree_.buildObstacleTree(obstacles_); } + std::vector getObstacleLines(); + size_t addObstacle(const std::vector & vertices, bool process_obstacles); + void deleteObstacles(); + +private: + std::vector> obstacles_; + RVO::ObstacleKdTree obstacle_kdtree_; +}; + +#endif // CUSTOMIZED_RVO2__PLUGINS__GLOBAL_OBSTACLE_PLUGIN_H diff --git a/simulation/context_gamma_planner/customized_rvo2/include/customized_rvo2/Plugins/OrcaPlugin.h b/simulation/context_gamma_planner/customized_rvo2/include/customized_rvo2/Plugins/OrcaPlugin.h new file mode 100644 index 00000000000..ff8df1e634c --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2/include/customized_rvo2/Plugins/OrcaPlugin.h @@ -0,0 +1,66 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CUSTOMIZED_RVO2__ORCA_PLUGIN_HPP_ +#define CUSTOMIZED_RVO2__ORCA_PLUGIN_HPP_ + +#include + +#include "customized_rvo2/Math.h" + +namespace RVO +{ +class Agent; +class AgentKdTree; +} // namespace RVO + +class OrcaPluginBase +{ +public: + using SharedPtr = std::shared_ptr; + OrcaPluginBase(std::string name) : name_(name) {} + const std::string & getName() const { return name_; } + enum class PluginType { + AGENT, + OBSTACLE, + }; + virtual PluginType getType() = 0; + void activate() { is_active_ = true; } + void deactivate() { is_active_ = false; } + bool isActive() const { return is_active_; } + +protected: + std::string name_; + bool is_active_ = true; +}; + +class OrcaAgentPluginBase : public OrcaPluginBase +{ +public: + using SharedPtr = std::shared_ptr; + OrcaAgentPluginBase(std::string name) : OrcaPluginBase(name) {} + virtual std::vector calcOrcaLines( + std::shared_ptr agent, std::shared_ptr, double time_step_s) = 0; + PluginType getType() override { return PluginType::AGENT; } +}; + +class OrcaObstaclePluginBase : public OrcaPluginBase +{ +public: + using SharedPtr = std::shared_ptr; + OrcaObstaclePluginBase(std::string name) : OrcaPluginBase(name) {} + virtual std::vector calcOrcaLines(std::shared_ptr agent) = 0; + PluginType getType() override { return PluginType::OBSTACLE; } +}; +#endif // CUSTOMIZED_RVO2__ORCA_PLUGIN_HPP_ \ No newline at end of file diff --git a/simulation/context_gamma_planner/customized_rvo2/include/customized_rvo2/RVO.h b/simulation/context_gamma_planner/customized_rvo2/include/customized_rvo2/RVO.h new file mode 100644 index 00000000000..dfc63995a4f --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2/include/customized_rvo2/RVO.h @@ -0,0 +1,523 @@ +/* + * RVO.h + * RVO2 Library + * + * Copyright 2008 University of North Carolina at Chapel Hill + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + * + * modified by Kotaro Yoshimoto + */ + +#ifndef CUSTOMIZED_RVO2__RVO_H_ +#define CUSTOMIZED_RVO2__RVO_H_ + +#include "Math.h" +#include "RVOSimulator.hpp" +#include "VisualizeMarker.hpp" + +/** + + \file RVO.h + \brief Includes all public headers in the library. + + \namespace RVO + \brief Contains all classes, functions, and constants used in the library. + + \mainpage RVO2 Library + + \author Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, and + Dinesh Manocha + + RVO2 Library is an easy-to-use C++ implementation of the + Optimal Reciprocal Collision + Avoidance (ORCA) formulation for multi-agent simulation. RVO2 + Library automatically uses parallelism for computing the motion of the + agents if your machine has multiple processors and your compiler supports OpenMP. + + Please follow the following steps to install and use RVO2 Library. + + - \subpage whatsnew + - \subpage building + - \subpage using + - \subpage params + + See the documentation of the RVO::RVOSimulator class for an exhaustive list of + public functions of RVO2 Library. + + RVO2 Library, accompanying example code, and this documentation is + released for educational, research, and non-profit purposes under the following + \subpage terms "terms and conditions". + + + \page whatsnew What Is New in RVO2 Library + + \section localca Local Collision Avoidance + + The main difference between RVO2 Library and %RVO Library 1.x is the + local collision avoidance technique used. RVO2 Library uses + Optimal Reciprocal Collision + Avoidance (ORCA), whereas %RVO Library 1.x uses Reciprocal Velocity Obstacles (%RVO). + For legacy reasons, and since both techniques are based on the same principles + of reciprocal collision avoidance and relative velocity, we did not change the + name of the library. + + A major consequence of the change of local collision avoidance technique is + that the simulation has become much faster in RVO2 Library. ORCA defines + velocity constraints with respect to other agents as half-planes, and an + optimal velocity is efficiently found using (two-dimensional) linear + programming. In contrast, %RVO Library 1.x uses random sampling to find a good + velocity. Also, the behavior of the agents is smoother in RVO2 Library. + It is proven mathematically that ORCA lets the velocity of agents evolve + continuously over time, whereas %RVO Library 1.x occasionally showed + oscillations and reciprocal dances. Furthermore, ORCA provides stronger + guarantees with respect to collision avoidance. + + \section global Global Path Planning + + Local collision avoidance as provided by RVO2 Library should in + principle be accompanied by global path planning that determines the preferred + velocity of each agent in each time step of the simulation. %RVO Library 1.x + has a built-in roadmap infrastructure to guide agents around obstacles to fixed + goals. However, besides roadmaps, other techniques for global planning, such as + navigation fields, cell decompositions, etc. exist. Therefore, RVO2 + Library does not provide global planning infrastructure anymore. Instead, + it is the responsibility of the external application to set the preferred + velocity of each agent ahead of each time step of the simulation. This makes + the library more flexible to use in varying application domains. In one of the + example applications that comes with RVO2 Library, we show how a roadmap + similar to %RVO Library 1.x is used externally to guide the global navigation + of the agents. As a consequence of this change, RVO2 Library does not + have a concept of a "goal position" or "preferred speed" + for each agent, but only relies on the preferred velocities of the agents set + by the external application. + + \section structure Structure of RVO2 Library + + The structure of RVO2 Library is similar to that of %RVO Library 1.x. + Users familiar with %RVO Library 1.x should find little trouble in using + RVO2 Library. However, RVO2 Library is not backwards compatible + with %RVO Library 1.x. The main reason for this is that the ORCA technique + requires different (and fewer) parameters to be set than %RVO. Also, the way + obstacles are represented is different. In %RVO Library 1.x, obstacles are + represented by an arbitrary collection of line segments. In RVO2 + Library, obstacles are non-intersecting polygons, specified by lists of + vertices in counterclockwise order. Further, in %RVO Library 1.x agents cannot + be added to the simulation after the simulation is initialized. In RVO2 + Library this restriction is removed. Obstacles still need to be processed + before the simulation starts, though. Lastly, in %RVO Library 1.x an instance + of the simulator is a singleton. This restriction is removed in RVO2 + Library. + + \section smaller Smaller Changes + + With RVO2 Library, we have adopted the philosophy that anything that is + not part of the core local collision avoidance technique is to be stripped from + the library. Therefore, besides the roadmap infrastructure, we have also + removed acceleration constraints of agents, orientation of agents, and the + unused "class" of agents. Each of these can be implemented external + of the library if needed. We did maintain a kd-tree infrastructure for + efficiently finding other agents and obstacles nearby each agent. + + Also, RVO2 Library allows accessing information about the simulation, + such as the neighbors and the collision-avoidance constraints of each agent, + that is hidden from the user in %RVO Library 1.x. + + + \page building Building RVO2 Library + + We assume that you have downloaded RVO2 Library and unpacked the ZIP + archive to a path $RVO_ROOT. + + \section xcode Apple Xcode 3.x + + Open $RVO_ROOT/RVO.xcodeproj and select the %RVO target and + a configuration (Debug or Release). A framework + RVO.framework will be built in the default build directory, e.g. + $RVO_ROOT/build/Release. + + \section cmake CMake + + Create and switch to your chosen build directory, e.g. + $RVO_ROOT/build. Run cmake inside the build directory on the + source directory, e.g. cmake $RVO_ROOT/src. Build files for the + default generator for your platform will be generated in the build directory. + + \section make GNU Make + + Switch to the source directory $RVO_ROOT/src and run make. + Public header files (RVO.h, RVOSimulator.h, and + Vector2.h) will be copied to the $RVO_ROOT/include directory + and a static library libRVO.a will be compiled into the + $RVO_ROOT/lib directory. + + \section visual Microsoft Visual Studio 2008 + + Open $RVO_ROOT/RVO.sln and select the %RVO project and a + configuration (Debug, ReleaseST, or ReleaseMT). + Public header files (RVO.h, RVOSimulator.h, and + Vector2.h) will be copied to the $RVO_ROOT/include directory + and a static library, e.g. RVO.lib, will be compiled into the + $RVO_ROOT/lib directory. + + + \page using Using RVO2 Library + + \section structure Structure + + A program performing an RVO2 Library simulation has the following global + structure. + + \code + #include + + std::vector goals; + + int main() + { + // Create a new simulator instance. + RVO::RVOSimulator* sim = new RVO::RVOSimulator(); + + // Set up the scenario. + setupScenario(sim); + + // Perform (and manipulate) the simulation. + do { + updateVisualization(sim); + setPreferredVelocities(sim); + sim->doStep(); + } while (!reachedGoal(sim)); + + delete sim; + } + \endcode + + In order to use RVO2 Library, the user needs to include RVO.h. The first + step is then to create an instance of RVO::RVOSimulator. Then, the process + consists of two stages. The first stage is specifying the simulation scenario + and its parameters. In the above example program, this is done in the method + setupScenario(...), which we will discuss below. The second stage is the actual + performing of the simulation. + + In the above example program, simulation steps are taken until all + the agents have reached some predefined goals. Prior to each simulation step, + we set the preferred velocity for each agent, i.e. the + velocity the agent would have taken if there were no other agents around, in + the method setPreferredVelocities(...). The simulator computes the actual + velocities of the agents and attempts to follow the preferred velocities as + closely as possible while guaranteeing collision avoidance at the same time. + During the simulation, the user may want to retrieve information from the + simulation for instance to visualize the simulation. In the above example + program, this is done in the method updateVisualization(...), which we will + discuss below. It is also possible to manipulate the simulation during the + simulation, for instance by changing positions, radii, velocities, etc. of the + agents. + + \section spec Setting up the Simulation Scenario + + A scenario that is to be simulated can be set up as follows. A scenario + consists of two types of objects: agents and obstacles. Each of them can be + manually specified. Agents may be added anytime before or during the + simulation. Obstacles, however, need to be defined prior to the simulation, and + RVO::RVOSimulator::processObstacles() need to be called in order for the + obstacles to be accounted for in the simulation. + The user may also want to define goal positions of the agents, or a + roadmap to guide the agents around obstacles. This is not done in RVO2 + Library, but needs to be taken care of in the user's external application. + + The following example creates a scenario with four agents exchanging positions + around a rectangular obstacle in the middle. + + \code + void setupScenario(RVO::RVOSimulator* sim) { + // Specify global time step of the simulation. + sim->setTimeStep(0.25f); + + // Specify default parameters for agents that are subsequently added. + sim->setAgentDefaults(15.0f, 10, 10.0f, 5.0f, 2.0f, 2.0f); + + // Add agents, specifying their start position. + sim->addAgent(RVO::Vector2(-50.0f, -50.0f)); + sim->addAgent(RVO::Vector2(50.0f, -50.0f)); + sim->addAgent(RVO::Vector2(50.0f, 50.0f)); + sim->addAgent(RVO::Vector2(-50.0f, 50.0f)); + + // Create goals (simulator is unaware of these). + for (size_t i = 0; i < sim->getNumAgents(); ++i) { + goals.push_back(-sim->getAgentPosition(i)); + } + + // Add (polygonal) obstacle(s), specifying vertices in counterclockwise order. + std::vector vertices; + vertices.push_back(RVO::Vector2(-7.0f, -20.0f)); + vertices.push_back(RVO::Vector2(7.0f, -20.0f)); + vertices.push_back(RVO::Vector2(7.0f, 20.0f)); + vertices.push_back(RVO::Vector2(-7.0f, 20.0f)); + + sim->addObstacle(vertices); + + // Process obstacles so that they are accounted for in the simulation. + sim->processObstacles(); + } + \endcode + + See the documentation on RVO::RVOSimulator for a full overview of the + functionality to specify scenarios. + + \section ret Retrieving Information from the Simulation + + During the simulation, the user can extract information from the simulation for + instance for visualization purposes, or to determine termination conditions of + the simulation. In the example program above, visualization is done in the + updateVisualization(...) method. Below we give an example that simply writes + the positions of each agent in each time step to the standard output. The + termination condition is checked in the reachedGoal(...) method. Here we give + an example that returns true if all agents are within one radius of their + goals. + + \code + void updateVisualization(RVO::RVOSimulator* sim) { + // Output the current global time. + std::cout << sim->getGlobalTime() << " "; + + // Output the position for all the agents. + for (size_t i = 0; i < sim->getNumAgents(); ++i) { + std::cout << sim->getAgentPosition(i) << " "; + } + + std::cout << std::endl; + } + \endcode + + \code + bool reachedGoal(RVO::RVOSimulator* sim) { + // Check whether all agents have arrived at their goals. + for (size_t i = 0; i < sim->getNumAgents(); ++i) { + if (absSq(goals[i] - sim->getAgentPosition(i)) > sim->getAgentRadius(i) * + sim->getAgentRadius(i)) { + // Agent is further away from its goal than one radius. + return false; + } + } + return true; + } + \endcode + + Using similar functions as the ones used in this example, the user can access + information about other parameters of the agents, as well as the global + parameters, and the obstacles. See the documentation of the class + RVO::RVOSimulator for an exhaustive list of public functions for retrieving + simulation information. + + \section manip Manipulating the Simulation + + During the simulation, the user can manipulate the simulation, for instance by + changing the global parameters, or changing the parameters of the agents + (potentially causing abrupt different behavior). It is also possible to give + the agents a new position, which make them jump through the scene. New agents + can be added to the simulation at any time, but it is not allowed to add + obstacles to the simulation after they have been processed by calling + RVO::RVOSimulator::processObstacles(). Also, it is impossible to change the + position of the vertices of the obstacles. + + See the documentation of the class RVO::RVOSimulator for an exhaustive list of + public functions for manipulating the simulation. + + To provide global guidance to the agents, the preferred velocities of the + agents can be changed ahead of each simulation step. In the above example + program, this happens in the method setPreferredVelocities(...). Here we give + an example that simply sets the preferred velocity to the unit vector towards + the agent's goal for each agent (i.e., the preferred speed is 1.0). Note that + this may not give convincing results with respect to global navigation around + the obstacles. For this a roadmap or other global planning techniques may be + used (see one of the \ref example "example programs" that accompanies RVO2 + Library). + + \code + void setPreferredVelocities(RVO::RVOSimulator* sim) { + // Set the preferred velocity for each agent. + for (size_t i = 0; i < sim->getNumAgents(); ++i) { + if (absSq(goals[i] - sim->getAgentPosition(i)) < sim->getAgentRadius(i) * + sim->getAgentRadius(i) ) { + // Agent is within one radius of its goal, set preferred velocity to zero + sim->setAgentPrefVelocity(i, RVO::Vector2(0.0f, 0.0f)); + } else { + // Agent is far away from its goal, set preferred velocity as unit vector + towards agent's goal. sim->setAgentPrefVelocity(i, normalize(goals[i] - + sim->getAgentPosition(i))); + } + } + } + \endcode + + \section example Example Programs + + RVO2 Library is accompanied by three example programs, which can be + found in the $RVO_ROOT/examples directory. The examples are named + Blocks, Circle, and Roadmap, and contain the following demonstration scenarios: + + + + + + + + + + + + + +
BlocksA scenario in which 100 agents, split in four groups initially + positioned in each of four corners of the environment, move to the + other side of the environment through a narrow passage generated by four + obstacles. There is no roadmap to guide the agents around the obstacles.
CircleA scenario in which 250 agents, initially positioned evenly + distributed on a circle, move to the antipodal position on the + circle. There are no obstacles.
RoadmapThe same scenario as ExampleBlocks, but now the preferred + velocities of the agents are determined using a roadmap guiding the agents + around the obstacles.
+ + + \page params Parameter Overview + + \section globalp Global Parameters + + + + + + + + + + + + +
ParameterType (unit)Meaning
timeStepfloat (time)The time step of the simulation. Must be positive.
+ + \section agent Agent Parameters + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
ParameterType (unit)Meaning
maxNeighborssize_tThe maximum number of other agents the agent takes into + account in the navigation. The larger this number, the + longer the running time of the simulation. If the number is + too low, the simulation will not be safe.
maxSpeedfloat (distance/time)The maximum speed of the agent. Must be non-negative.
neighborDistfloat (distance)The maximum distance (center point to center point) to + other agents the agent takes into account in the + navigation. The larger this number, the longer the running + time of the simulation. If the number is too low, the + simulation will not be safe. Must be non-negative.
positionRVO::Vector2 (distance, distance)The current position of the agent.
prefVelocityRVO::Vector2 (distance/time, distance/time) + The current preferred velocity of the agent. This is the + velocity the agent would take if no other agents or + obstacles were around. The simulator computes an actual + velocity for the agent that follows the preferred velocity + as closely as possible, but at the same time guarantees + collision avoidance.
radiusfloat (distance)The radius of the agent. Must be non-negative.
timeHorizonfloat (time)The minimal amount of time for which the agent's velocities + that are computed by the simulation are safe with respect + to other agents. The larger this number, the sooner this + agent will respond to the presence of other agents, but the + less freedom the agent has in choosing its velocities. + Must be positive.
timeHorizonObstfloat (time)The minimal amount of time for which the agent's velocities + that are computed by the simulation are safe with respect + to obstacles. The larger this number, the sooner this agent + will respond to the presence of obstacles, but the less + freedom the agent has in choosing its velocities. + Must be positive.
velocityRVO::Vector2 (distance/time, distance/time) + The (current) velocity of the agent.
+ + + \page terms Terms and Conditions + + RVO2 Library + + Copyright 2008 University of North Carolina at Chapel Hill + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + + */ + +#endif // CUSTOMIZED_RVO2__RVO_H_ diff --git a/simulation/context_gamma_planner/customized_rvo2/include/customized_rvo2/RVOSimulator.hpp b/simulation/context_gamma_planner/customized_rvo2/include/customized_rvo2/RVOSimulator.hpp new file mode 100644 index 00000000000..f7706a4fe09 --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2/include/customized_rvo2/RVOSimulator.hpp @@ -0,0 +1,115 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CUSTOMIZED_RVO2__RVOSIMULATOR_HPP_ +#define CUSTOMIZED_RVO2__RVOSIMULATOR_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "customized_rvo2/Agent.hpp" +#include "customized_rvo2/Plugins/AgentPlugin.h" +#include "customized_rvo2/Plugins/ObstaclePlugin.h" + +namespace RVO +{ +/** + * + */ +class RVOSimulator +{ +public: + explicit RVOSimulator(float time_step_s = 0.025f); + void update(); + void update(Agent::SharedPtr agent); + void setTimeStep(float time_step_s) { time_step_ = time_step_s; } + +#define FORWARD_TO_AGENT(NAME) \ + decltype(auto) NAME(const std::string name) const { return getAgent(name)->NAME(); } + FORWARD_TO_AGENT(getPosition); + FORWARD_TO_AGENT(getId); + FORWARD_TO_AGENT(getAgentConfig); + FORWARD_TO_AGENT(getObstacleLines); + + void addOrcaLinePlugin(OrcaPluginBase::SharedPtr plugin) { orca_plugins_.emplace_back(plugin); } + void addAgent(const std::shared_ptr agent); + const std::unordered_map> & getAgents() const + { + return agents_; + } + const std::shared_ptr getAgent(const std::string & name) const; + float getGlobalTime() const; + + /// @brief Add static obstacle to rvo simulator + /// @param obstacle Static obstacle polygons + /// @param process_obstacles Build a kdtree from static obstacles + /// It is recommended to basically set it to false and run it + /// only once after all obstacles have been added, + /// as the process takes a long time. + /// @return obstacle id + size_t addGlobalObstacle(const std::vector & obstacle, bool process_obstacles = true); + + /// @brief Add static obstacles to rvo simulator + /// @param obstacles Static obstacle polygons list + /// @param process_obstacles Build a kdtree from static obstacles + /// It is recommended to basically set it to false and run it + /// only once after all obstacles have been added, + /// as the process takes a long time. + void addGlobalObstacles( + const std::vector> & obstacles, bool process_obstacles = false); + bool agentExist(const std::string & name) const; + void addObstacleORCAPlugin(OrcaObstaclePluginBase::SharedPtr plugin) + { + orca_obstacle_plugins_.emplace_back(plugin); + } + void addAgentORCAPlugin(OrcaAgentPluginBase::SharedPtr plugin) + { + orca_agent_plugins_.emplace_back(plugin); + } + + /// @brief Build a kdtree from static obstacles + void processObstacles() { obstacle_orca_plugin_->processObstacles(); } + std::vector getObstacleLines() { return obstacle_orca_plugin_->getObstacleLines(); } + void markUnavailableAllAgent(); + void deleteUnavailableAgent(); + void setAgentMaxSpeed(const std::string & name, double speed) + { + agents_[name]->setMaxSpeed(speed); + } + void deleteObstacles(); + + AgentConfig createVehicleConfig( + const traffic_simulator_msgs::msg::VehicleParameters & parameters) const; + AgentConfig createPedestrianConfig( + const traffic_simulator_msgs::msg::PedestrianParameters & parameters) const; + +protected: + void updateAgent(Agent::SharedPtr agent); + std::vector orca_plugins_; + std::shared_ptr kdtree_; + std::unordered_map> agents_; + std::vector orca_agent_plugins_; + std::vector orca_obstacle_plugins_; + std::shared_ptr obstacle_orca_plugin_; + std::shared_ptr agent_plugin_; + float time_step_; + float global_time_; +}; +} // namespace RVO + +#endif // CUSTOMIZED_RVO2__RVOSIMULATOR_HPP_ diff --git a/simulation/context_gamma_planner/customized_rvo2/include/customized_rvo2/VisualizeMarker.hpp b/simulation/context_gamma_planner/customized_rvo2/include/customized_rvo2/VisualizeMarker.hpp new file mode 100644 index 00000000000..4f437dbdbea --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2/include/customized_rvo2/VisualizeMarker.hpp @@ -0,0 +1,54 @@ +// Copyright 2022 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CUSTOMIZED_RVO2__VISUALIZEMARKER_HPP_ +#define CUSTOMIZED_RVO2__VISUALIZEMARKER_HPP_ + +#include "customized_rvo2/RVOSimulator.hpp" +#include "rclcpp/rclcpp.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +namespace RVO +{ +/** + * + */ +class VisualizeMarker +{ +public: + void visualizeAgents( + visualization_msgs::msg::MarkerArray & marker_array, const RVOSimulator & sim_); + void visualizeObstacles(visualization_msgs::msg::MarkerArray & marker_array, RVOSimulator & sim_); + void visualizeObstacleORCALine( + visualization_msgs::msg::MarkerArray & marker_array, const RVOSimulator & sim_); + void visualizeAgentORCALine( + visualization_msgs::msg::MarkerArray & marker_array, const RVOSimulator & sim_); + void visualizeVelocitySpace( + visualization_msgs::msg::MarkerArray & marker_array, const RVOSimulator & sim_); + void visualizeAgentVelocity( + visualization_msgs::msg::MarkerArray & marker_array, const RVOSimulator & sim_); + void visualizeAgentPrefVelocity( + visualization_msgs::msg::MarkerArray & marker_array, const RVOSimulator & sim_); + void visualizeAllMarkers( + visualization_msgs::msg::MarkerArray & marker_array, RVOSimulator & sim_); + void visualizeNextGoal( + visualization_msgs::msg::MarkerArray & marker_array, + const geometry_msgs::msg::Point & goal_point); + void visualizeGoalPoses( + visualization_msgs::msg::MarkerArray & marker_array, + const std::vector & goal_poses); +}; +} // namespace RVO + +#endif // CUSTOMIZED_RVO2__VISUALIZEMARKER_HPP_ \ No newline at end of file diff --git a/simulation/context_gamma_planner/customized_rvo2/package.xml b/simulation/context_gamma_planner/customized_rvo2/package.xml new file mode 100644 index 00000000000..e437cfa6eba --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2/package.xml @@ -0,0 +1,26 @@ + + + + customized_rvo2 + 0.0.0 + a package for customized RVO2 library + Kotaro Yoshimoto + Apache-2.0 + + ament_cmake_auto + + scenario_simulator_exception + traffic_simulator + traffic_simulator_msgs + tf2_geometry_msgs + visualization_msgs + + ament_lint_auto + ament_cmake_clang_format + ament_cmake_pep257 + ament_cmake_xmllint + + + ament_cmake + + \ No newline at end of file diff --git a/simulation/context_gamma_planner/customized_rvo2/src/Agent.cpp b/simulation/context_gamma_planner/customized_rvo2/src/Agent.cpp new file mode 100644 index 00000000000..59f238926e2 --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2/src/Agent.cpp @@ -0,0 +1,277 @@ +/* + * Agent.cpp + * copied and modified from AgentImpl.cpp in RVO2 Library + * + * Copyright 2008 University of North Carolina at Chapel Hill + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + * + * modified by Kotaro Yoshimoto + */ + +#include "customized_rvo2/Agent.hpp" + +#include +#include +#include + +#include "customized_rvo2/KdTree.h" +#include "customized_rvo2/Plugins/AgentPlugin.h" +#include "customized_rvo2/Plugins/ObstaclePlugin.h" + +namespace RVO +{ +const size_t & Agent::getId() const { return this->id_; } + +void Agent::setId(const size_t & id) { id_ = id; } + +Vector2 Agent::getPosition() const +{ + return pose_.position + Vector2(-config_.base_link_to_center, 0.0).rotate(getOrientationYaw()); +} +void Agent::setPosition(const Vector2 & position) +{ + pose_.position = position + Vector2(config_.base_link_to_center, 0.0).rotate(getOrientationYaw()); +} +const Vector2 & Agent::getCenterPosition() const { return pose_.position; } +const Vector2 & Agent::getVelocity() const { return velocity_; } +float Agent::getOrientationYaw() const +{ + Eigen::Quaterniond eigen_quat( + pose_.orientation.w, pose_.orientation.x, pose_.orientation.y, pose_.orientation.z); + return eigen_quat.toRotationMatrix().eulerAngles(0, 1, 2)[2]; +} +const geometry_msgs::msg::Quaternion & Agent::getOrientation() const { return pose_.orientation; } +void Agent::setOrientation(const float & orientation) +{ + Eigen::Quaterniond eigen_quat(Eigen::AngleAxisd(orientation, Eigen::Vector3d::UnitZ())); + pose_.orientation.x = eigen_quat.x(); + pose_.orientation.y = eigen_quat.y(); + pose_.orientation.z = eigen_quat.z(); + pose_.orientation.w = eigen_quat.w(); +} +void Agent::setOrientation(const geometry_msgs::msg::Quaternion & quat) +{ + pose_.orientation = quat; +} + +void Agent::setPose(const geometry_msgs::msg::Pose & pose) +{ + setPosition(RVO::Vector2(pose.position.x, pose.position.y)); + setOrientation(pose.orientation); +} +geometry_msgs::msg::Pose Agent::getPose() const +{ + geometry_msgs::msg::Pose pose; + pose.position.x = getPosition().x(); + pose.position.y = getPosition().y(); + pose.orientation = getOrientation(); + return pose; +} + +void Agent::setMaxSpeed(const float & max_speed) { config_.max_speed = max_speed; } +const float & Agent::getMaxSpeed() const { return config_.max_speed; } +void Agent::setMaxAcceleration(const float & max_acceleration) +{ + config_.max_acceleration = max_acceleration; +} +const float & Agent::getMaxAcceleration() const { return config_.max_acceleration; } +void Agent::setMaxDeceleration(const float & max_deceleration) +{ + config_.max_deceleration = max_deceleration; +} +const float & Agent::getMaxDeceleration() const { return config_.max_deceleration; } + +const AgentConfig & Agent::getAgentConfig() const { return config_; } + +/** + * @brief Adds a list of waypoints to the agent's existing waypoints. + * + * @param wps The list of waypoints to be added. + */ +void Agent::addWaypoints(const std::vector & wps) +{ + waypoints_.insert(waypoints_.end(), wps.begin(), wps.end()); +} + +/** + * @brief Updates the front waypoint of the agent. + * + * If the agent's waypoints list is empty, the given waypoint is added to the list. + * Otherwise, the first waypoint in the list is updated with the given waypoint. + * + * @param wp The new front waypoint. + */ +void Agent::updateFrontWaypoint(const RVO::Vector2 & wp) +{ + if (waypoints_.empty()) { + waypoints_.emplace_back(wp); + } else { + waypoints_.front() = wp; + } +} + +/** + * @brief the new velocity for an agent based on the given agent lines, obstacle lines, and number of obstacle lines. + * + * @param agent_lines The vector of agent lines. + * @param obstacle_lines The vector of obstacle lines. + * @param numObstLines The number of obstacle lines. + * @return The computed velocity as a Vector2 object. + */ +Vector2 Agent::computeNewVelocity( + const std::vector & agent_lines, const std::vector & obstacle_lines, + const int numObstLines) const +{ + Vector2 calc_velocity; + std::vector lines; + lines.reserve(agent_lines.size() + obstacle_lines.size()); + std::copy(agent_lines.begin(), agent_lines.end(), std::back_inserter(lines)); + std::copy(obstacle_lines.begin(), obstacle_lines.end(), std::back_inserter(lines)); + + size_t lineFail = linearProgram2(lines, config_.max_speed, pref_velocity_, false, calc_velocity); + + if (lineFail < lines.size()) { + linearProgram3(lines, lineFail, numObstLines, config_.max_speed, calc_velocity); + } + return calc_velocity; +} + +/** + * @brief Updates the agent's simple navigation based on the current waypoints. + * + * This function calculates the goal vector by subtracting the agent's current position from the first waypoint. + * If the squared magnitude of the goal vector is greater than 1.0, it is normalized. + * If there are more than one waypoints, the first waypoint is removed from the list. + * The preferred velocity is then calculated by multiplying the goal vector with the maximum speed. + */ +void Agent::updateSimpleNavigation() +{ + RVO::Vector2 goal_vector = waypoints_.front() - pose_.position; + + if (RVO::absSq(goal_vector) > 1.0f) { + goal_vector = RVO::normalize(goal_vector); + } else if (waypoints_.size() > 1) { + waypoints_.pop_front(); + } + //goal_vector = RVO::normalize(goal_vector); + pref_velocity_ = goal_vector * config_.max_speed; +} + +/** + * @brief Constructor for the Agent class. + * + * @param name The name of the agent. + * @param pos The initial position of the agent. + * @param config The configuration settings for the agent. + * @param orientation The initial orientation of the agent. + */ +Agent::Agent( + const std::string & name, const RVO::Vector2 & pos, const AgentConfig & config, + const double orientation) +: name(name), config_(config) +{ + setPosition(pos); + setVelocity(RVO::Vector2(0.f, 0.f)); + setOrientation(orientation); + addWaypoint(this->pose_.position); +} + +/** + * @brief Constructor for the Agent class. + * + * @param name The name of the agent. + * @param pose The initial pose of the agent. + * @param config The configuration for the agent. + */ +Agent::Agent( + const std::string & name, const geometry_msgs::msg::Pose & pose, const AgentConfig & config) +: name(name), config_(config) +{ + setPosition(RVO::Vector2(pose.position.x, pose.position.y)); + setVelocity(RVO::Vector2(0.f, 0.f)); + setOrientation(pose.orientation); + addWaypoint(this->pose_.position); +} + +const Vector2 & Agent::getPrefVelocity() const { return this->pref_velocity_; } + +/** + * @brief Updates the agent's state based on the given ORCA lines and time step. + * + * This function calculates the preferred velocity for the agent using the updateSimpleNavigation() function. + * It then calculates the ORCA lines for obstacles and other agents using the provided vectors. + * The ORCA lines are used to compute the new velocity for the agent. + * The agent's state is updated based on the new velocity and the given time step. + * + * @param orca_lines_obst The ORCA lines for obstacles. + * @param orca_lines_agent The ORCA lines for other agents. + * @param time_step_s The time step in seconds. + * @param stop_velocity_threshold The threshold for stopping velocity. + */ +void Agent::update( + const std::vector & orca_lines_obst, const std::vector & orca_lines_agent, + double time_step_s, double stop_velocity_threshold) +{ + updateSimpleNavigation(); + + new_velocity_ = computeNewVelocity(orca_lines_agent, orca_lines_obst, orca_lines_obst.size()); + // update state + if (RVO::absSq(new_velocity_) < stop_velocity_threshold) { + velocity_ = new_velocity_; + } else if (abs(new_velocity_) < abs(velocity_) - config_.max_deceleration * time_step_s) { + velocity_ = + (abs(velocity_) - config_.max_deceleration * time_step_s) * normalize(new_velocity_); + } else if (abs(velocity_) + config_.max_acceleration * time_step_s < abs(new_velocity_)) { + velocity_ = + (abs(velocity_) + config_.max_acceleration * time_step_s) * normalize(new_velocity_); + } else { + velocity_ = new_velocity_; + } + pose_.position += velocity_ * time_step_s; + if (RVO::absSq(velocity_) > stop_velocity_threshold) { + setOrientation(std::atan2(velocity_.y(), velocity_.x())); + } +} + +/** + * @brief Prints the status of the agent. + * + * This function prints the name, type, position, yaw angle, and velocity of the agent. + */ +void Agent::printStatus() +{ + std::cout << "##### " << name << " #####" << std::endl; + std::cout << "name : " << name << std::endl; + std::cout << "type : " << config_.entity_type << std::endl; + std::cout << "position : " << pose_.position.x() << ", " << pose_.position.y() << std::endl; + std::cout << "yaw angle : " << getOrientationYaw() << "[rad] (" + << getOrientationYaw() * 180.0 / M_PI << " [deg])" << std::endl; + std::cout << "velocity : " << velocity_.x() << ", " << velocity_.y() << std::endl; + std::cout << "##### " << name << " #####" << std::endl; +} + +} // namespace RVO \ No newline at end of file diff --git a/simulation/context_gamma_planner/customized_rvo2/src/KdTree.cpp b/simulation/context_gamma_planner/customized_rvo2/src/KdTree.cpp new file mode 100644 index 00000000000..edcad9c087d --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2/src/KdTree.cpp @@ -0,0 +1,429 @@ +/* + * KdTree.cpp + * RVO2 Library + * + * Copyright 2008 University of North Carolina at Chapel Hill + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + * + * modified by Kotaro Yoshimoto + */ + +#include "customized_rvo2/KdTree.h" + +#include +#include +#include +#include + +#include "customized_rvo2/Agent.hpp" +#include "customized_rvo2/Obstacle.h" + +namespace RVO +{ +AgentKdTree::AgentKdTree() {} +void AgentKdTree::rebuildAgentTree( + std::unordered_map> & all_agents) +{ + if (!agents_.empty()) agents_.clear(); + if (!agent_tree_.empty()) agent_tree_.clear(); + agents_.reserve(all_agents.size()); + for (auto agent : all_agents) { + agents_.emplace_back(agent.second); + } + agent_tree_.resize(2 * agents_.size() - 1); + + if (!agents_.empty()) { + buildAgentTreeRecursive(0, agents_.size(), 0); + } +} +/* +void AgentKdTree::updateAgentTree(std::unordered_map> & all_agents) +{ + if (agents_.size() < all_agents.size()) { + for (size_t i = agents_.size(); i < all_agents.size(); ++i) { + agents_.push_back(all_agents[i]); + } + + agent_tree_.resize(2 * agents_.size() - 1); + } + + if (!agents_.empty()) { + buildAgentTreeRecursive(0, agents_.size(), 0); + } +}*/ +void AgentKdTree::buildAgentTreeRecursive(size_t begin, size_t end, size_t node) +{ + agent_tree_[node].begin = begin; + agent_tree_[node].end = end; + agent_tree_[node].min_x = agent_tree_[node].max_x = agents_[begin]->getCenterPosition().x(); + agent_tree_[node].min_y = agent_tree_[node].max_y = agents_[begin]->getCenterPosition().y(); + + for (size_t i = begin + 1; i < end; ++i) { + agent_tree_[node].max_x = + std::max(agent_tree_[node].max_x, agents_[i]->getCenterPosition().x()); + agent_tree_[node].min_x = + std::min(agent_tree_[node].min_x, agents_[i]->getCenterPosition().x()); + agent_tree_[node].max_y = + std::max(agent_tree_[node].max_y, agents_[i]->getCenterPosition().y()); + agent_tree_[node].min_y = + std::min(agent_tree_[node].min_y, agents_[i]->getCenterPosition().y()); + } + + if (end - begin > MAX_LEAF_SIZE) { + /* No leaf node. */ + const bool is_vertical = + (agent_tree_[node].max_x - agent_tree_[node].min_x > + agent_tree_[node].max_y - agent_tree_[node].min_y); + const auto splitValue = [&]() -> float { + if (is_vertical) { + return 0.5f * (agent_tree_[node].max_x + agent_tree_[node].min_x); + } + return 0.5f * (agent_tree_[node].max_y + agent_tree_[node].min_y); + }; + size_t left = begin; + size_t right = end; + + while (left < right) { + auto verticalPosition = [&](size_t index) { + if (is_vertical) { + return agents_[index]->getCenterPosition().x(); + } + return agents_[index]->getCenterPosition().y(); + }; + while (left < right && verticalPosition(left) < splitValue()) { + ++left; + } + while (right > left && verticalPosition(right - 1) >= splitValue()) { + --right; + } + if (left < right) { + std::swap(agents_[left], agents_[right - 1]); + ++left; + --right; + } + } + + if (left == begin) { + ++left; + ++right; + } + + agent_tree_[node].left = node + 1; + agent_tree_[node].right = node + 2 * (left - begin); + + buildAgentTreeRecursive(begin, left, agent_tree_[node].left); + buildAgentTreeRecursive(left, end, agent_tree_[node].right); + } +} +std::vector>> AgentKdTree::computeAgentNeighbors( + const RVO::Vector2 position, const float range) const +{ + float range_sq = sqr(range); + std::vector>> ret; + queryAgentTreeRecursive(position, range_sq, 0, ret); + return ret; +} +void AgentKdTree::queryAgentTreeRecursive( + RVO::Vector2 position, float & range_sq, size_t node, + std::vector>> & ret) const +{ + if (agent_tree_[node].end - agent_tree_[node].begin <= MAX_LEAF_SIZE) { + for (size_t i = agent_tree_[node].begin; i < agent_tree_[node].end; ++i) { + ret.emplace_back(std::make_pair(range_sq, agents_[i])); + } + } else { + const float dist_sq_left = + sqr(std::max(0.0f, agent_tree_[agent_tree_[node].left].min_x - position.x())) + + sqr(std::max(0.0f, position.x() - agent_tree_[agent_tree_[node].left].max_x)) + + sqr(std::max(0.0f, agent_tree_[agent_tree_[node].left].min_y - position.y())) + + sqr(std::max(0.0f, position.y() - agent_tree_[agent_tree_[node].left].max_y)); + + const float dist_sq_right = + sqr(std::max(0.0f, agent_tree_[agent_tree_[node].right].min_x - position.x())) + + sqr(std::max(0.0f, position.x() - agent_tree_[agent_tree_[node].right].max_x)) + + sqr(std::max(0.0f, agent_tree_[agent_tree_[node].right].min_y - position.y())) + + sqr(std::max(0.0f, position.y() - agent_tree_[agent_tree_[node].right].max_y)); + + if (dist_sq_left < dist_sq_right) { + if (dist_sq_left < range_sq) { + queryAgentTreeRecursive(position, range_sq, agent_tree_[node].left, ret); + + if (dist_sq_right < range_sq) { + queryAgentTreeRecursive(position, range_sq, agent_tree_[node].right, ret); + } + } + } else { + if (dist_sq_right < range_sq) { + queryAgentTreeRecursive(position, range_sq, agent_tree_[node].right, ret); + + if (dist_sq_left < range_sq) { + queryAgentTreeRecursive(position, range_sq, agent_tree_[node].left, ret); + } + } + } + } +} + +ObstacleKdTree::ObstacleKdTree() : obstacle_tree_(NULL) {} + +void ObstacleKdTree::buildObstacleTree(std::vector & obstacles) +{ + deleteObstacleTree(); + delete_flag_ = true; + std::vector obstacle_root_node; + std::copy(obstacles.begin(), obstacles.end(), std::back_inserter(obstacle_root_node)); + obstacle_tree_ = buildObstacleTreeRecursive(obstacle_root_node, obstacles); +} +ObstacleKdTree::ObstacleTreeNode * ObstacleKdTree::buildObstacleTreeRecursive( + std::vector & obstacles_node, + std::vector & obstacles_original) +{ + if (obstacles_node.empty()) { + return NULL; + } else { + ObstacleTreeNode * const node = new ObstacleTreeNode; + + size_t optimal_split = 0; + size_t min_left = obstacles_node.size(); + size_t min_right = obstacles_node.size(); + + for (size_t i = 0; i < obstacles_node.size(); ++i) { + size_t left_size = 0; + size_t right_size = 0; + + Obstacle::SharedPtr obstacle_i1 = obstacles_node.at(i); + auto obstacle_i2 = obstacles_node.at(i)->next_obstacle_; + + /* Compute optimal split node. */ + for (size_t j = 0; j < obstacles_node.size(); ++j) { + if (i == j) { + continue; + } + + const auto obstacle_j1 = obstacles_node[j]; + const auto obstacle_j2 = obstacle_j1->next_obstacle_; + + const float j1_left_of_i = + leftOf(obstacle_i1->point_, obstacle_i2->point_, obstacle_j1->point_); + const float j2_left_of_i = + leftOf(obstacle_i1->point_, obstacle_i2->point_, obstacle_j2->point_); + + if (j1_left_of_i >= -RVO_EPSILON && j2_left_of_i >= -RVO_EPSILON) { + ++left_size; + } else if (j1_left_of_i <= RVO_EPSILON && j2_left_of_i <= RVO_EPSILON) { + ++right_size; + } else { + ++left_size; + ++right_size; + } + + if ( + std::make_pair(std::max(left_size, right_size), std::min(left_size, right_size)) >= + std::make_pair(std::max(min_left, min_right), std::min(min_left, min_right))) { + break; + } + } + + if ( + std::make_pair(std::max(left_size, right_size), std::min(left_size, right_size)) < + std::make_pair(std::max(min_left, min_right), std::min(min_left, min_right))) { + min_left = left_size; + min_right = right_size; + optimal_split = i; + } + } + + /* Build split node. */ + std::vector left_obstacles(min_left); + std::vector right_obstacles(min_right); + + size_t left_counter = 0; + size_t right_counter = 0; + const size_t i = optimal_split; + + auto obstacle_i1 = obstacles_node[i]; + auto obstacle_i2 = obstacle_i1->next_obstacle_; + + for (size_t j = 0; j < obstacles_node.size(); ++j) { + if (i == j) { + continue; + } + + const auto obstacle_j1 = obstacles_node[j]; + const auto obstacle_j2 = obstacle_j1->next_obstacle_; + + const float j1_left_of_i = + leftOf(obstacle_i1->point_, obstacle_i2->point_, obstacle_j1->point_); + const float j2_left_of_i = + leftOf(obstacle_i1->point_, obstacle_i2->point_, obstacle_j2->point_); + + if (j1_left_of_i >= -RVO_EPSILON && j2_left_of_i >= -RVO_EPSILON) { + left_obstacles[left_counter++] = obstacles_node[j]; + } else if (j1_left_of_i <= RVO_EPSILON && j2_left_of_i <= RVO_EPSILON) { + right_obstacles[right_counter++] = obstacles_node[j]; + } else { + /* Split obstacle j. */ + const float t = + det( + obstacle_i2->point_ - obstacle_i1->point_, obstacle_j1->point_ - obstacle_i1->point_) / + det(obstacle_i2->point_ - obstacle_i1->point_, obstacle_j1->point_ - obstacle_j2->point_); + + const Vector2 split_point = + obstacle_j1->point_ + t * (obstacle_j2->point_ - obstacle_j1->point_); + + auto new_obstacle = std::make_shared(); + new_obstacle->point_ = split_point; + new_obstacle->prev_obstacle_ = obstacle_j1; + new_obstacle->next_obstacle_ = obstacle_j2; + new_obstacle->is_convex_ = true; + new_obstacle->unit_dir_ = obstacle_j1->unit_dir_; + + new_obstacle->id_ = obstacles_original.size(); + + obstacles_original.push_back(new_obstacle); + + obstacle_j1->next_obstacle_ = new_obstacle; + obstacle_j2->prev_obstacle_ = new_obstacle; + + if (j1_left_of_i > 0.0f) { + left_obstacles[left_counter++] = obstacle_j1; + right_obstacles[right_counter++] = new_obstacle; + } else { + right_obstacles[right_counter++] = obstacle_j1; + left_obstacles[left_counter++] = new_obstacle; + } + } + } + + node->obstacle = obstacle_i1; + node->left = buildObstacleTreeRecursive(left_obstacles, obstacles_original); + node->right = buildObstacleTreeRecursive(right_obstacles, obstacles_original); + return node; + } +} +std::vector> ObstacleKdTree::computeObstacleNeighbors( + RVO::Vector2 position, float & range) const +{ + float range_sq = sqr(range); + std::vector> ret; + queryObstacleTreeRecursive(position, range_sq, obstacle_tree_, ret); + return ret; +} +void ObstacleKdTree::deleteObstacleTree() +{ + if (delete_flag_) { + deleteObstacleTree(obstacle_tree_); + delete_flag_ = false; + } +} +void ObstacleKdTree::deleteObstacleTree(ObstacleTreeNode * node) +{ + if (node != NULL) { + deleteObstacleTree(node->left); + deleteObstacleTree(node->right); + delete node; + } +} +void ObstacleKdTree::queryObstacleTreeRecursive( + RVO::Vector2 position, float & range_sq, const ObstacleTreeNode * node, + std::vector> & ret) const +{ + if (node == NULL) { + return; + } else { + const auto obstacle1 = node->obstacle; + const auto obstacle2 = obstacle1->next_obstacle_; + + const float agent_left_of_line = leftOf(obstacle1->point_, obstacle2->point_, position); + + queryObstacleTreeRecursive( + position, range_sq, (agent_left_of_line >= 0.0f ? node->left : node->right), ret); + + const float dist_sq_line = + sqr(agent_left_of_line) / absSq(obstacle2->point_ - obstacle1->point_); + + if (dist_sq_line < range_sq) { + if (agent_left_of_line < 0.0f) { + /* + * Try obstacle at this node only if agent is on right side of + * obstacle (and can see obstacle). + */ + ret.emplace_back(std::make_pair(range_sq, node->obstacle)); + } + + /* Try other side of line. */ + queryObstacleTreeRecursive( + position, range_sq, (agent_left_of_line >= 0.0f ? node->right : node->left), ret); + } + } +} +bool ObstacleKdTree::queryVisibility(const Vector2 & q1, const Vector2 & q2, float radius) const +{ + return queryVisibilityRecursive(q1, q2, radius, obstacle_tree_); +} + +bool ObstacleKdTree::queryVisibilityRecursive( + const Vector2 & q1, const Vector2 & q2, float radius, const ObstacleTreeNode * node) const +{ + if (node == NULL) { + return true; + } else { + const auto obstacle1 = node->obstacle; + const auto obstacle2 = obstacle1->next_obstacle_; + + const float q1_left_of_i = leftOf(obstacle1->point_, obstacle2->point_, q1); + const float q2_left_of_i = leftOf(obstacle1->point_, obstacle2->point_, q2); + const float inv_length_i = 1.0f / absSq(obstacle2->point_ - obstacle1->point_); + + if (q1_left_of_i >= 0.0f && q2_left_of_i >= 0.0f) { + return queryVisibilityRecursive(q1, q2, radius, node->left) && + ((sqr(q1_left_of_i) * inv_length_i >= sqr(radius) && + sqr(q2_left_of_i) * inv_length_i >= sqr(radius)) || + queryVisibilityRecursive(q1, q2, radius, node->right)); + } else if (q1_left_of_i <= 0.0f && q2_left_of_i <= 0.0f) { + return queryVisibilityRecursive(q1, q2, radius, node->right) && + ((sqr(q1_left_of_i) * inv_length_i >= sqr(radius) && + sqr(q2_left_of_i) * inv_length_i >= sqr(radius)) || + queryVisibilityRecursive(q1, q2, radius, node->left)); + } else if (q1_left_of_i >= 0.0f && q2_left_of_i <= 0.0f) { + /* One can see through obstacle from left to right. */ + return queryVisibilityRecursive(q1, q2, radius, node->left) && + queryVisibilityRecursive(q1, q2, radius, node->right); + } else { + const float point1_left_of_q = leftOf(q1, q2, obstacle1->point_); + const float point2_left_of_q = leftOf(q1, q2, obstacle2->point_); + const float inv_length_q = 1.0f / absSq(q2 - q1); + + return point1_left_of_q * point2_left_of_q >= 0.0f && + sqr(point1_left_of_q) * inv_length_q > sqr(radius) && + sqr(point2_left_of_q) * inv_length_q > sqr(radius) && + queryVisibilityRecursive(q1, q2, radius, node->left) && + queryVisibilityRecursive(q1, q2, radius, node->right); + } + } +} +} // namespace RVO diff --git a/simulation/context_gamma_planner/customized_rvo2/src/Math.cpp b/simulation/context_gamma_planner/customized_rvo2/src/Math.cpp new file mode 100644 index 00000000000..020e2706aad --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2/src/Math.cpp @@ -0,0 +1,192 @@ +/* +* Math.cpp +* copied and modified from Vector2.h in RVO2 Library +* +* Copyright 2008 University of North Carolina at Chapel Hill +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +* +* Please send all bug reports to . +* +* The authors may be contacted via: +* +* Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha +* Dept. of Computer Science +* 201 S. Columbia St. +* Frederick P. Brooks, Jr. Computer Science Bldg. +* Chapel Hill, N.C. 27599-3175 +* United States of America +* +* +* +* modified by Kotaro Yoshimoto +*/ + +#include "customized_rvo2/Math.h" + +#include +#include + +bool RVO::linearProgram1( + const std::vector & lines, size_t line_no, float radius, const Vector2 & opt_velocity, + bool direction_opt, Vector2 & result) +{ + const float dot_product = lines[line_no].point * lines[line_no].direction; + const float discriminant = sqr(dot_product) + sqr(radius) - absSq(lines[line_no].point); + + if (discriminant < 0.0f) { + /* Max speed circle fully invalidates line line_no. */ + return false; + } + + const float sqrt_discriminant = std::sqrt(discriminant); + float t_left = -dot_product - sqrt_discriminant; + float t_right = -dot_product + sqrt_discriminant; + + for (size_t i = 0; i < line_no; ++i) { + const float denominator = det(lines[line_no].direction, lines[i].direction); + const float numerator = det(lines[i].direction, lines[line_no].point - lines[i].point); + + if (std::fabs(denominator) <= RVO_EPSILON) { + /* Lines line_no and i are (almost) parallel. */ + if (numerator < 0.0f) { + return false; + } else { + continue; + } + } + + const float t = numerator / denominator; + + if (denominator >= 0.0f) { + /* Line i bounds line line_no on the right. */ + t_right = std::min(t_right, t); + } else { + /* Line i bounds line line_no on the left. */ + t_left = std::max(t_left, t); + } + if (t_left > t_right) { + return false; + } + } + + if (direction_opt) { + /* Optimize direction. */ + if (opt_velocity * lines[line_no].direction > 0.0f) { + /* Take right extreme. */ + result = lines[line_no].point + t_right * lines[line_no].direction; + } else { + /* Take left extreme. */ + result = lines[line_no].point + t_left * lines[line_no].direction; + } + } else { + /* Optimize closest point. */ + const float t = lines[line_no].direction * (opt_velocity - lines[line_no].point); + + if (t < t_left) { + result = lines[line_no].point + t_left * lines[line_no].direction; + } else if (t > t_right) { + result = lines[line_no].point + t_right * lines[line_no].direction; + } else { + result = lines[line_no].point + t * lines[line_no].direction; + } + } + + return true; +} +size_t RVO::linearProgram2( + const std::vector & lines, float radius, const Vector2 & opt_velocity, bool direction_opt, + Vector2 & result) +{ + if (direction_opt) { + /* + * Optimize direction. Note that the optimization velocity is of unit + * length in this case. + */ + result = opt_velocity * radius; + } else if (absSq(opt_velocity) > sqr(radius)) { + /* Optimize closest point and outside circle. */ + result = normalize(opt_velocity) * radius; + } else { + /* Optimize closest point and inside circle. */ + result = opt_velocity; + } + + for (size_t i = 0; i < lines.size(); ++i) { + if (det(lines[i].direction, lines[i].point - result) > 0.0f) { + /* Result does not satisfy constraint i. Compute new optimal result. */ + const Vector2 temp_result = result; + + if (!linearProgram1(lines, i, radius, opt_velocity, direction_opt, result)) { + result = temp_result; + return i; + } + } + } + + return lines.size(); +} +void RVO::linearProgram3( + const std::vector & lines, size_t num_obst_lines, size_t begin_line, float radius, + Vector2 & result) +{ + float distance = 0.0f; + + for (size_t i = begin_line; i < lines.size(); ++i) { + if (det(lines[i].direction, lines[i].point - result) > distance) { + /* Result does not satisfy constraint of line i. */ + std::vector proj_lines( + lines.begin(), lines.begin() + static_cast(num_obst_lines)); + + for (size_t j = num_obst_lines; j < i; ++j) { + Line line; + + float determinant = det(lines[i].direction, lines[j].direction); + + if (std::fabs(determinant) <= RVO_EPSILON) { + /* Line i and line j are parallel. */ + if (lines[i].direction * lines[j].direction > 0.0f) { + /* Line i and line j point in the same direction. */ + continue; + } else { + /* Line i and line j point in opposite direction. */ + line.point = 0.5f * (lines[i].point + lines[j].point); + } + } else { + line.point = lines[i].point + + (det(lines[j].direction, lines[i].point - lines[j].point) / determinant) * + lines[i].direction; + } + + line.direction = normalize(lines[j].direction - lines[i].direction); + proj_lines.push_back(line); + } + + const Vector2 temp_result = result; + + if ( + linearProgram2( + proj_lines, radius, Vector2(-lines[i].direction.y(), lines[i].direction.x()), true, + result) < proj_lines.size()) { + /* This should in principle not happen. The result is by definition + * already in the feasible region of this linear program. If it fails, + * it is due to small floating point error, and the current result is + * kept. + */ + result = temp_result; + } + + distance = det(lines[i].direction, lines[i].point - result); + } + } +} diff --git a/simulation/context_gamma_planner/customized_rvo2/src/Obstacle.cpp b/simulation/context_gamma_planner/customized_rvo2/src/Obstacle.cpp new file mode 100644 index 00000000000..08c03516436 --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2/src/Obstacle.cpp @@ -0,0 +1,40 @@ +/* + * Obstacle.cpp + * RVO2 Library + * + * Copyright 2008 University of North Carolina at Chapel Hill + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + * + * modified by Kotaro Yoshimoto + */ + +#include "customized_rvo2/Obstacle.h" + +namespace RVO +{ +Obstacle::Obstacle() : is_convex_(false), id_(0) {} +} // namespace RVO diff --git a/simulation/context_gamma_planner/customized_rvo2/src/Plugins/AgentPlugin.cpp b/simulation/context_gamma_planner/customized_rvo2/src/Plugins/AgentPlugin.cpp new file mode 100644 index 00000000000..a174ee3ad6f --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2/src/Plugins/AgentPlugin.cpp @@ -0,0 +1,174 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "customized_rvo2/Plugins/AgentPlugin.h" + +#include "customized_rvo2/Agent.hpp" + +using namespace RVO; +/* Compute linear constraints on dynamic Agents */ +std::vector AgentPlugin::calcOrcaLines( + RVO::Agent::SharedPtr agent, std::shared_ptr agent_kdtree, double time_step_s) +{ + std::vector orca_lines; + if (!is_active_) { + return orca_lines; + } + + auto orcaLines = std::make_shared>(); + const float inv_time_horizon = 1.0f / agent->getAgentConfig().time_horizon; + Vector2 position = agent->getCenterPosition(); + Vector2 velocity = agent->getVelocity(); + + AgentNeighborList neighbor_list = + agent_kdtree->computeAgentNeighbors(position, agent->getAgentConfig().neighbor_dist); + filterAgentNeighbors(agent, neighbor_list); + /* Create agent ORCA lines. */ + // Collision avoidance between Agents + for (auto neighbor : neighbor_list) { + const auto other = neighbor.second; + const Vector2 relative_position = other->getCenterPosition() - position; + const Vector2 relative_velocity = velocity - other->getVelocity(); + const float dist_sq = absSq(relative_position); + + //Calculating the radius of an ellipse + const float relative_angle = std::atan2(relative_position.y(), relative_position.x()); + /// @note: The radius of the ellipse is calculated from the relative angle of the other agent. + auto getRadius = [](const Agent::SharedPtr agent, float relative_angle) { + const float relative_tan = std::tan(relative_angle - agent->getOrientationYaw()); + return agent->getAgentConfig().radius * agent->getAgentConfig().ellipticity * + std::sqrt( + (1 + relative_tan * relative_tan) / + (agent->getAgentConfig().ellipticity * agent->getAgentConfig().ellipticity + + relative_tan * relative_tan)); + }; + const float agent_radius = getRadius(agent, relative_angle); + const float other_radius = getRadius(other, M_PI + relative_angle); + + /// @note: Calculate as a circle when the distance is far, and as an ellipse when the distance is near. + /// If ellipses are used in all areas of the calculation, the radius changes abruptly + /// as the agents pass each other, and the path of movement is not smooth. + const float max_combined_radius = + agent->getAgentConfig().radius + other->getAgentConfig().radius; + const float min_combined_radius = agent_radius + other_radius; + float combined_radius = min_combined_radius; + if (dist_sq > max_combined_radius) { + combined_radius = max_combined_radius; + } else { + combined_radius = + std::max(0.5f * (dist_sq - min_combined_radius) + min_combined_radius, min_combined_radius); + } + const float combined_radius_sq = sqr(combined_radius); + + Line line; + Vector2 u; + if (dist_sq >= combined_radius_sq) { + /* When there is no conflict between itself and the target Dynamic Agent */ + /* No collision. */ + const Vector2 w = relative_velocity - inv_time_horizon * relative_position; + /* Vector from cutoff center to relative velocity. */ + const float w_length_sq = absSq(w); + /* Inner product of the dynamic Agent of itself and the target */ + const float dot_product1 = w * relative_position; + + if (dot_product1 < 0.0f && sqr(dot_product1) > combined_radius_sq * w_length_sq) { + /* Project on cut-off circle. */ + const float w_length = std::sqrt(w_length_sq); + const Vector2 unit_w = w / w_length; + + line.direction = Vector2(unit_w.y(), -unit_w.x()); + u = (combined_radius * inv_time_horizon - w_length) * unit_w; + } else { + /* Project on legs. */ + const float leg = std::sqrt(dist_sq - combined_radius_sq); + + if (det(relative_position, w) > 0.0f) { + /* Project on left leg. */ + line.direction = + Vector2( + relative_position.x() * leg - relative_position.y() * combined_radius, + relative_position.x() * combined_radius + relative_position.y() * leg) / + dist_sq; + } else { + /* Project on right leg. */ + line.direction = + -Vector2( + relative_position.x() * leg + relative_position.y() * combined_radius, + -relative_position.x() * combined_radius + relative_position.y() * leg) / + dist_sq; + } + + const float dot_product2 = relative_velocity * line.direction; + + u = dot_product2 * line.direction - relative_velocity; + } + } else { + /* When there is a conflict between itself and the target Dynamic Agent */ + /* Collision. Project on cut-off circle of time timeStep. */ + if (time_step_s <= std::numeric_limits::epsilon()) { + THROW_SIMULATION_ERROR("Time step is too small : ", time_step_s, "[s]"); + } + const float inv_time_step = 1.0f / time_step_s; + + /* Vector from cutoff center to relative velocity. */ + const Vector2 w = relative_velocity - inv_time_step * relative_position; + + const float w_length = abs(w); + const Vector2 unit_w = w / w_length; + line.direction = Vector2(unit_w.y(), -unit_w.x()); + u = (combined_radius * inv_time_step - w_length) * unit_w; + } + + line.point = velocity + 0.5f * u; + /*****************************************/ + orca_lines.push_back(line); + /*****************************************/ + } + + return orca_lines; +} + +void AgentPlugin::filterAgentNeighbors( + RVO::Agent::SharedPtr agent, AgentNeighborList & neighbor_list) +{ + // filter by ignore_list_ + const auto & ignore_list = agent->getIgnoreList(); + neighbor_list.erase( + std::remove_if( + neighbor_list.begin(), neighbor_list.end(), + [ignore_list](const std::pair & neighbor) -> bool { + return std::any_of( + ignore_list.begin(), ignore_list.end(), [neighbor](const uint8_t & entity_type) { + return entity_type == neighbor.second->getAgentConfig().entity_type; + }); + }), + neighbor_list.end()); + + // filter with agent->getAgentConfig().max_neighbors + if (neighbor_list.size() > agent->getAgentConfig().max_neighbors) { + // sort neighbors by distance (nearer first) + std::sort( + neighbor_list.begin(), neighbor_list.end(), + []( + const std::pair & lhs, + const std::pair & rhs) { return lhs.first < rhs.first; }); + // erase more neighbors than agent->getAgentConfig().max_neighbors + neighbor_list.erase( + neighbor_list.begin() + agent->getAgentConfig().max_neighbors, neighbor_list.end()); + } + + neighbor_list.erase(std::remove_if( + neighbor_list.begin(), neighbor_list.end(), + [agent](const auto & x) -> bool { return x.second->name == agent->name; })); +} diff --git a/simulation/context_gamma_planner/customized_rvo2/src/Plugins/ObstaclePlugin.cpp b/simulation/context_gamma_planner/customized_rvo2/src/Plugins/ObstaclePlugin.cpp new file mode 100644 index 00000000000..6a81cbbdf32 --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2/src/Plugins/ObstaclePlugin.cpp @@ -0,0 +1,389 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "customized_rvo2/Plugins/ObstaclePlugin.h" + +#include "customized_rvo2/Agent.hpp" + +using namespace RVO; +std::vector ObstaclePlugin::calcOrcaLines(RVO::Agent::SharedPtr agent) +{ + std::vector orca_lines; + if (!is_active_) { + return orca_lines; + } + Vector2 position = agent->getCenterPosition(); + Vector2 velocity = agent->getVelocity(); + float neighbor_dist = agent->getAgentConfig().neighbor_dist; + ObstacleNeighborList neighbor_list = + obstacle_kdtree_.computeObstacleNeighbors(position, neighbor_dist); + const float invTimeHorizonObst = 1.0f / agent->getAgentConfig().time_horizon_obst; + + /* Create obstacle ORCA lines. */ + // Obstacle Lines + for (size_t i = 0; i < neighbor_list.size(); ++i) { + Obstacle::SharedPtr obstacle1 = neighbor_list[i].second; + Obstacle::SharedPtr obstacle2 = obstacle1->next_obstacle_; + + const Vector2 relativePosition1 = obstacle1->point_ - position; + const Vector2 relativePosition2 = obstacle2->point_ - position; + const Vector2 position12 = obstacle2->point_ - obstacle1->point_; + + // Calculating the radius of an ellipse + const float agent_angle = agent->getOrientationYaw(); + Vector2 relativePosition; + if (dot(-relativePosition1, position12) < 0) { + relativePosition = relativePosition1; + } else if (dot(-relativePosition2, -position12) < 0) { + relativePosition = relativePosition2; + } else { + const double norm13 = dot(position12, -relativePosition1) / abs(position12); + relativePosition = relativePosition1 + normalize(position12) * norm13; + } + + const float relative_angle = std::atan2(relativePosition.y(), relativePosition.x()); + const float agent_relative_tan = std::tan(relative_angle - agent_angle); + const float agent_radius = + agent->getAgentConfig().radius * agent->getAgentConfig().ellipticity * + std::sqrt( + (1 + agent_relative_tan * agent_relative_tan) / + (agent->getAgentConfig().ellipticity * agent->getAgentConfig().ellipticity + + agent_relative_tan * agent_relative_tan)); + //std::cout << "obstacle_radius: " << agent_radius << std::endl; + /* + * Check if velocity obstacle of obstacle is already taken care of by + * previously constructed obstacle ORCA lines. + */ + bool alreadyCovered = false; + + for (size_t j = 0; j < orca_lines.size(); ++j) { + if ( + det( + invTimeHorizonObst * relativePosition1 - orca_lines.at(j).point, + orca_lines.at(j).direction) - + invTimeHorizonObst * agent_radius >= + -RVO_EPSILON && + det( + invTimeHorizonObst * relativePosition2 - orca_lines.at(j).point, + orca_lines.at(j).direction) - + invTimeHorizonObst * agent_radius >= + -RVO_EPSILON) { + alreadyCovered = true; + break; + } + } + + if (alreadyCovered) { + continue; + } + + /* Not yet covered. Check for collisions. */ + + const float distSq1 = absSq(relativePosition1); + const float distSq2 = absSq(relativePosition2); + + const float radiusSq = sqr(agent_radius); + + const Vector2 obstacleVector = obstacle2->point_ - obstacle1->point_; + const float s = (-relativePosition1 * obstacleVector) / absSq(obstacleVector); + const float distSqLine = absSq(-relativePosition1 - s * obstacleVector); + + Line line; + + if (s < 0.0f && distSq1 <= radiusSq) { + /* Collision with left vertex. Ignore if non-convex. */ + if (obstacle1->is_convex_) { + line.point = Vector2(0.0f, 0.0f); + line.direction = normalize(Vector2(-relativePosition1.y(), relativePosition1.x())); + /*****************************************/ + orca_lines.push_back(line); + /*****************************************/ + } + + continue; + } else if (s > 1.0f && distSq2 <= radiusSq) { + /* Collision with right vertex. Ignore if non-convex + * or if it will be taken care of by neighoring obstace */ + if (obstacle2->is_convex_ && det(relativePosition2, obstacle2->unit_dir_) >= 0.0f) { + line.point = Vector2(0.0f, 0.0f); + line.direction = normalize(Vector2(-relativePosition2.y(), relativePosition2.x())); + /*****************************************/ + orca_lines.push_back(line); + /*****************************************/ + } + + continue; + } else if (s >= 0.0f && s < 1.0f && distSqLine <= radiusSq) { + /* Collision with obstacle segment. */ + line.point = Vector2(0.0f, 0.0f); + line.direction = -obstacle1->unit_dir_; + /*****************************************/ + orca_lines.push_back(line); + /*****************************************/ + continue; + } + + /* + * No collision. + * Compute legs. When obliquely viewed, both legs can come from a single + * vertex. Legs extend cut-off line when nonconvex vertex. + */ + + Vector2 leftLegDirection, rightLegDirection; + + if (s < 0.0f && distSqLine <= radiusSq) { + /* + * Obstacle viewed obliquely so that left vertex + * defines velocity obstacle. + */ + if (!obstacle1->is_convex_) { + /* Ignore obstacle. */ + continue; + } + + obstacle2 = obstacle1; + + const float leg1 = std::sqrt(distSq1 - radiusSq); + leftLegDirection = Vector2( + relativePosition1.x() * leg1 - relativePosition1.y() * agent_radius, + relativePosition1.x() * agent_radius + relativePosition1.y() * leg1) / + distSq1; + rightLegDirection = Vector2( + relativePosition1.x() * leg1 + relativePosition1.y() * agent_radius, + -relativePosition1.x() * agent_radius + relativePosition1.y() * leg1) / + distSq1; + } else if (s > 1.0f && distSqLine <= radiusSq) { + /* + * Obstacle viewed obliquely so that + * right vertex defines velocity obstacle. + */ + if (!obstacle2->is_convex_) { + /* Ignore obstacle. */ + continue; + } + + obstacle1 = obstacle2; + + const float leg2 = std::sqrt(distSq2 - radiusSq); + leftLegDirection = Vector2( + relativePosition2.x() * leg2 - relativePosition2.y() * agent_radius, + relativePosition2.x() * agent_radius + relativePosition2.y() * leg2) / + distSq2; + rightLegDirection = Vector2( + relativePosition2.x() * leg2 + relativePosition2.y() * agent_radius, + -relativePosition2.x() * agent_radius + relativePosition2.y() * leg2) / + distSq2; + } else { + /* Usual situation. */ + if (obstacle1->is_convex_) { + const float leg1 = std::sqrt(distSq1 - radiusSq); + leftLegDirection = Vector2( + relativePosition1.x() * leg1 - relativePosition1.y() * agent_radius, + relativePosition1.x() * agent_radius + relativePosition1.y() * leg1) / + distSq1; + } else { + /* Left vertex non-convex; left leg extends cut-off line. */ + leftLegDirection = -obstacle1->unit_dir_; + } + + if (obstacle2->is_convex_) { + const float leg2 = std::sqrt(distSq2 - radiusSq); + rightLegDirection = + Vector2( + relativePosition2.x() * leg2 + relativePosition2.y() * agent_radius, + -relativePosition2.x() * agent_radius + relativePosition2.y() * leg2) / + distSq2; + } else { + /* Right vertex non-convex; right leg extends cut-off line. */ + rightLegDirection = obstacle1->unit_dir_; + } + } + + /* + * Legs can never point into neighboring edge when convex vertex, + * take cutoff-line of neighboring edge instead. If velocity projected on + * "foreign" leg, no constraint is added. + */ + + const Obstacle::SharedPtr leftNeighbor = obstacle1->prev_obstacle_; + + bool isLeftLegForeign = false; + bool isRightLegForeign = false; + + if (obstacle1->is_convex_ && det(leftLegDirection, -leftNeighbor->unit_dir_) >= 0.0f) { + /* Left leg points into obstacle. */ + leftLegDirection = -leftNeighbor->unit_dir_; + isLeftLegForeign = true; + } + + if (obstacle2->is_convex_ && det(rightLegDirection, obstacle2->unit_dir_) <= 0.0f) { + /* Right leg points into obstacle. */ + rightLegDirection = obstacle2->unit_dir_; + isRightLegForeign = true; + } + + /* Compute cut-off centers. */ + const Vector2 leftCutoff = invTimeHorizonObst * (obstacle1->point_ - position); + const Vector2 rightCutoff = invTimeHorizonObst * (obstacle2->point_ - position); + const Vector2 cutoffVec = rightCutoff - leftCutoff; + + /* Project current velocity on velocity obstacle. */ + + /* Check if current velocity is projected on cutoff circles. */ + const float t = + (obstacle1 == obstacle2 ? 0.5f : ((velocity - leftCutoff) * cutoffVec) / absSq(cutoffVec)); + const float tLeft = ((velocity - leftCutoff) * leftLegDirection); + const float tRight = ((velocity - rightCutoff) * rightLegDirection); + + if ((t < 0.0f && tLeft < 0.0f) || (obstacle1 == obstacle2 && tLeft < 0.0f && tRight < 0.0f)) { + /* Project on left cut-off circle. */ + const Vector2 unitW = normalize(velocity - leftCutoff); + + line.direction = Vector2(unitW.y(), -unitW.x()); + line.point = leftCutoff + agent_radius * invTimeHorizonObst * unitW; + /*****************************************/ + orca_lines.push_back(line); + /*****************************************/ + continue; + } else if (t > 1.0f && tRight < 0.0f) { + /* Project on right cut-off circle. */ + const Vector2 unitW = normalize(velocity - rightCutoff); + + line.direction = Vector2(unitW.y(), -unitW.x()); + line.point = rightCutoff + agent_radius * invTimeHorizonObst * unitW; + /*****************************************/ + orca_lines.push_back(line); + /*****************************************/ + continue; + } + + /* + * Project on left leg, right leg, or cut-off line, whichever is closest + * to velocity. + */ + const float distSqCutoff = + ((t < 0.0f || t > 1.0f || obstacle1 == obstacle2) + ? std::numeric_limits::infinity() + : absSq(velocity - (leftCutoff + t * cutoffVec))); + const float distSqLeft = + ((tLeft < 0.0f) ? std::numeric_limits::infinity() + : absSq(velocity - (leftCutoff + tLeft * leftLegDirection))); + const float distSqRight = + ((tRight < 0.0f) ? std::numeric_limits::infinity() + : absSq(velocity - (rightCutoff + tRight * rightLegDirection))); + + if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight) { + /* Project on cut-off line. */ + line.direction = -obstacle1->unit_dir_; + line.point = leftCutoff + agent_radius * invTimeHorizonObst * + Vector2(-line.direction.y(), line.direction.x()); + /*****************************************/ + orca_lines.push_back(line); + /*****************************************/ + continue; + } else if (distSqLeft <= distSqRight) { + /* Project on left leg. */ + if (isLeftLegForeign) { + continue; + } + + line.direction = leftLegDirection; + line.point = leftCutoff + agent_radius * invTimeHorizonObst * + Vector2(-line.direction.y(), line.direction.x()); + /*****************************************/ + orca_lines.push_back(line); + /*****************************************/ + continue; + } else { + /* Project on right leg. */ + if (isRightLegForeign) { + continue; + } + + line.direction = -rightLegDirection; + line.point = rightCutoff + agent_radius * invTimeHorizonObst * + Vector2(-line.direction.y(), line.direction.x()); + /*****************************************/ + orca_lines.push_back(line); + /*****************************************/ + continue; + } + } + return orca_lines; +} + +size_t ObstaclePlugin::addObstacle( + const std::vector & vertices, bool process_obstacles = true) +{ + if (vertices.size() < 2) { + return RVO::RVO_ERROR; + } + + const size_t obstacleNo = obstacles_.size(); + + for (size_t i = 0; i < vertices.size(); ++i) { + auto obstacle = std::make_shared(); + obstacle->point_ = vertices[i]; + + if (i != 0) { + obstacle->prev_obstacle_ = obstacles_.back(); + obstacle->prev_obstacle_->next_obstacle_ = obstacle; + } + + if (i == vertices.size() - 1) { + obstacle->next_obstacle_ = obstacles_[obstacleNo]; + obstacle->next_obstacle_->prev_obstacle_ = obstacle; + } + + obstacle->unit_dir_ = normalize(vertices[(i == vertices.size() - 1 ? 0 : i + 1)] - vertices[i]); + + if (vertices.size() == 2) { + obstacle->is_convex_ = true; + } else { + obstacle->is_convex_ = + (leftOf( + vertices[(i == 0 ? vertices.size() - 1 : i - 1)], vertices[i], + vertices[(i == vertices.size() - 1 ? 0 : i + 1)]) >= 0.0f); + } + + obstacle->id_ = obstacles_.size(); + + obstacles_.emplace_back(obstacle); + } + + if (process_obstacles) { + processObstacles(); + } + + return obstacleNo; +} + +std::vector ObstaclePlugin::getObstacleLines() +{ + std::vector lines; + for (size_t index = 0; index < obstacles_.size(); index++) { + auto vertex = obstacles_[index]; + lines.emplace_back(vertex->point_); + lines.emplace_back(vertex->next_obstacle_->point_); + } + return lines; +} + +void ObstaclePlugin::deleteObstacles() +{ + if (obstacles_.size() > 0) { + obstacle_kdtree_.deleteObstacleTree(); + obstacles_.clear(); + } +} \ No newline at end of file diff --git a/simulation/context_gamma_planner/customized_rvo2/src/RVOSimulator.cpp b/simulation/context_gamma_planner/customized_rvo2/src/RVOSimulator.cpp new file mode 100644 index 00000000000..934cf312253 --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2/src/RVOSimulator.cpp @@ -0,0 +1,190 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "customized_rvo2/RVOSimulator.hpp" + +#include +#include +#include + +namespace RVO +{ +RVOSimulator::RVOSimulator(float time_step_s) +{ + setTimeStep(time_step_s); + kdtree_ = std::make_shared(); + + obstacle_orca_plugin_ = std::make_shared(); + addObstacleORCAPlugin(obstacle_orca_plugin_); + + agent_plugin_ = std::make_shared(); + addAgentORCAPlugin(agent_plugin_); +} + +void RVOSimulator::update() +{ + if (agents_.size() != 0) { + kdtree_->rebuildAgentTree(agents_); + } + + for (auto agent : agents_) { + updateAgent(agent.second); + } + global_time_ += time_step_; +} + +void RVOSimulator::update(Agent::SharedPtr agent) +{ + if (agents_.size() != 0) { + kdtree_->rebuildAgentTree(agents_); + } + + updateAgent(agent); + global_time_ += time_step_; +} + +void RVOSimulator::updateAgent(Agent::SharedPtr agent) +{ + agent->obst_orca_lines_.clear(); + for (auto & plugin : orca_obstacle_plugins_) { + if (!plugin->isActive()) { + continue; + } + auto lines = plugin->calcOrcaLines(agent); + for (auto line : lines) { + agent->obst_orca_lines_.emplace_back(line); + } + } + agent->agent_orca_lines_.clear(); + for (auto & plugin : orca_agent_plugins_) { + if (!plugin->isActive()) { + continue; + } + auto lines = plugin->calcOrcaLines(agent, kdtree_, time_step_); + for (auto line : lines) { + agent->agent_orca_lines_.emplace_back(line); + } + } + agent->update(agent->obst_orca_lines_, agent->agent_orca_lines_, time_step_); +} +const std::shared_ptr RVOSimulator::getAgent(const std::string & name) const +{ + if (agents_.find(name) != agents_.end()) { + return agents_.at(name); + } + std::cout << "cannot find an agent named " << name << std::endl; + return nullptr; + // THROW_SIMULATION_ERROR("agent : ", name, " does not exist."); +} + +void RVOSimulator::addAgent(const std::shared_ptr agent) +{ + if (agents_.find(agent->name) != agents_.end()) { + THROW_SIMULATION_ERROR("agent : ", agent->name, " already exist."); + } + agent->setId(agents_.size()); + agents_.emplace(agent->name, agent); +} + +bool RVOSimulator::agentExist(const std::string & name) const +{ + return agents_.find(name) != agents_.end(); +} + +float RVOSimulator::getGlobalTime() const { return global_time_; } + +size_t RVOSimulator::addGlobalObstacle( + const std::vector & obstacle, bool process_obstacles) +{ + if (obstacle.size() < 2) { + THROW_SIMULATION_ERROR("obstacle should be consisted of at least three points"); + if (obstacle.size() < 2) { + return RVO::RVO_ERROR; + } + } + return obstacle_orca_plugin_->addObstacle(obstacle, process_obstacles); +} + +void RVOSimulator::addGlobalObstacles( + const std::vector > & obstacles, bool process_obstacles) +{ + for (const auto & obstacle : obstacles) { + addGlobalObstacle(obstacle, process_obstacles); + } +} + +void RVOSimulator::deleteObstacles() { obstacle_orca_plugin_->deleteObstacles(); } + +void RVOSimulator::markUnavailableAllAgent() +{ + for (auto agent : agents_) { + agent.second->is_available_ = false; + } +} + +void RVOSimulator::deleteUnavailableAgent() +{ + /* + agents_.erase( + std::remove_if( + agents_.begin(), agents_.end(), [](const auto & agent) { return !agent.second->is_available_; }), + agents_.end()); + */ + for (auto it = agents_.begin(); it != agents_.end();) { + if (!it->second->is_available_) { + it = agents_.erase(it); + } else { + ++it; + } + } +} + +AgentConfig RVOSimulator::createVehicleConfig( + const traffic_simulator_msgs::msg::VehicleParameters & parameters) const +{ + RVO::AgentConfig agent_config; + agent_config.radius = parameters.bounding_box.dimensions.x / 2.0; + agent_config.ellipticity = + parameters.bounding_box.dimensions.y / parameters.bounding_box.dimensions.x; + agent_config.base_link_to_center = parameters.bounding_box.center.x; + agent_config.entity_type = traffic_simulator_msgs::msg::EntityType::VEHICLE; + agent_config.max_speed = parameters.performance.max_speed; + agent_config.max_acceleration = parameters.performance.max_acceleration; + agent_config.max_deceleration = parameters.performance.max_deceleration; + // Parameters are set to large values to allow the car to maintain a distance while driving. + // Reduced value to consider only obstacles in the vicinity. + agent_config.neighbor_dist = 10.0f; + agent_config.time_horizon = 10.0f; + agent_config.time_horizon_obst = 1.0f; + return agent_config; +} + +AgentConfig RVOSimulator::createPedestrianConfig( + const traffic_simulator_msgs::msg::PedestrianParameters & parameters) const +{ + RVO::AgentConfig agent_config; + agent_config.radius = parameters.bounding_box.dimensions.x / 2.0; + agent_config.ellipticity = + parameters.bounding_box.dimensions.y / parameters.bounding_box.dimensions.x; + agent_config.base_link_to_center = parameters.bounding_box.center.x; + agent_config.entity_type = traffic_simulator_msgs::msg::EntityType::PEDESTRIAN; + agent_config.max_speed = 1.0f; + agent_config.neighbor_dist = 4.0f; + // The original parameters were too large, and even irrelevant places such as adjacent lanes + // were included in the constraints, so we made them smaller. + agent_config.time_horizon = 5.0f; + agent_config.time_horizon_obst = 2.0f; + return agent_config; +} + +} // namespace RVO diff --git a/simulation/context_gamma_planner/customized_rvo2/src/VisualizeMarker.cpp b/simulation/context_gamma_planner/customized_rvo2/src/VisualizeMarker.cpp new file mode 100644 index 00000000000..a34c456c72c --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2/src/VisualizeMarker.cpp @@ -0,0 +1,422 @@ +// Copyright 2022 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CUSTOMIZED_RVO2__VISUALIZEMARKER_CPP_ +#define CUSTOMIZED_RVO2__VISUALIZEMARKER_CPP_ + +#include "customized_rvo2/VisualizeMarker.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace RVO +{ +/** + * + */ + +void VisualizeMarker::visualizeAgents( + visualization_msgs::msg::MarkerArray & marker_array, const RVOSimulator & sim_) +{ + size_t i = 0; + for (const auto agent : sim_.getAgents()) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Clock().now(); + marker.id = i; + marker.ns = "mpc"; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.type = visualization_msgs::msg::Marker::CYLINDER; + + auto position = + agent.second->getPosition() + Vector2(agent.second->getAgentConfig().base_link_to_center, 0.0) + .rotate(agent.second->getOrientationYaw()); + marker.pose.position.x = position.x(); + marker.pose.position.y = position.y(); + marker.pose.position.z = 0.0; + marker.pose.orientation.w = 1.0; + + double diameter = agent.second->getAgentConfig().radius * 2; + marker.scale.x = diameter; // diameter in x direction + marker.scale.y = + diameter * agent.second->getAgentConfig().ellipticity; // diameter in x direction + marker.scale.z = 0.5; // height + marker.pose.orientation = agent.second->getOrientation(); + marker.color.a = 0.5; + marker.color.r = 1; + marker.color.g = 0; + marker.color.b = 0; + + marker.lifetime = rclcpp::Duration::from_seconds(0.1); + marker_array.markers.emplace_back(marker); + ++i; + } +} + +/** + * + * @param marker_array + */ +void VisualizeMarker::visualizeObstacles( + visualization_msgs::msg::MarkerArray & marker_array, RVOSimulator & sim_) +{ + visualization_msgs::msg::Marker obstacle_marker; + obstacle_marker.header.frame_id = "map"; + obstacle_marker.header.stamp = rclcpp::Clock().now(); + obstacle_marker.id = 0; + obstacle_marker.ns = "obstacle"; + obstacle_marker.action = visualization_msgs::msg::Marker::ADD; + obstacle_marker.type = visualization_msgs::msg::Marker::LINE_LIST; + obstacle_marker.scale.x = 0.2; + obstacle_marker.color.a = 1; + obstacle_marker.color.r = 1; + obstacle_marker.color.g = 1; + obstacle_marker.color.b = 0; + auto lines = sim_.getObstacleLines(); + for (auto p : lines) { + geometry_msgs::msg::Point pt; + pt.x = p.x(); + pt.y = p.y(); + pt.z = 0.0; + obstacle_marker.points.emplace_back(pt); + } + obstacle_marker.lifetime = rclcpp::Duration::from_seconds(0.1); + marker_array.markers.emplace_back(obstacle_marker); +} + +void VisualizeMarker::visualizeObstacleORCALine( + visualization_msgs::msg::MarkerArray & marker_array, const RVOSimulator & sim_) +{ + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Clock().now(); + marker.id = 0; + marker.ns = "obstacle_orca"; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.type = visualization_msgs::msg::Marker::LINE_LIST; + marker.scale.x = 0.05; + marker.color.a = 0.5; + marker.color.r = 1; + marker.color.g = 1; + marker.color.b = 0.5; + + for (auto agent : sim_.getAgents()) { + auto pos = agent.second->getPosition(); + for (auto line : agent.second->obst_orca_lines_) { + geometry_msgs::msg::Point msg; + msg.x = pos.x() + line.point.x() - line.direction.x() * 2.0f; + msg.y = pos.y() + line.point.y() - line.direction.y() * 2.0f; + msg.z = 0.0; + marker.points.push_back(msg); + + msg.x += line.direction.x() * 4.0f; + msg.y += line.direction.y() * 4.0f; + marker.points.emplace_back(msg); + } + } + marker.lifetime = rclcpp::Duration::from_seconds(0.1); + marker_array.markers.emplace_back(marker); +} + +void VisualizeMarker::visualizeAgentORCALine( + visualization_msgs::msg::MarkerArray & marker_array, const RVOSimulator & sim_) +{ + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Clock().now(); + marker.id = 0; + marker.ns = "agent_orca"; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.type = visualization_msgs::msg::Marker::LINE_LIST; + marker.scale.x = 0.05; + marker.color.a = 0.5; + marker.color.r = 1; + marker.color.g = 0.5; + marker.color.b = 0.5; + + for (auto agent : sim_.getAgents()) { + auto pos = agent.second->getPosition(); + for (auto line : agent.second->agent_orca_lines_) { + geometry_msgs::msg::Point msg; + msg.x = pos.x() + line.point.x() - line.direction.x() * 2.0f; + msg.y = pos.y() + line.point.y() - line.direction.y() * 2.0f; + msg.z = 0.0; + marker.points.push_back(msg); + + msg.x += line.direction.x() * 4.0f; + msg.y += line.direction.y() * 4.0f; + marker.points.emplace_back(msg); + } + } + marker.lifetime = rclcpp::Duration::from_seconds(0.1); + marker_array.markers.emplace_back(marker); +} + +void VisualizeMarker::visualizeVelocitySpace( + visualization_msgs::msg::MarkerArray & marker_array, const RVOSimulator & sim_) +{ + namespace bg = boost::geometry; + typedef bg::model::d2::point_xy point; + typedef bg::model::polygon polygon; + //typedef bg::model::box box; + namespace buffer = boost::geometry::strategy::buffer; + + typedef boost::geometry::model::d2::point_xy point; + typedef boost::geometry::model::polygon polygon; + + int index = -1; + //#pragma omp parallel for + for (auto agent : sim_.getAgents()) { + index += 1; + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Clock().now(); + marker.id = index; + marker.ns = "velocity_space"; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.scale.x = 0.1; + marker.color.a = 1.0; + marker.color.r = 0.5; + marker.color.g = 1; + marker.color.b = 0.5; + + auto pos = agent.second->getPosition(); + const double radius = 10.0; // radius of circle + point pt; + pt.x(pos.x()); + pt.y(pos.y()); + boost::geometry::model::multi_polygon result; + { + const int points_per_circle = 36; + buffer::distance_symmetric distance_strategy(radius); + buffer::join_round join_strategy(points_per_circle); + buffer::end_round end_strategy(points_per_circle); + buffer::point_circle circle_strategy(points_per_circle); + buffer::side_straight side_strategy; + boost::geometry::buffer( + pt, result, distance_strategy, side_strategy, join_strategy, end_strategy, circle_strategy); + } + if (result.empty()) { + return; + } + const auto max_velocity_circle = result.front(); + auto velocity_space = max_velocity_circle; + for (auto line : agent.second->agent_orca_lines_) { + RVO::Vector2 scaled_dir = line.direction * (radius * 2); + RVO::Vector2 scaled_dir_norm(scaled_dir.y(), -scaled_dir.x()); + + polygon orca_line; + auto base_point = pos + line.point; + auto p1 = base_point + scaled_dir; + auto p2 = base_point - scaled_dir; + auto p3 = p2 + scaled_dir_norm; + auto p4 = p1 + scaled_dir_norm; + orca_line.outer().push_back({p1.x(), p1.y()}); + orca_line.outer().push_back({p2.x(), p2.y()}); + orca_line.outer().push_back({p3.x(), p3.y()}); + orca_line.outer().push_back({p4.x(), p4.y()}); + + std::vector out; + bg::intersection(velocity_space, orca_line, out); + if (out.size() == 1) { + velocity_space = out[0]; + } + } + + for (auto line : agent.second->obst_orca_lines_) { + RVO::Vector2 scaled_dir = line.direction * (radius * 2); + RVO::Vector2 scaled_dir_norm(scaled_dir.y(), -scaled_dir.x()); + + polygon orca_line; + auto base_point = pos + line.point; + auto p1 = base_point + scaled_dir; + auto p2 = base_point - scaled_dir; + auto p3 = p2 + scaled_dir_norm; + auto p4 = p1 + scaled_dir_norm; + orca_line.outer().push_back({p1.x(), p1.y()}); + orca_line.outer().push_back({p2.x(), p2.y()}); + orca_line.outer().push_back({p3.x(), p3.y()}); + orca_line.outer().push_back({p4.x(), p4.y()}); + + std::vector out; + bg::intersection(velocity_space, orca_line, out); + if (out.size() == 1) { + velocity_space = out[0]; + } + } + + for (auto p : velocity_space.outer()) { + geometry_msgs::msg::Point point_msg; + point_msg.x = p.x(); + point_msg.y = p.y(); + point_msg.z = 0.0; + marker.points.push_back(point_msg); + } + geometry_msgs::msg::Point first_point_msg; + first_point_msg.x = velocity_space.outer().front().x(); + first_point_msg.y = velocity_space.outer().front().y(); + first_point_msg.z = 0.0; + marker.points.push_back(first_point_msg); + marker.lifetime = rclcpp::Duration::from_seconds(0.1); + marker_array.markers.emplace_back(marker); + } +} + +void VisualizeMarker::visualizeAgentVelocity( + visualization_msgs::msg::MarkerArray & marker_array, const RVOSimulator & sim_) +{ + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Clock().now(); + marker.id = 0; + marker.ns = "velocity"; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.type = visualization_msgs::msg::Marker::LINE_LIST; + marker.scale.x = 0.05; + marker.color.a = 1.0; + marker.color.r = 1; + marker.color.g = 0.5; + marker.color.b = 0.5; + for (auto agent : sim_.getAgents()) { + auto vel = agent.second->getVelocity(); + auto pos = agent.second->getPosition(); + geometry_msgs::msg::Point msg; + msg.x = pos.x(); + msg.y = pos.y(); + msg.z = 1.0; + marker.points.push_back(msg); + + msg.x += vel.x(); + msg.y += vel.y(); + marker.points.emplace_back(msg); + } + marker.lifetime = rclcpp::Duration::from_seconds(0.1); + marker_array.markers.emplace_back(marker); +} + +void VisualizeMarker::visualizeAgentPrefVelocity( + visualization_msgs::msg::MarkerArray & marker_array, const RVOSimulator & sim_) +{ + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Clock().now(); + marker.id = 0; + marker.ns = "pref_velocity"; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.type = visualization_msgs::msg::Marker::LINE_LIST; + marker.scale.x = 0.05; + marker.color.a = 0.5; + marker.color.r = 1; + marker.color.g = 0.5; + marker.color.b = 0.5; + for (auto agent : sim_.getAgents()) { + auto pref_vel = agent.second->getPrefVelocity(); + auto pos = agent.second->getPosition(); + geometry_msgs::msg::Point msg; + msg.x = pos.x(); + msg.y = pos.y(); + msg.z = 1.0; + marker.points.push_back(msg); + + msg.x += pref_vel.x(); + msg.y += pref_vel.y(); + marker.points.push_back(msg); + } + marker.lifetime = rclcpp::Duration::from_seconds(0.1); + marker_array.markers.emplace_back(marker); +} + +/** + * + */ +void VisualizeMarker::visualizeAllMarkers( + visualization_msgs::msg::MarkerArray & marker_array, RVOSimulator & sim_) +{ + visualizeAgents(marker_array, sim_); + visualizeObstacles(marker_array, sim_); + visualizeObstacleORCALine(marker_array, sim_); + visualizeAgentORCALine(marker_array, sim_); + visualizeVelocitySpace(marker_array, sim_); + visualizeAgentVelocity(marker_array, sim_); + visualizeAgentPrefVelocity(marker_array, sim_); +} + +void VisualizeMarker::visualizeNextGoal( + visualization_msgs::msg::MarkerArray & marker_array, const geometry_msgs::msg::Point & goal_point) +{ + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Clock().now(); + marker.id = 0; + marker.ns = "nextgoal"; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.type = visualization_msgs::msg::Marker::CYLINDER; + + marker.pose.position = goal_point; + marker.pose.orientation.w = 1.0; + + double diameter = 0.5; + marker.scale.x = diameter; // diameter in x direction + marker.scale.y = diameter; // diameter in x direction + marker.scale.z = 1; // height + + marker.color.a = 0.5; + marker.color.r = 0; + marker.color.g = 1.0; + marker.color.b = 0; + marker.lifetime = rclcpp::Duration::from_seconds(0.1); + marker_array.markers.emplace_back(marker); +} + +void VisualizeMarker::visualizeGoalPoses( + visualization_msgs::msg::MarkerArray & marker_array, + const std::vector & goal_poses) +{ + int id = 0; + for (const auto goal_pose : goal_poses) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Clock().now(); + marker.id = id; + marker.ns = "goalposes"; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.type = visualization_msgs::msg::Marker::CYLINDER; + + marker.pose.position = goal_pose.position; + marker.pose.orientation.w = 1.0; + + double diameter = 0.15; + marker.scale.x = diameter; // diameter in x direction + marker.scale.y = diameter; // diameter in x direction + marker.scale.z = 1; // height + + marker.color.a = 0.5; + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + marker.lifetime = rclcpp::Duration::from_seconds(0.1); + marker_array.markers.emplace_back(marker); + ++id; + } +} + +} // namespace RVO + +#endif // CUSTOMIZED_RVO2__VISUALIZEMARKER_CPP_ \ No newline at end of file diff --git a/simulation/context_gamma_planner/customized_rvo2/test/src/test_rvo2.cpp b/simulation/context_gamma_planner/customized_rvo2/test/src/test_rvo2.cpp new file mode 100644 index 00000000000..7277d86b908 --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2/test/src/test_rvo2.cpp @@ -0,0 +1,425 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +class Rvo2Test : public ::testing::Test +{ +protected: + virtual void SetUp() { simulator_ = std::make_unique(0.025f); } + virtual void TearDown() { simulator_.release(); } + +public: + std::unique_ptr simulator_; +}; + +bool checkVector2(const RVO::Vector2 & vec0, const RVO::Vector2 & vec1, float torelance = 0.01) +{ + float diff = std::hypot(vec1.x() - vec0.x(), vec1.y() - vec0.y()); + if (diff <= torelance) { + return true; + } + std::cout << "vec0:" << vec0.x() << "," << vec0.y() << std::endl; + std::cout << "vec1:" << vec1.x() << "," << vec1.y() << std::endl; + return false; +} + +TEST_F(Rvo2Test, UpdateEmpty) { simulator_->update(); } + +TEST_F(Rvo2Test, addAgent) +{ + std::shared_ptr agent = std::make_shared("agent", RVO::Vector2(0, 0)); + simulator_->addAgent(agent); +} + +TEST_F(Rvo2Test, addAgentWithSameName) +{ + std::shared_ptr agent = std::make_shared("agent", RVO::Vector2(0, 0)); + EXPECT_NO_THROW(simulator_->addAgent(agent)); + EXPECT_THROW(simulator_->addAgent(agent), common::SimulationError); +} + +//TEST_F(Rvo2Test, getAgent) +//{ +// std::shared_ptr agent = std::make_shared("agent", RVO::Vector2(0, 0)); +// simulator_->addAgent(agent); +// EXPECT_NO_THROW(simulator_->getAgent("agent")); +// EXPECT_STREQ(simulator_->getAgent("agent")->name.c_str(), "agent"); +// EXPECT_THROW(simulator_->getAgent("expect_fail"), common::SimulationError); +//} + +//TEST_F(Rvo2Test, getId) +//{ +// std::shared_ptr agent = std::make_shared("agent", RVO::Vector2(0, 0)); +// simulator_->addAgent(agent); +// EXPECT_EQ(simulator_->getId("agent"), 0); +// EXPECT_THROW(simulator_->getId("expect_fail"), common::SimulationError); +//} + +TEST_F(Rvo2Test, getIdAgent) +{ + std::shared_ptr agent = std::make_shared("agent", RVO::Vector2(0, 0)); + EXPECT_EQ(agent->getId(), 0); +} + +TEST_F(Rvo2Test, setId) +{ + std::shared_ptr agent = std::make_shared("agent", RVO::Vector2(0, 0)); + simulator_->addAgent(agent); + EXPECT_EQ(simulator_->getId("agent"), 0); + agent->setId(2); + EXPECT_EQ(simulator_->getId("agent"), 2); +} + +TEST_F(Rvo2Test, getPosition) +{ + std::shared_ptr agent = std::make_shared("agent", RVO::Vector2(0, 1.0)); + simulator_->addAgent(agent); + EXPECT_FLOAT_EQ(simulator_->getPosition("agent").x(), 0.0); + EXPECT_FLOAT_EQ(simulator_->getPosition("agent").y(), 1.0); +} + +TEST_F(Rvo2Test, getOrientation) +{ + std::shared_ptr agent = std::make_shared("agent", RVO::Vector2(0, 1.0)); + simulator_->addAgent(agent); + geometry_msgs::msg::Quaternion orientation; + orientation.x = 0.0; + orientation.y = 0.0; + orientation.z = 1.0; + orientation.w = 0.0; + agent->setOrientation(orientation); + EXPECT_FLOAT_EQ(simulator_->getAgent("agent")->getOrientation().x, 0.0); + EXPECT_FLOAT_EQ(simulator_->getAgent("agent")->getOrientation().y, 0.0); + EXPECT_FLOAT_EQ(simulator_->getAgent("agent")->getOrientation().z, 1.0); + EXPECT_FLOAT_EQ(simulator_->getAgent("agent")->getOrientation().w, 0.0); + EXPECT_FLOAT_EQ(simulator_->getAgent("agent")->getOrientationYaw(), -M_PI); +} + +TEST_F(Rvo2Test, getAcceleration) +{ + std::shared_ptr agent = std::make_shared("agent", RVO::Vector2(0, 1.0)); + simulator_->addAgent(agent); + agent->setMaxAcceleration(10.0); + agent->setMaxDeceleration(-10.0); + EXPECT_FLOAT_EQ(simulator_->getAgent("agent")->getMaxAcceleration(), 10.0); + EXPECT_FLOAT_EQ(simulator_->getAgent("agent")->getMaxDeceleration(), -10.0); +} + +TEST_F(Rvo2Test, getPositionAgent) +{ + std::shared_ptr agent = std::make_shared("agent", RVO::Vector2(0, 1.0)); + EXPECT_FLOAT_EQ(agent->getPosition().x(), 0.0); + EXPECT_FLOAT_EQ(agent->getPosition().y(), 1.0); +} + +//TEST(Rvo2Test, constructors){ +// RVO::AgentConfig config; +// EXPECT_FLOAT_EQ(RVO::Agent("agent1", RVO::Vector2(0, 1.0)).getPosition().y(),1.0); +//// EXPECT_EQ(std::make_shared("agent2", RVO::Vector2(0, 1.0),config)->getId(),0); +//} + +TEST_F(Rvo2Test, getAgentConfig) +{ + RVO::AgentConfig config; + config.max_speed = 1000.0; + std::shared_ptr agent = + std::make_shared("agent", RVO::Vector2(0, 1.0), config); + simulator_->addAgent(agent); + EXPECT_FLOAT_EQ(simulator_->getAgentConfig("agent").max_speed, 1000.0); +} + +TEST_F(Rvo2Test, getAgentConfigAgent) +{ + RVO::AgentConfig config; + config.max_speed = 1000.0; + std::shared_ptr agent = + std::make_shared("agent", RVO::Vector2(0, 1.0), config); + EXPECT_FLOAT_EQ(agent->getAgentConfig().max_speed, 1000.0); +} + +TEST_F(Rvo2Test, addWaypoint) +{ + std::shared_ptr agent = std::make_shared("agent", RVO::Vector2(0, 0)); + agent->addWaypoint(RVO::Vector2(1, 0)); + simulator_->addAgent(agent); +} + +TEST_F(Rvo2Test, addWaypoints) +{ + std::shared_ptr agent = std::make_shared("agent", RVO::Vector2(0, 0)); + agent->addWaypoints({RVO::Vector2(1, 0), RVO::Vector2(2, 0)}); + simulator_->addAgent(agent); +} + +TEST_F(Rvo2Test, agentExists) +{ + std::shared_ptr agent = std::make_shared("agent", RVO::Vector2(0, 0)); + EXPECT_FALSE(simulator_->agentExist("agent")); + EXPECT_FALSE(simulator_->agentExist("expect_false")); + simulator_->addAgent(agent); + EXPECT_TRUE(simulator_->agentExist("agent")); + EXPECT_FALSE(simulator_->agentExist("expect_false")); +} + +TEST_F(Rvo2Test, addObstacle) +{ + std::shared_ptr agent = std::make_shared("agent", RVO::Vector2(0, 0)); + std::vector wrong_obstacle = {RVO::Vector2(-1, -1)}; + EXPECT_THROW(simulator_->addGlobalObstacle(wrong_obstacle, false), common::SimulationError); +} + +TEST_F(Rvo2Test, getObstacleLines) +{ + std::shared_ptr agent = std::make_shared("agent", RVO::Vector2(0, 0)); + simulator_->addAgent(agent); + simulator_->addGlobalObstacle( + {RVO::Vector2(-1, -1), RVO::Vector2(-1, 1), RVO::Vector2(-1, 2)}, true); + EXPECT_EQ(simulator_->getObstacleLines().size(), static_cast(6)); +} + +TEST_F(Rvo2Test, goForward) +{ + std::shared_ptr agent = std::make_shared("agent", RVO::Vector2(0, 0)); + agent->addWaypoint(RVO::Vector2(1, 0)); + simulator_->addAgent(agent); + const auto position = simulator_->getAgents().at("agent")->getPosition(); + EXPECT_TRUE(checkVector2(position, RVO::Vector2(0, 0))); + while (simulator_->getGlobalTime() <= 10) { + simulator_->update(); + } + const auto position_end = simulator_->getAgents().at("agent")->getPosition(); + EXPECT_TRUE(checkVector2(position_end, RVO::Vector2(1.0, 0))); +} + +TEST_F(Rvo2Test, goForwardWithStraightLane) +{ + std::shared_ptr agent = std::make_shared("agent", RVO::Vector2(0, 0)); + agent->addWaypoint(RVO::Vector2(1, 0)); + simulator_->addAgent(agent); + simulator_->addGlobalObstacle({RVO::Vector2(-1, 1), RVO::Vector2(2, 1)}); + simulator_->addGlobalObstacle({RVO::Vector2(-1, 1), RVO::Vector2(2, 1)}); + const auto position = simulator_->getAgents().at("agent")->getPosition(); + EXPECT_TRUE(checkVector2(position, RVO::Vector2(0, 0))); + while (simulator_->getGlobalTime() <= 10) { + simulator_->update(); + } + const auto position_end = simulator_->getAgents().at("agent")->getPosition(); + EXPECT_TRUE(checkVector2(position_end, RVO::Vector2(1.0, 0))); +} + +TEST_F(Rvo2Test, passIntersection) +{ + RVO::AgentConfig conf; + conf.radius = 1.0f; + conf.max_speed = 2.0f; + conf.neighbor_dist = 4.0f; + conf.time_horizon = 20.0f; + conf.time_horizon_obst = 20.0f; + auto agent1 = std::make_shared("agent1", RVO::Vector2(20.0f, 0.1f), conf); + agent1->addWaypoint(RVO::Vector2(-20.0f, 0.0f)); + simulator_->addAgent(agent1); + + auto agent2 = std::make_shared("agent2", RVO::Vector2(-20.0f, -0.1f), conf); + agent2->addWaypoint(RVO::Vector2(20.0f, 0.0f)); + simulator_->addAgent(agent2); + + std::vector obstacle1, obstacle2, obstacle3, obstacle4; + + obstacle1.push_back(RVO::Vector2(4.0f, 6.0f)); + obstacle1.push_back(RVO::Vector2(6.0f, 4.0f)); + obstacle1.push_back(RVO::Vector2(20.0f, 4.0f)); + obstacle1.push_back(RVO::Vector2(20.0f, 20.0f)); + obstacle1.push_back(RVO::Vector2(4.0f, 20.0f)); + + obstacle2.push_back(RVO::Vector2(4.0f, -20.0f)); + obstacle2.push_back(RVO::Vector2(20.0f, -20.0f)); + obstacle2.push_back(RVO::Vector2(20.0f, -4.0f)); + obstacle2.push_back(RVO::Vector2(6.0f, -4.0f)); + obstacle2.push_back(RVO::Vector2(4.0f, -6.0f)); + + obstacle3.push_back(RVO::Vector2(-4.0f, -6.0f)); + obstacle3.push_back(RVO::Vector2(-6.0f, -4.0f)); + obstacle3.push_back(RVO::Vector2(-20.0f, -4.0f)); + obstacle3.push_back(RVO::Vector2(-20.0f, -20.0f)); + obstacle3.push_back(RVO::Vector2(-4.0f, -20.0f)); + + obstacle4.push_back(RVO::Vector2(-4.0f, 20.0f)); + obstacle4.push_back(RVO::Vector2(-20.0f, 20.0f)); + obstacle4.push_back(RVO::Vector2(-20.0f, 4.0f)); + obstacle4.push_back(RVO::Vector2(-6.0f, 4.0f)); + obstacle4.push_back(RVO::Vector2(-4.0f, 6.0f)); + + simulator_->addGlobalObstacle(obstacle1); + simulator_->addGlobalObstacle(obstacle2); + simulator_->addGlobalObstacle(obstacle3); + simulator_->addGlobalObstacle(obstacle4); + + // distance : 40[m] + // max speed : 2.0[m/s] + // => expected : over 20[s] + while (simulator_->getGlobalTime() <= 25) { + simulator_->update(); + } + + EXPECT_TRUE( + checkVector2(simulator_->getAgents().at("agent1")->getPosition(), RVO::Vector2(-20, 0))); + EXPECT_TRUE( + checkVector2(simulator_->getAgents().at("agent2")->getPosition(), RVO::Vector2(20, 0))); +} + +TEST_F(Rvo2Test, checkAgentIgnoreFunction) +{ + RVO::AgentConfig conf; + conf.radius = 1.0f; + conf.max_speed = 2.0f; + conf.neighbor_dist = 4.0f; + conf.time_horizon = 20.0f; + conf.time_horizon_obst = 20.0f; + conf.entity_type = traffic_simulator_msgs::msg::EntityType::VEHICLE; + + auto vehicle_no_avoid = + std::make_shared("vehicle_no_avoid", RVO::Vector2(2.f, 1.5f), conf); + vehicle_no_avoid->addWaypoint({22.0f, 1.5f}); + vehicle_no_avoid->addIgnoreList(traffic_simulator_msgs::msg::EntityType::PEDESTRIAN); + simulator_->addAgent(vehicle_no_avoid); + + auto vehicle_pedestrian_avoid = + std::make_shared("vehicle_pedestrian_avoid", RVO::Vector2(-2.f, 1.5f), conf); + vehicle_pedestrian_avoid->addWaypoint({18.0f, 1.5f}); + simulator_->addAgent(vehicle_pedestrian_avoid); + + conf.entity_type = traffic_simulator_msgs::msg::EntityType::PEDESTRIAN; + auto pedestrian = std::make_shared("pedestrian", RVO::Vector2(10.f, 2.f), conf); + pedestrian->addIgnoreList(traffic_simulator_msgs::msg::EntityType::VEHICLE); + pedestrian->addWaypoint({10.f, 2.f}); + simulator_->addAgent(pedestrian); + + bool has_pass_through_pedestrian[2] = {false, false}; + while (simulator_->getGlobalTime() <= 25) { + simulator_->update(); + // check each agent to pass through the pedestrian + if (absSq(vehicle_no_avoid->getPosition() - pedestrian->getPosition()) < 1.0f) { + has_pass_through_pedestrian[0] = true; + } + if (absSq(vehicle_pedestrian_avoid->getPosition() - pedestrian->getPosition()) < 1.0f) { + has_pass_through_pedestrian[1] = true; + } + } + // no avoidance + EXPECT_TRUE(has_pass_through_pedestrian[0]); + // with avoidance + EXPECT_FALSE(has_pass_through_pedestrian[1]); +} + +TEST_F(Rvo2Test, obstaclePluginGetType) +{ + ObstaclePlugin plugin; + EXPECT_EQ(plugin.getType(), OrcaPluginBase::PluginType::OBSTACLE); +} + +TEST_F(Rvo2Test, agentPluginGetType) +{ + AgentPlugin plugin; + EXPECT_EQ(plugin.getType(), OrcaPluginBase::PluginType::AGENT); +} + +TEST_F(Rvo2Test, critical_corner_case) +{ + float center_x = 0; + float center_y = 0; + float inside_radius = 100; + float outside_radius = inside_radius + 10.0f; + auto pos1 = RVO::Vector2(inside_radius + 4.0f, 0.0f); + auto pos2 = RVO::Vector2( + center_x + (inside_radius + 4.0f) * std::cos(M_PI_2), + center_y + (inside_radius + 4.0f) * std::sin(M_PI_2)); + + RVO::AgentConfig conf; + // If radius is set to 2.0, the agent cannot avoid the obstacle and stops. + // If radius is set to 1.0 and smaller, and neighbor_dist is set to 6 or higher, the agent can go over the obstacle, but contact occurs. + conf.radius = 1.0f; // fail 2.0f + conf.max_speed = 2.0f; + conf.neighbor_dist = 6.0f; // fail 5.0f, success 6.0f 10.0f + conf.time_horizon = 10.0f; + conf.time_horizon_obst = 10.0f; + auto agent1 = std::make_shared("agent1", pos1, conf); + simulator_->addAgent(agent1); + + std::vector curve_outside, curve_inside, obstacle; + for (float theta = 0; theta < M_PI_2; theta += 0.01f) { + curve_inside.push_back(RVO::Vector2( + center_x + inside_radius * std::cos(theta), center_y + inside_radius * std::sin(theta))); + curve_outside.push_back(RVO::Vector2( + center_x + outside_radius * std::cos(M_PI_2 - theta), + center_y + outside_radius * std::sin(M_PI_2 - theta))); + + agent1->addWaypoint(RVO::Vector2( + center_x + (inside_radius + 4.0f) * std::cos(theta), + center_y + (inside_radius + 4.0f) * std::sin(theta))); + } + agent1->addWaypoint(pos1); + + curve_inside.push_back(RVO::Vector2(center_x, center_y)); + curve_outside.push_back(RVO::Vector2(center_x + outside_radius, center_y + outside_radius)); + + obstacle.push_back(RVO::Vector2( + center_x + inside_radius * std::cos(M_PI / 5) + 3.0f, + center_y + inside_radius * std::sin(M_PI / 5) + 3.0f)); + obstacle.push_back(RVO::Vector2( + center_x + inside_radius * std::cos(M_PI / 5), center_y + inside_radius * std::sin(M_PI / 5))); + + simulator_->addGlobalObstacle(curve_inside); + simulator_->addGlobalObstacle(curve_outside); + simulator_->addGlobalObstacle(obstacle); + + bool has_collided = false; + bool has_reached_goal = false; + auto obstacle_pos = RVO::Vector2( + center_x + inside_radius * std::cos(M_PI / 5) + 1.5f, + center_y + inside_radius * std::sin(M_PI / 5) + 1.5f); + while (simulator_->getGlobalTime() <= 150) { + simulator_->update(); + + if (absSq(agent1->getPosition() - pos2) < 1.0f) { + has_reached_goal = true; + } + + if (absSq(agent1->getPosition() - obstacle_pos) < conf.radius) { + has_collided = true; + } + } + EXPECT_TRUE(has_reached_goal); + EXPECT_FALSE(has_collided); +} +/* +TEST_F(Rvo2Test, chenge_agent_speed) +{ + std::shared_ptr agent = std::make_shared("agent", RVO::Vector2(0, 0)); + simulator_->addAgent(agent); + auto plugin = std::make_shared(); + plugin->setTargetSpeed(15); + simulator_->addAgentORCAPlugin(plugin); + plugin->setTargetSpeed(15); + EXPECT_DOUBLE_EQ(simulator_->getAgent("agent")->getAgentConfig().max_speed, 15); +}*/ + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/simulation/context_gamma_planner/customized_rvo2_examples/CMakeLists.txt b/simulation/context_gamma_planner/customized_rvo2_examples/CMakeLists.txt new file mode 100644 index 00000000000..5ccaf71b52c --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2_examples/CMakeLists.txt @@ -0,0 +1,81 @@ +cmake_minimum_required(VERSION 3.5) +project(customized_rvo2_examples) + +# find dependencies +find_package(ament_cmake_auto REQUIRED) +#find_package(OpenMP REQUIRED) +#set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") +#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") +find_package(Boost REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_add_executable(circle_node src/customized_rvo2_examples/circle_example_node.cpp) +ament_auto_add_executable(block_node src/customized_rvo2_examples/block_example_node.cpp) +ament_auto_add_executable(intersection_node src/customized_rvo2_examples/intersection_example_node.cpp) +ament_auto_add_executable(curve_node src/customized_rvo2_examples/curve_example_node.cpp) +ament_auto_add_executable(test_single_agent_node src/customized_rvo2_examples/single_agent_avoid_node.cpp) +ament_auto_add_executable(test_multiple_agent_node src/customized_rvo2_examples/multiple_agent_avoid_node.cpp) +ament_auto_add_executable(road_demo src/customized_rvo2_examples/road_demo_node.cpp) +ament_auto_add_executable(agent_ignore_node src/customized_rvo2_examples/agent_ignore_node.cpp) +ament_auto_add_executable(critical_corner_case src/customized_rvo2_examples/critical_corner_case.cpp) +ament_auto_add_executable(grid_obstacle_node src/customized_rvo2_examples/grid_obstacle_node.cpp) +ament_auto_add_executable(dynamic_velocity_node src/customized_rvo2_examples/dynamic_velocity_node.cpp) +include_directories(PRIVATE ${GENERATED_PATH}) +include_directories(include) +link_libraries(${Boost_LIBRARIES}) + +# workaround to allow deprecated header to build on both galactic and humble +if(${tf2_geometry_msgs_VERSION} VERSION_LESS 0.18.0) + target_compile_definitions(circle_node PUBLIC + USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER + ) + target_compile_definitions(block_node PUBLIC + USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER + ) + target_compile_definitions(intersection_node PUBLIC + USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER + ) + target_compile_definitions(curve_node PUBLIC + USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER + ) + target_compile_definitions(test_single_agent_node PUBLIC + USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER + ) + target_compile_definitions(test_multiple_agent_node PUBLIC + USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER + ) + target_compile_definitions(road_demo PUBLIC + USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER + ) + target_compile_definitions(agent_ignore_node PUBLIC + USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER + ) + target_compile_definitions(critical_corner_case PUBLIC + USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER + ) + target_compile_definitions(grid_obstacle_node PUBLIC + USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER + ) + target_compile_definitions(dynamic_velocity_node PUBLIC + USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER + ) +endif() + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(test_examples test/src/test_examples.cpp) + ament_target_dependencies(test_examples customized_rvo2) + if(${tf2_geometry_msgs_VERSION} VERSION_LESS 0.18.0) + target_compile_definitions(test_examples PUBLIC + USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER + ) + endif() +endif() + +install( + DIRECTORY launch + DESTINATION share/${PROJECT_NAME}) + +ament_auto_package() diff --git a/simulation/context_gamma_planner/customized_rvo2_examples/include/customized_rvo2_examples/example_scenarios.hpp b/simulation/context_gamma_planner/customized_rvo2_examples/include/customized_rvo2_examples/example_scenarios.hpp new file mode 100644 index 00000000000..511cf2103bb --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2_examples/include/customized_rvo2_examples/example_scenarios.hpp @@ -0,0 +1,471 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CUSTOMIZED_RVO2_EXAMPLES__EXAMPLE_SCENARIO_HPP_ +#define CUSTOMIZED_RVO2_EXAMPLES__EXAMPLE_SCENARIO_HPP_ + +#include "customized_rvo2_examples/scenario.hpp" + +void setupAgentIgnoreScenario(RVO::Scenario & scenario) +{ + std::vector road_upper_bound, road_lower_bound; + road_upper_bound.push_back(RVO::Vector2(0.0f, 3.0f)); + road_upper_bound.push_back(RVO::Vector2(20.0f, 3.0f)); + road_upper_bound.push_back(RVO::Vector2(20.0f, 3.1f)); + road_upper_bound.push_back(RVO::Vector2(0.0f, 3.1f)); + + road_lower_bound.push_back(RVO::Vector2(0.0f, -3.1f)); + road_lower_bound.push_back(RVO::Vector2(20.0f, -3.1f)); + road_lower_bound.push_back(RVO::Vector2(20.0f, -3.0f)); + road_lower_bound.push_back(RVO::Vector2(0.0f, -3.0f)); + + scenario.addGlobalObstacle(road_upper_bound); + scenario.addGlobalObstacle(road_lower_bound); + + RVO::AgentConfig conf; + conf.radius = 1.0f; + conf.max_speed = 2.0f; + conf.neighbor_dist = 4.0f; + conf.time_horizon = 20.0f; + conf.time_horizon_obst = 20.0f; + conf.entity_type = traffic_simulator_msgs::msg::EntityType::VEHICLE; + + auto vehicle_no_avoid = + std::make_shared("vehicle_no_avoid", RVO::Vector2(2.f, 1.5f), conf); + vehicle_no_avoid->addWaypoint({22.0f, 1.5f}); + vehicle_no_avoid->addIgnoreList(traffic_simulator_msgs::msg::EntityType::PEDESTRIAN); + scenario.addAgent(vehicle_no_avoid); + + auto vehicle_pedestrian_avoid = + std::make_shared("vehicle_pedestrian_avoid", RVO::Vector2(-2.f, 1.5f), conf); + vehicle_pedestrian_avoid->addWaypoint({18.0f, 1.5f}); + scenario.addAgent(vehicle_pedestrian_avoid); + + conf.entity_type = traffic_simulator_msgs::msg::EntityType::PEDESTRIAN; + auto pedestrian = std::make_shared("pedestrian", RVO::Vector2(10.f, 2.f), conf); + pedestrian->addIgnoreList(traffic_simulator_msgs::msg::EntityType::VEHICLE); + pedestrian->addWaypoint({10.f, 2.f}); + scenario.addAgent(pedestrian); +} + +void setupBlockScenario(RVO::Scenario & scenario) +{ + for (size_t i = 0; i < 5; ++i) { + for (size_t j = 0; j < 5; ++j) { + auto agent1 = std::make_shared( + "agent1_" + std::to_string(i * 5 + j), RVO::Vector2(55.0f + i * 10.0f, 55.0f + j * 10.0f)); + agent1->addWaypoint(RVO::Vector2(-75.0f, -75.0f)); + scenario.addAgent(agent1); + + auto agent2 = std::make_shared( + "agent2_" + std::to_string(i * 5 + j), RVO::Vector2(-55.0f - i * 10.0f, 55.0f + j * 10.0f)); + agent2->addWaypoint(RVO::Vector2(75.0f, -75.0f)); + scenario.addAgent(agent2); + + auto agent3 = std::make_shared( + "agent3_" + std::to_string(i * 5 + j), RVO::Vector2(55.0f + i * 10.0f, -55.0f - j * 10.0f)); + agent3->addWaypoint(RVO::Vector2(-75.0f, 75.0f)); + scenario.addAgent(agent3); + + auto agent4 = std::make_shared( + "agent4_" + std::to_string(i * 5 + j), + RVO::Vector2(-55.0f - i * 10.0f, -55.0f - j * 10.0f)); + agent4->addWaypoint(RVO::Vector2(75.0f, 75.0f)); + scenario.addAgent(agent4); + } + } + + /* + * Add (polygonal) obstacles, specifying their vertices in counterclockwise + * order. + */ + std::vector obstacle1, obstacle2, obstacle3, obstacle4; + + obstacle1.push_back(RVO::Vector2(-10.0f, 40.0f)); + obstacle1.push_back(RVO::Vector2(-40.0f, 40.0f)); + obstacle1.push_back(RVO::Vector2(-40.0f, 10.0f)); + obstacle1.push_back(RVO::Vector2(-10.0f, 10.0f)); + + obstacle2.push_back(RVO::Vector2(10.0f, 40.0f)); + obstacle2.push_back(RVO::Vector2(10.0f, 10.0f)); + obstacle2.push_back(RVO::Vector2(40.0f, 10.0f)); + obstacle2.push_back(RVO::Vector2(40.0f, 40.0f)); + + obstacle3.push_back(RVO::Vector2(10.0f, -40.0f)); + obstacle3.push_back(RVO::Vector2(40.0f, -40.0f)); + obstacle3.push_back(RVO::Vector2(40.0f, -10.0f)); + obstacle3.push_back(RVO::Vector2(10.0f, -10.0f)); + + obstacle4.push_back(RVO::Vector2(-10.0f, -40.0f)); + obstacle4.push_back(RVO::Vector2(-10.0f, -10.0f)); + obstacle4.push_back(RVO::Vector2(-40.0f, -10.0f)); + obstacle4.push_back(RVO::Vector2(-40.0f, -40.0f)); + + scenario.addGlobalObstacle(obstacle1); + scenario.addGlobalObstacle(obstacle2); + scenario.addGlobalObstacle(obstacle3); + scenario.addGlobalObstacle(obstacle4); +} + +void setupCircleScenario(RVO::Scenario & scenario) +{ + for (float r = 50.0f; r < 200.f; r += 20.f) { + int n = static_cast(r / 5); + for (size_t i = 0; i < static_cast(n); ++i) { + auto agent = std::make_shared( + "agent_" + std::to_string(i) + "_" + std::to_string(r), + (r + static_cast(i) / n * 10.f) * + RVO::Vector2(std::cos(i * 2.0f * M_PI / n), std::sin(i * 2.0f * M_PI / n))); + agent->addWaypoint(-agent->getPosition()); + scenario.addAgent(agent); + } + } +} + +void setupCurveScenario(RVO::Scenario & scenario) +{ + float center_x = 0; + float center_y = 0; + float inside_radius = 100; + float outside_radius = inside_radius + 10.0f; + + /* + * Add agents, specifying their start position, and store their goals on the + * opposite side of the environment. + */ + + auto pos1 = RVO::Vector2(0.0f, inside_radius + 5.0f); + auto pos2 = RVO::Vector2(inside_radius + 5.0f, 0.0f); + auto agent1 = std::make_shared("agent1", pos1); + agent1->addWaypoint(pos2); + scenario.addAgent(agent1); + + auto agent2 = std::make_shared("agent2", pos2); + agent2->addWaypoint(pos1); + scenario.addAgent(agent2); + + /* + * Add (polygonal) obstacles, specifying their vertices in counterclockwise + * order. + */ + std::vector curve_outside, curve_inside; + + for (float theta = 0; theta < M_PI_2; theta += 0.01f) { + curve_inside.push_back(RVO::Vector2( + center_x + inside_radius * std::cos(theta), center_y + inside_radius * std::sin(theta))); + curve_outside.push_back(RVO::Vector2( + center_x + outside_radius * std::cos(M_PI_2 - theta), + center_y + outside_radius * std::sin(M_PI_2 - theta))); + agent2->addWaypoint(RVO::Vector2( + center_x + (outside_radius - 4.0f) * std::cos(M_PI_2 - theta), + center_y + (outside_radius - 4.0f) * std::sin(M_PI_2 - theta))); + agent1->addWaypoint(RVO::Vector2( + center_x + (inside_radius + 4.0f) * std::cos(theta), + center_y + (inside_radius + 4.0f) * std::sin(theta))); + } + + curve_inside.push_back(RVO::Vector2(center_x, center_y)); + + curve_outside.push_back(RVO::Vector2(center_x + outside_radius, center_y + outside_radius)); + + scenario.addGlobalObstacle(curve_inside); + scenario.addGlobalObstacle(curve_outside); +} + +void setupIntersectionScenario(RVO::Scenario & scenario) +{ + /* + * Add agents, specifying their start position, and store their goals on the + * opposite side of the environment. + */ + + RVO::AgentConfig conf; + conf.radius = 1.0f; + conf.max_speed = 2.0f; + conf.neighbor_dist = 4.0f; + conf.time_horizon = 20.0f; + conf.time_horizon_obst = 20.0f; + auto agent1 = std::make_shared("agent1", RVO::Vector2(20.0f, 0.1f), conf); + agent1->addWaypoint(RVO::Vector2(-20.0f, 0.0f)); + scenario.addAgent(agent1); + + auto agent2 = std::make_shared("agent2", RVO::Vector2(-20.0f, -0.1f), conf); + agent2->addWaypoint(RVO::Vector2(20.0f, 0.0f)); + scenario.addAgent(agent2); + + // 道路形状（交差点） + std::vector obstacle1, obstacle2, obstacle3, obstacle4; + + obstacle1.push_back(RVO::Vector2(4.0f, 6.0f)); + obstacle1.push_back(RVO::Vector2(6.0f, 4.0f)); + obstacle1.push_back(RVO::Vector2(20.0f, 4.0f)); + obstacle1.push_back(RVO::Vector2(20.0f, 20.0f)); + obstacle1.push_back(RVO::Vector2(4.0f, 20.0f)); + + obstacle2.push_back(RVO::Vector2(4.0f, -20.0f)); + obstacle2.push_back(RVO::Vector2(20.0f, -20.0f)); + obstacle2.push_back(RVO::Vector2(20.0f, -4.0f)); + obstacle2.push_back(RVO::Vector2(6.0f, -4.0f)); + obstacle2.push_back(RVO::Vector2(4.0f, -6.0f)); + + obstacle3.push_back(RVO::Vector2(-4.0f, -6.0f)); + obstacle3.push_back(RVO::Vector2(-6.0f, -4.0f)); + obstacle3.push_back(RVO::Vector2(-20.0f, -4.0f)); + obstacle3.push_back(RVO::Vector2(-20.0f, -20.0f)); + obstacle3.push_back(RVO::Vector2(-4.0f, -20.0f)); + + obstacle4.push_back(RVO::Vector2(-4.0f, 20.0f)); + obstacle4.push_back(RVO::Vector2(-20.0f, 20.0f)); + obstacle4.push_back(RVO::Vector2(-20.0f, 4.0f)); + obstacle4.push_back(RVO::Vector2(-6.0f, 4.0f)); + obstacle4.push_back(RVO::Vector2(-4.0f, 6.0f)); + + scenario.addGlobalObstacle(obstacle1); + scenario.addGlobalObstacle(obstacle2); + scenario.addGlobalObstacle(obstacle3); + scenario.addGlobalObstacle(obstacle4); +} + +void setupCriticalCornerCaseScenario(RVO::Scenario & scenario) +{ + float center_x = 0; + float center_y = 0; + float inside_radius = 100; + float outside_radius = inside_radius + 10.0f; + auto pos1 = RVO::Vector2(inside_radius + 4.0f, 0.0f); + + RVO::AgentConfig conf; + // If radius is set to 2.0, the agent cannot avoid the obstacle and stops. + // If radius is set to 1.0 and smaller, and neighbor_dist is set to 6 or higher, the agent can go over the obstacle, but contact occurs. + conf.radius = 2.0f; // fail 2.0f + conf.max_speed = 2.0f; + conf.neighbor_dist = 5.0f; // fail 5.0f, success 6.0f 10.0f + conf.time_horizon = 10.0f; + conf.time_horizon_obst = 10.0f; + auto agent1 = std::make_shared("agent1", pos1, conf); + scenario.addAgent(agent1); + + std::vector curve_outside, curve_inside, obstacle; + for (float theta = 0; theta < M_PI_2; theta += 0.01f) { + curve_inside.push_back(RVO::Vector2( + center_x + inside_radius * std::cos(theta), center_y + inside_radius * std::sin(theta))); + curve_outside.push_back(RVO::Vector2( + center_x + outside_radius * std::cos(M_PI_2 - theta), + center_y + outside_radius * std::sin(M_PI_2 - theta))); + + agent1->addWaypoint(RVO::Vector2( + center_x + (inside_radius + 4.0f) * std::cos(theta), + center_y + (inside_radius + 4.0f) * std::sin(theta))); + } + agent1->addWaypoint(pos1); + + curve_inside.push_back(RVO::Vector2(center_x, center_y)); + curve_outside.push_back(RVO::Vector2(center_x + outside_radius, center_y + outside_radius)); + + // obstacle.push_back(RVO::Vector2( + // center_x + inside_radius * std::cos(M_PI / 4), center_y + inside_radius * std::sin(M_PI / 4))); + // obstacle.push_back(RVO::Vector2( + // center_x + inside_radius * std::cos(M_PI / 4) + 3.0f, + // center_y + inside_radius * std::sin(M_PI / 4) + 3.0f)); + obstacle.push_back(RVO::Vector2( + center_x + inside_radius * std::cos(M_PI / 5) + 3.0f, + center_y + inside_radius * std::sin(M_PI / 5) + 3.0f)); + obstacle.push_back(RVO::Vector2( + center_x + inside_radius * std::cos(M_PI / 5), center_y + inside_radius * std::sin(M_PI / 5))); + // obstacle.push_back(RVO::Vector2( + // center_x + inside_radius * std::cos(M_PI / 4), center_y + inside_radius * std::sin(M_PI / 4))); + + scenario.addGlobalObstacle(curve_inside); + scenario.addGlobalObstacle(curve_outside); + scenario.addGlobalObstacle(obstacle); +} + +void setupRoadScenario(RVO::Scenario & scenario) +{ + std::vector curve_outside, curve_inside; + RVO::Vector2 pos1(0.f, 105.f); + RVO::Vector2 pos2(105.f, 0.f); + auto agent1 = std::make_shared("agent1", pos1); + auto agent2 = std::make_shared("agent2", pos2); + + float center_x = 0; + float center_y = 0; + float inside_radius = 100; + float outside_radius = inside_radius + 10.0f; + float polygon_width = 0.5f; + + for (float theta = 0; theta < M_PI_2; theta += 0.01f) { + curve_inside.push_back(RVO::Vector2( + center_x + inside_radius * std::cos(theta), center_y + inside_radius * std::sin(theta))); + curve_outside.push_back(RVO::Vector2( + center_x + outside_radius * std::cos(M_PI_2 - theta), + center_y + outside_radius * std::sin(M_PI_2 - theta))); + agent1->addWaypoint(RVO::Vector2( + center_x + (outside_radius - 3.0f) * std::cos(M_PI_2 - theta), + center_y + (outside_radius - 3.0f) * std::sin(M_PI_2 - theta))); + agent2->addWaypoint(RVO::Vector2( + center_x + (inside_radius + 3.0f) * std::cos(theta), + center_y + (inside_radius + 3.0f) * std::sin(theta))); + } + + scenario.addAgent(agent1); + scenario.addAgent(agent2); + + for (float theta = M_PI_2; theta > 0.f; theta -= 0.01f) { + curve_inside.push_back(RVO::Vector2( + center_x + (inside_radius - polygon_width) * std::cos(theta), + center_y + (inside_radius - polygon_width) * std::sin(theta))); + curve_outside.push_back(RVO::Vector2( + center_x + (outside_radius + polygon_width) * std::cos(M_PI_2 - theta), + center_y + (outside_radius + polygon_width) * std::sin(M_PI_2 - theta))); + } + + scenario.addGlobalObstacle(curve_inside); + scenario.addGlobalObstacle(curve_outside); +} + +void setupMultipleAgentAvoidScenario(RVO::Scenario & scenario) +{ + std::vector obstacle; + obstacle.push_back(RVO::Vector2(15.0f, 2.75f)); + obstacle.push_back(RVO::Vector2(11.0f, 2.75f)); + obstacle.push_back(RVO::Vector2(11.1f, 1.00f)); + obstacle.push_back(RVO::Vector2(15.0f, 1.00f)); + + scenario.addGlobalObstacle(obstacle); + + std::vector road_upper_bound, road_lower_bound; + road_upper_bound.push_back(RVO::Vector2(0.0f, 3.0f)); + road_upper_bound.push_back(RVO::Vector2(20.0f, 3.0f)); + road_upper_bound.push_back(RVO::Vector2(20.0f, 3.1f)); + road_upper_bound.push_back(RVO::Vector2(0.0f, 3.1f)); + + road_lower_bound.push_back(RVO::Vector2(0.0f, -3.1f)); + road_lower_bound.push_back(RVO::Vector2(20.0f, -3.1f)); + road_lower_bound.push_back(RVO::Vector2(20.0f, -3.0f)); + road_lower_bound.push_back(RVO::Vector2(0.0f, -3.0f)); + + scenario.addGlobalObstacle(road_upper_bound); + scenario.addGlobalObstacle(road_lower_bound); + + RVO::AgentConfig conf; + conf.radius = 1.0f; + conf.max_speed = 2.0f; + conf.neighbor_dist = 4.0f; + conf.time_horizon = 20.0f; + conf.time_horizon_obst = 20.0f; + + RVO::Vector2 pos1(0.f, 1.5f); + auto agent1 = std::make_shared("agent1", pos1, conf); + agent1->addWaypoint({20.0f, 1.5f}); + scenario.addAgent(agent1); + + RVO::Vector2 pos2(20.f, -1.5f); + auto agent2 = std::make_shared("agent2", pos2, conf); + agent2->addWaypoint({0.0f, -1.5f}); + scenario.addAgent(agent2); +} + +void setupSingleAgentAvoidScenario(RVO::Scenario & scenario) +{ + RVO::Vector2 pos(0.f, 0.f); + auto agent = std::make_shared("agent", pos); + + agent->addWaypoint({20.0f, 0.0f}); + agent->addWaypoint({0.0f, 0.0f}); + + scenario.addAgent(agent); + + std::vector obstacle; + obstacle.push_back(RVO::Vector2(11.0f, -0.1f)); + obstacle.push_back(RVO::Vector2(9.0f, -0.1f)); + obstacle.push_back(RVO::Vector2(9.0f, -3.f)); + obstacle.push_back(RVO::Vector2(11.0f, -3.f)); + + scenario.addGlobalObstacle(obstacle); +} + +void setupGridObstacleScenario(RVO::Scenario & scenario) +{ + RVO::AgentConfig conf; + conf.radius = 1.0f; + conf.max_speed = 2.0f; + conf.neighbor_dist = 1.0; //4.0f; + conf.time_horizon = 20.0f; + conf.time_horizon_obst = 10.0f; + + RVO::Vector2 pos1(-1.0f, -1.0f); + auto agent1 = std::make_shared("agent1", pos1, conf); + agent1->addWaypoint({31.0f, 31.0f}); + scenario.addAgent(agent1); + //scenario.addAgent(agent1); + + RVO::Vector2 pos2(31.0f, -1.0f); + auto agent2 = std::make_shared("agent2", pos2, conf); + agent2->addWaypoint({-1.0f, 31.0f}); + scenario.addAgent(agent2); + + RVO::Vector2 pos3(31.0f, 31.0f); + auto agent3 = std::make_shared("agent3", pos3, conf); + agent3->addWaypoint({-1.0f, -1.0f}); + scenario.addAgent(agent3); + + RVO::Vector2 pos4(-1.0f, 31.0f); + auto agent4 = std::make_shared("agent4", pos4, conf); + agent4->addWaypoint({31.0f, -1.0f}); + scenario.addAgent(agent4); + + float obstacle_size = 2.0; + float path_width = 5.0; + float obstacle_origin_x = 0.0; + float obstacle_origin_y = 0.0; + int obstacle_num = 5; + + for (int x = 0; x < obstacle_num; x++) { + for (int y = 0; y < obstacle_num; y++) { + std::vector obstacle; + obstacle.push_back(RVO::Vector2( + x * (obstacle_origin_x + obstacle_size + path_width), + obstacle_origin_y + y * (obstacle_size + path_width))); + obstacle.push_back(RVO::Vector2( + x * (obstacle_origin_x + obstacle_size + path_width) + obstacle_size, + obstacle_origin_y + y * (obstacle_size + path_width))); + obstacle.push_back(RVO::Vector2( + x * (obstacle_origin_x + obstacle_size + path_width) + obstacle_size, + obstacle_origin_y + y * (obstacle_size + path_width) + obstacle_size)); + obstacle.push_back(RVO::Vector2( + x * (obstacle_origin_x + obstacle_size + path_width), + obstacle_origin_y + y * (obstacle_size + path_width) + obstacle_size)); + //obstacles.push_back(obstacle); + scenario.addGlobalObstacle(obstacle); + } + } +} + +void setupDynamicVelocityScenario(RVO::Scenario & scenario) +{ + RVO::AgentConfig conf; + conf.radius = 1.0f; + conf.max_speed = 2.0f; + conf.neighbor_dist = 1.0; //4.0f; + conf.time_horizon = 20.0f; + conf.time_horizon_obst = 10.0f; + + RVO::Vector2 pos1(-0.0f, 0.0f); + auto agent1 = std::make_shared("agent1", pos1, conf); + agent1->addWaypoint({20.0f, 20.0f}); + scenario.addAgent(agent1); +} + +#endif // CUSTOMIZED_RVO2_EXAMPLES__EXAMPLE_SCENARIO_HPP_ diff --git a/simulation/context_gamma_planner/customized_rvo2_examples/include/customized_rvo2_examples/rvo2_ros2.hpp b/simulation/context_gamma_planner/customized_rvo2_examples/include/customized_rvo2_examples/rvo2_ros2.hpp new file mode 100755 index 00000000000..41eb0454a7e --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2_examples/include/customized_rvo2_examples/rvo2_ros2.hpp @@ -0,0 +1,393 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CUSTOMIZED_RVO2_EXAMPLES__RVO2_ROS2_HPP_ +#define CUSTOMIZED_RVO2_EXAMPLES__RVO2_ROS2_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "customized_rvo2/RVO.h" +#include "rclcpp/rclcpp.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +/** + * + */ +class RVO2ROS2Component : public rclcpp::Node +{ +public: + /** + * + * @param option + * @param time_scale + */ + explicit RVO2ROS2Component(const rclcpp::NodeOptions & option, float time_scale = 1.0f) + : rclcpp::Node("rvo2_ros2", option), ros_clock_(rcl_clock_type_t::RCL_ROS_TIME) + { + declare_parameter("visualize_velocity_space", false); + get_parameter("visualize_velocity_space", visualize_velocity_space_); + sim_ = new RVO::RVOSimulator(); + using std::literals::chrono_literals::operator""ms; + timer_ = + create_wall_timer(time_scale * 25ms, std::bind(&RVO2ROS2Component::timerCallback, this)); + marker_pub_ = create_publisher("marker", 1); + } + /** + * + */ + ~RVO2ROS2Component() { delete sim_; } + +protected: + /** + * + * @param marker_array + */ + void visualizeAgents(visualization_msgs::msg::MarkerArray & marker_array) + { + size_t i = 0; + for (const auto agent : sim_->getAgents()) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = ros_clock_.now(); + marker.id = i; + marker.ns = "mpc"; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.type = visualization_msgs::msg::Marker::CYLINDER; + + auto position = agent.second->getPosition(); + marker.pose.position.x = position.x(); + marker.pose.position.y = position.y(); + marker.pose.position.z = 0.0; + marker.pose.orientation.w = 1.0; + + double diameter = agent.second->getAgentConfig().radius * 2; + marker.scale.x = diameter; // diameter in x direction + marker.scale.y = diameter; // diameter in x direction + marker.scale.z = 0.5; // height + + marker.color.a = 0.5; + marker.color.r = 1; + marker.color.g = 0; + marker.color.b = 0; + + marker_array.markers.push_back(marker); + ++i; + } + } + + /** + * + * @param marker_array + */ + void visualizeObstacles(visualization_msgs::msg::MarkerArray & marker_array) + { + visualization_msgs::msg::Marker obstacle_marker; + obstacle_marker.header.frame_id = "map"; + obstacle_marker.header.stamp = ros_clock_.now(); + obstacle_marker.id = 0; + obstacle_marker.ns = "obstacle"; + obstacle_marker.action = visualization_msgs::msg::Marker::ADD; + obstacle_marker.type = visualization_msgs::msg::Marker::LINE_LIST; + obstacle_marker.scale.x = 0.2; + obstacle_marker.color.a = 1; + obstacle_marker.color.r = 1; + obstacle_marker.color.g = 1; + obstacle_marker.color.b = 0; + + auto lines = sim_->getObstacleLines(); + for (auto p : lines) { + geometry_msgs::msg::Point pt; + pt.x = p.x(); + pt.y = p.y(); + pt.z = 0.0; + obstacle_marker.points.push_back(pt); + } + + marker_array.markers.push_back(obstacle_marker); + } + + void visualizeObstacleORCALine(visualization_msgs::msg::MarkerArray & marker_array) + { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = ros_clock_.now(); + marker.id = 0; + marker.ns = "obstacle_orca"; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.type = visualization_msgs::msg::Marker::LINE_LIST; + marker.scale.x = 0.05; + marker.color.a = 0.5; + marker.color.r = 1; + marker.color.g = 1; + marker.color.b = 0.5; + + for (auto agent : sim_->getAgents()) { + auto pos = agent.second->getPosition(); + for (auto line : agent.second->obst_orca_lines_) { + geometry_msgs::msg::Point msg; + msg.x = pos.x() + line.point.x() - line.direction.x() * 2.0f; + msg.y = pos.y() + line.point.y() - line.direction.y() * 2.0f; + msg.z = 0.0; + marker.points.push_back(msg); + + msg.x += line.direction.x() * 4.0f; + msg.y += line.direction.y() * 4.0f; + marker.points.push_back(msg); + } + } + marker_array.markers.push_back(marker); + } + + void visualizeAgentORCALine(visualization_msgs::msg::MarkerArray & marker_array) + { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = ros_clock_.now(); + marker.id = 0; + marker.ns = "agent_orca"; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.type = visualization_msgs::msg::Marker::LINE_LIST; + marker.scale.x = 0.05; + marker.color.a = 0.5; + marker.color.r = 1; + marker.color.g = 0.5; + marker.color.b = 0.5; + + for (auto agent : sim_->getAgents()) { + auto pos = agent.second->getPosition(); + for (auto line : agent.second->agent_orca_lines_) { + geometry_msgs::msg::Point msg; + msg.x = pos.x() + line.point.x() - line.direction.x() * 2.0f; + msg.y = pos.y() + line.point.y() - line.direction.y() * 2.0f; + msg.z = 0.0; + marker.points.push_back(msg); + + msg.x += line.direction.x() * 4.0f; + msg.y += line.direction.y() * 4.0f; + marker.points.push_back(msg); + } + } + marker_array.markers.push_back(marker); + } + + void visualizeVelocitySpace(visualization_msgs::msg::MarkerArray & marker_array) + { + namespace bg = boost::geometry; + typedef bg::model::d2::point_xy point; + typedef bg::model::polygon polygon; + typedef bg::model::box box; + namespace buffer = boost::geometry::strategy::buffer; + + typedef boost::geometry::model::d2::point_xy point; + typedef boost::geometry::model::polygon polygon; + + int index = -1; + //#pragma omp parallel for + for (auto agent : sim_->getAgents()) { + index += 1; + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = ros_clock_.now(); + marker.id = index; + marker.ns = "velocity_space"; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.scale.x = 0.1; + marker.color.a = 1.0; + marker.color.r = 0.5; + marker.color.g = 1; + marker.color.b = 0.5; + + auto pos = agent.second->getPosition(); + const double radius = 10.0; // radius of circle + point pt; + pt.x(pos.x()); + pt.y(pos.y()); + boost::geometry::model::multi_polygon result; + { + const int points_per_circle = 36; + buffer::distance_symmetric distance_strategy(radius); + buffer::join_round join_strategy(points_per_circle); + buffer::end_round end_strategy(points_per_circle); + buffer::point_circle circle_strategy(points_per_circle); + buffer::side_straight side_strategy; + boost::geometry::buffer( + pt, result, distance_strategy, side_strategy, join_strategy, end_strategy, + circle_strategy); + } + const auto max_velocity_circle = result.front(); + auto velocity_space = max_velocity_circle; + for (auto line : agent.second->agent_orca_lines_) { + RVO::Vector2 scaled_dir = line.direction * (radius * 2); + RVO::Vector2 scaled_dir_norm(scaled_dir.y(), -scaled_dir.x()); + + polygon orca_line; + auto base_point = pos + line.point; + auto p1 = base_point + scaled_dir; + auto p2 = base_point - scaled_dir; + auto p3 = p2 + scaled_dir_norm; + auto p4 = p1 + scaled_dir_norm; + orca_line.outer().push_back({p1.x(), p1.y()}); + orca_line.outer().push_back({p2.x(), p2.y()}); + orca_line.outer().push_back({p3.x(), p3.y()}); + orca_line.outer().push_back({p4.x(), p4.y()}); + + std::vector out; + bg::intersection(velocity_space, orca_line, out); + if (out.size() == 1) { + velocity_space = out[0]; + } + } + + for (auto line : agent.second->obst_orca_lines_) { + RVO::Vector2 scaled_dir = line.direction * (radius * 2); + RVO::Vector2 scaled_dir_norm(scaled_dir.y(), -scaled_dir.x()); + + polygon orca_line; + auto base_point = pos + line.point; + auto p1 = base_point + scaled_dir; + auto p2 = base_point - scaled_dir; + auto p3 = p2 + scaled_dir_norm; + auto p4 = p1 + scaled_dir_norm; + orca_line.outer().push_back({p1.x(), p1.y()}); + orca_line.outer().push_back({p2.x(), p2.y()}); + orca_line.outer().push_back({p3.x(), p3.y()}); + orca_line.outer().push_back({p4.x(), p4.y()}); + + std::vector out; + bg::intersection(velocity_space, orca_line, out); + if (out.size() == 1) { + velocity_space = out[0]; + } + } + + for (auto p : velocity_space.outer()) { + geometry_msgs::msg::Point point_msg; + point_msg.x = p.x(); + point_msg.y = p.y(); + point_msg.z = 0.0; + marker.points.push_back(point_msg); + } + geometry_msgs::msg::Point first_point_msg; + first_point_msg.x = velocity_space.outer().front().x(); + first_point_msg.y = velocity_space.outer().front().y(); + first_point_msg.z = 0.0; + marker.points.push_back(first_point_msg); + + marker_array.markers.push_back(marker); + } + } + + void visualizeAgentVelocity(visualization_msgs::msg::MarkerArray & marker_array) + { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = ros_clock_.now(); + marker.id = 0; + marker.ns = "velocity"; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.type = visualization_msgs::msg::Marker::LINE_LIST; + marker.scale.x = 0.05; + marker.color.a = 1.0; + marker.color.r = 1; + marker.color.g = 0.5; + marker.color.b = 0.5; + for (auto agent : sim_->getAgents()) { + auto vel = agent.second->getVelocity(); + auto pos = agent.second->getPosition(); + geometry_msgs::msg::Point msg; + msg.x = pos.x(); + msg.y = pos.y(); + msg.z = 1.0; + marker.points.push_back(msg); + + msg.x += vel.x(); + msg.y += vel.y(); + marker.points.push_back(msg); + } + marker_array.markers.push_back(marker); + } + + void visualizeAgentPrefVelocity(visualization_msgs::msg::MarkerArray & marker_array) + { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = ros_clock_.now(); + marker.id = 0; + marker.ns = "pref_velocity"; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.type = visualization_msgs::msg::Marker::LINE_LIST; + marker.scale.x = 0.05; + marker.color.a = 0.5; + marker.color.r = 1; + marker.color.g = 0.5; + marker.color.b = 0.5; + for (auto agent : sim_->getAgents()) { + auto pref_vel = agent.second->getPrefVelocity(); + auto pos = agent.second->getPosition(); + geometry_msgs::msg::Point msg; + msg.x = pos.x(); + msg.y = pos.y(); + msg.z = 1.0; + marker.points.push_back(msg); + + msg.x += pref_vel.x(); + msg.y += pref_vel.y(); + marker.points.push_back(msg); + } + marker_array.markers.push_back(marker); + } + + /** + * + */ + void publishMarkers() + { + visualization_msgs::msg::MarkerArray marker_array; + + visualizeAgents(marker_array); + visualizeObstacles(marker_array); + visualizeObstacleORCALine(marker_array); + visualizeAgentORCALine(marker_array); + if (visualize_velocity_space_) { + visualizeVelocitySpace(marker_array); + } + visualizeAgentVelocity(marker_array); + visualizeAgentPrefVelocity(marker_array); + + marker_pub_->publish(marker_array); + } + /** + * + */ + void timerCallback() + { + sim_->update(); + publishMarkers(); + } + +protected: + RVO::RVOSimulator * sim_ = nullptr; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr marker_pub_; + rclcpp::Clock ros_clock_; + bool visualize_velocity_space_; +}; +#endif // CUSTOMIZED_RVO2_EXAMPLES__RVO2_ROS2_HPP_ diff --git a/simulation/context_gamma_planner/customized_rvo2_examples/include/customized_rvo2_examples/scenario.hpp b/simulation/context_gamma_planner/customized_rvo2_examples/include/customized_rvo2_examples/scenario.hpp new file mode 100644 index 00000000000..72e7272f646 --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2_examples/include/customized_rvo2_examples/scenario.hpp @@ -0,0 +1,93 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CUSTOMIZED_RVO2_EXAMPLES__SCENARIO_HPP_ +#define CUSTOMIZED_RVO2_EXAMPLES__SCENARIO_HPP_ + +#include +#include + +#include "customized_rvo2/Agent.hpp" +#include "customized_rvo2/RVOSimulator.hpp" + +namespace RVO +{ +/** + * + */ +struct Scenario +{ +public: + /** + * + * @param obstacle + */ + void addGlobalObstacle(std::vector obstacle) + { + auto obstacle_ptr = std::make_shared>(obstacle); + addGlobalObstacle(obstacle_ptr); + } + /** + * + * @param obstacle + */ + void addGlobalObstacle(std::shared_ptr> obstacle) + { + obstacles_.emplace_back(obstacle); + } + /** + * + * @param agent + */ + void addAgent(std::shared_ptr agent) { agents_.emplace_back(agent); } + /** + * + * @param sim + */ + void setupScenario(RVOSimulator * sim) + { + for (auto obstacle : obstacles_) { + sim->addGlobalObstacle(*obstacle, false); + } + sim->processObstacles(); + + for (auto & agent : agents_) { + sim->addAgent(agent); + } + } + + void setupScenario(std::shared_ptr sim) + { + for (auto obstacle : obstacles_) { + sim->addGlobalObstacle(*obstacle, false); + } + sim->processObstacles(); + + for (auto & agent : agents_) { + sim->addAgent(agent); + } + } + + /** + * + * @param sim + */ + void setupScenario(RVOSimulator & sim) { setupScenario(&sim); } + +private: + std::vector>> obstacles_; + std::vector> agents_; +}; +} // namespace RVO +#endif // CUSTOMIZED_RVO2_EXAMPLES__SCENARIO_HPP_ diff --git a/simulation/context_gamma_planner/customized_rvo2_examples/include/customized_rvo2_examples/visibility.hpp b/simulation/context_gamma_planner/customized_rvo2_examples/include/customized_rvo2_examples/visibility.hpp new file mode 100644 index 00000000000..18bf20e663e --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2_examples/include/customized_rvo2_examples/visibility.hpp @@ -0,0 +1,65 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CUSTOMIZED_RVO2_EXAMPLES__VISIBILITY_HPP_ +#define CUSTOMIZED_RVO2_EXAMPLES__VISIBILITY_HPP_ + +#ifdef __cplusplus +extern "C" { +#endif + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + +#ifdef __GNUC__ +#define RVO2_ROS2_EXPORT __attribute__((dllexport)) +#define RVO2_ROS2_IMPORT __attribute__((dllimport)) +#else +#define RVO2_ROS2_EXPORT __declspec(dllexport) +#define RVO2_ROS2_IMPORT __declspec(dllimport) +#endif + +#ifdef RVO2_ROS2_DLL +#define RVO2_ROS2_PUBLIC RVO2_ROS2_EXPORT +#else +#define RVO2_ROS2_PUBLIC RVO2_ROS2_IMPORT +#endif + +#define RVO2_ROS2_PUBLIC_TYPE RVO2_ROS2_PUBLIC + +#define RVO2_ROS2_LOCAL + +#else + +#define RVO2_ROS2_EXPORT __attribute__((visibility("default"))) +#define RVO2_ROS2_IMPORT + +#if __GNUC__ >= 4 +#define RVO2_ROS2_PUBLIC __attribute__((visibility("default"))) +#define RVO2_ROS2_LOCAL __attribute__((visibility("hidden"))) +#else +#define RVO2_ROS2_PUBLIC +#define RVO2_ROS2_LOCAL +#endif + +#define RVO2_ROS2_PUBLIC_TYPE +#endif + +#ifdef __cplusplus +} +#endif + +#endif // CUSTOMIZED_RVO2_EXAMPLES__VISIBILITY_HPP_ diff --git a/simulation/context_gamma_planner/customized_rvo2_examples/launch/run.launch.py b/simulation/context_gamma_planner/customized_rvo2_examples/launch/run.launch.py new file mode 100644 index 00000000000..2dfeb6c4707 --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2_examples/launch/run.launch.py @@ -0,0 +1,148 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +"""Launch description for scenario runner moc.""" + +# Copyright (c) 2020 Tier IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from logging import shutdown +import os +import sys + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +import launch + +from launch.conditions import IfCondition +from launch.events import Shutdown +from launch.event_handlers import OnProcessExit, OnProcessIO + +from launch.actions import EmitEvent, RegisterEventHandler, LogInfo, TimerAction +from launch.actions.declare_launch_argument import DeclareLaunchArgument +from launch.substitutions.launch_configuration import LaunchConfiguration + +from launch_ros.actions import Node + +from typing import cast + +from pathlib import Path + + +class Color: + BLACK = "\033[30m" + RED = "\033[31m" + GREEN = "\033[32m" + YELLOW = "\033[33m" + BLUE = "\033[34m" + PURPLE = "\033[35m" + CYAN = "\033[36m" + WHITE = "\033[37m" + END = "\033[0m" + BOLD = "\038[1m" + UNDERLINE = "\033[4m" + INVISIBLE = "\033[08m" + REVERSE = "\033[07m" + + +def on_stderr_output(event: launch.Event) -> None: + lines = event.text.decode().splitlines() + if len(lines) == 1: + if lines[0] == "cpp_scenario:failure": + print(Color.RED + "Scenario Failed" + Color.END) + + +def on_stdout_output(event: launch.Event) -> None: + lines = event.text.decode().splitlines() + if len(lines) == 1: + if lines[0] == "cpp_scenario:success": + print(Color.GREEN + "Scenario Succeed" + Color.END) + + +def generate_launch_description(): + scenario = LaunchConfiguration("scenario", default="") + scenario_package = LaunchConfiguration("package", default="customized_rvo2_examples") + launch_rviz = LaunchConfiguration("launch_rviz", default=False) + visualize_velocity_space = LaunchConfiguration("visualize_velocity_space", default="False") + + scenario_node = Node( + package=scenario_package, + executable=scenario, + name=scenario, + output="screen", + arguments=[("__log_level:=info")], + parameters=[{"visualize_velocity_space":visualize_velocity_space}], + ) + io_handler = OnProcessIO( + target_action=scenario_node, + on_stderr=on_stderr_output, + on_stdout=on_stdout_output, + ) + shutdown_handler = OnProcessExit( + target_action=scenario_node, on_exit=[EmitEvent(event=Shutdown())] + ) + description = LaunchDescription( + [ + DeclareLaunchArgument( + "scenario", default_value=scenario, description="Name of the scenario." + ), + DeclareLaunchArgument( + "package", + default_value=scenario_package, + description="Name of package your scenario exists", + ), + DeclareLaunchArgument( + "launch_rviz", + default_value=launch_rviz, + description="If true, launch with rviz.", + ), + DeclareLaunchArgument( + "visualize_velocity_space", + default_value=visualize_velocity_space, + description="If true, visualization of velocity space in each agent (this is very heavy process) will be enabled.", + ), + scenario_node, + RegisterEventHandler(event_handler=io_handler), + RegisterEventHandler(event_handler=shutdown_handler), + Node( + package="simple_sensor_simulator", + executable="simple_sensor_simulator_node", + name="simple_sensor_simulator_node", + output="log", + arguments=[("__log_level:=warn")], + ), + Node( + package="traffic_simulator", + executable="visualization_node", + name="visualizer", + output="screen", + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output={"stderr": "log", "stdout": "log"}, + condition=IfCondition(launch_rviz), + arguments=[ + "-d", + str( + Path(get_package_share_directory("customized_rvo2_examples")) + / "config/viewer.rviz" + ), + ], + ), + ] + ) + return description \ No newline at end of file diff --git a/simulation/context_gamma_planner/customized_rvo2_examples/package.xml b/simulation/context_gamma_planner/customized_rvo2_examples/package.xml new file mode 100644 index 00000000000..47c00e6ab7c --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2_examples/package.xml @@ -0,0 +1,29 @@ + + + + customized_rvo2_examples + 0.0.0 + Examples of customized_rvo2 + Kotaro Yoshimoto + Apache-2.0 + + ament_cmake_auto + + rclcpp + visualization_msgs + customized_rvo2 + yaml-cpp + traffic_simulator + tf2_geometry_msgs + + scenario_simulator_exception + ament_lint_auto + ament_cmake_clang_format + ament_cmake_copyright + ament_cmake_pep257 + ament_cmake_xmllint + + + ament_cmake + + \ No newline at end of file diff --git a/simulation/context_gamma_planner/customized_rvo2_examples/src/customized_rvo2_examples/agent_ignore_node.cpp b/simulation/context_gamma_planner/customized_rvo2_examples/src/customized_rvo2_examples/agent_ignore_node.cpp new file mode 100644 index 00000000000..e735ae56a4b --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2_examples/src/customized_rvo2_examples/agent_ignore_node.cpp @@ -0,0 +1,41 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "customized_rvo2_examples/example_scenarios.hpp" +#include "customized_rvo2_examples/rvo2_ros2.hpp" +#include "customized_rvo2_examples/scenario.hpp" +#include "rclcpp/rclcpp.hpp" + +class TestNode : public RVO2ROS2Component +{ +public: + explicit TestNode(const rclcpp::NodeOptions & options) : RVO2ROS2Component(options, 1.0f) + { + RVO::Scenario scenario; + setupAgentIgnoreScenario(scenario); + scenario.setupScenario(sim_); + } +}; +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + rclcpp::shutdown(); + return 0; +} diff --git a/simulation/context_gamma_planner/customized_rvo2_examples/src/customized_rvo2_examples/block_example_node.cpp b/simulation/context_gamma_planner/customized_rvo2_examples/src/customized_rvo2_examples/block_example_node.cpp new file mode 100644 index 00000000000..f4c6724c774 --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2_examples/src/customized_rvo2_examples/block_example_node.cpp @@ -0,0 +1,42 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "customized_rvo2_examples/example_scenarios.hpp" +#include "customized_rvo2_examples/rvo2_ros2.hpp" +#include "customized_rvo2_examples/scenario.hpp" +#include "rclcpp/rclcpp.hpp" + +class BlockNode : public RVO2ROS2Component +{ +public: + explicit BlockNode(const rclcpp::NodeOptions & options) : RVO2ROS2Component(options, 0.02f) + { + RVO::Scenario scenario; + setupBlockScenario(scenario); + scenario.setupScenario(sim_); + } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + rclcpp::shutdown(); + return 0; +} diff --git a/simulation/context_gamma_planner/customized_rvo2_examples/src/customized_rvo2_examples/circle_example_node.cpp b/simulation/context_gamma_planner/customized_rvo2_examples/src/customized_rvo2_examples/circle_example_node.cpp new file mode 100644 index 00000000000..5bab4d9576e --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2_examples/src/customized_rvo2_examples/circle_example_node.cpp @@ -0,0 +1,41 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "customized_rvo2_examples/example_scenarios.hpp" +#include "customized_rvo2_examples/rvo2_ros2.hpp" +#include "customized_rvo2_examples/scenario.hpp" +#include "rclcpp/rclcpp.hpp" + +class CircleNode : public RVO2ROS2Component +{ +public: + explicit CircleNode(const rclcpp::NodeOptions & options) : RVO2ROS2Component(options, 1.0f) + { + RVO::Scenario scenario; + setupCircleScenario(scenario); + scenario.setupScenario(sim_); + } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + rclcpp::shutdown(); + return 0; +} diff --git a/simulation/context_gamma_planner/customized_rvo2_examples/src/customized_rvo2_examples/critical_corner_case.cpp b/simulation/context_gamma_planner/customized_rvo2_examples/src/customized_rvo2_examples/critical_corner_case.cpp new file mode 100644 index 00000000000..245726142ec --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2_examples/src/customized_rvo2_examples/critical_corner_case.cpp @@ -0,0 +1,43 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "customized_rvo2_examples/example_scenarios.hpp" +#include "customized_rvo2_examples/rvo2_ros2.hpp" +#include "customized_rvo2_examples/scenario.hpp" +#include "rclcpp/rclcpp.hpp" + +class CriticalCornerCaseNode : public RVO2ROS2Component +{ +public: + explicit CriticalCornerCaseNode(const rclcpp::NodeOptions & options) + : RVO2ROS2Component(options, 0.1f) + { + RVO::Scenario scenario; + setupCriticalCornerCaseScenario(scenario); + scenario.setupScenario(sim_); + } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + rclcpp::shutdown(); + return 0; +} diff --git a/simulation/context_gamma_planner/customized_rvo2_examples/src/customized_rvo2_examples/curve_example_node.cpp b/simulation/context_gamma_planner/customized_rvo2_examples/src/customized_rvo2_examples/curve_example_node.cpp new file mode 100644 index 00000000000..5751f37636d --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2_examples/src/customized_rvo2_examples/curve_example_node.cpp @@ -0,0 +1,42 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "customized_rvo2_examples/example_scenarios.hpp" +#include "customized_rvo2_examples/rvo2_ros2.hpp" +#include "customized_rvo2_examples/scenario.hpp" +#include "rclcpp/rclcpp.hpp" + +class CurveNode : public RVO2ROS2Component +{ +public: + explicit CurveNode(const rclcpp::NodeOptions & options) : RVO2ROS2Component(options, 1.0f) + { + RVO::Scenario scenario; + setupCurveScenario(scenario); + scenario.setupScenario(sim_); + } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + rclcpp::shutdown(); + return 0; +} diff --git a/simulation/context_gamma_planner/customized_rvo2_examples/src/customized_rvo2_examples/dynamic_velocity_node.cpp b/simulation/context_gamma_planner/customized_rvo2_examples/src/customized_rvo2_examples/dynamic_velocity_node.cpp new file mode 100644 index 00000000000..fde0061dd02 --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2_examples/src/customized_rvo2_examples/dynamic_velocity_node.cpp @@ -0,0 +1,43 @@ +// Copyright 2022 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "customized_rvo2_examples/example_scenarios.hpp" +#include "customized_rvo2_examples/rvo2_ros2.hpp" +#include "customized_rvo2_examples/scenario.hpp" +#include "rclcpp/rclcpp.hpp" + +class DynamicVelocityNode : public RVO2ROS2Component +{ +public: + explicit DynamicVelocityNode(const rclcpp::NodeOptions & options) + : RVO2ROS2Component(options, 1.0f) + { + RVO::Scenario scenario; + setupDynamicVelocityScenario(scenario); + scenario.setupScenario(sim_); + } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + rclcpp::shutdown(); + return 0; +} diff --git a/simulation/context_gamma_planner/customized_rvo2_examples/src/customized_rvo2_examples/grid_obstacle_node.cpp b/simulation/context_gamma_planner/customized_rvo2_examples/src/customized_rvo2_examples/grid_obstacle_node.cpp new file mode 100644 index 00000000000..7d216236b46 --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2_examples/src/customized_rvo2_examples/grid_obstacle_node.cpp @@ -0,0 +1,42 @@ +// Copyright 2022 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "customized_rvo2_examples/example_scenarios.hpp" +#include "customized_rvo2_examples/rvo2_ros2.hpp" +#include "customized_rvo2_examples/scenario.hpp" +#include "rclcpp/rclcpp.hpp" + +class GridObstacleNode : public RVO2ROS2Component +{ +public: + explicit GridObstacleNode(const rclcpp::NodeOptions & options) : RVO2ROS2Component(options, 1.0f) + { + RVO::Scenario scenario; + setupGridObstacleScenario(scenario); + scenario.setupScenario(sim_); + } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + rclcpp::shutdown(); + return 0; +} diff --git a/simulation/context_gamma_planner/customized_rvo2_examples/src/customized_rvo2_examples/intersection_example_node.cpp b/simulation/context_gamma_planner/customized_rvo2_examples/src/customized_rvo2_examples/intersection_example_node.cpp new file mode 100644 index 00000000000..b742c062e37 --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2_examples/src/customized_rvo2_examples/intersection_example_node.cpp @@ -0,0 +1,42 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "customized_rvo2_examples/example_scenarios.hpp" +#include "customized_rvo2_examples/rvo2_ros2.hpp" +#include "customized_rvo2_examples/scenario.hpp" +#include "rclcpp/rclcpp.hpp" + +class IntersectionNode : public RVO2ROS2Component +{ +public: + explicit IntersectionNode(const rclcpp::NodeOptions & options) : RVO2ROS2Component(options, 1.0f) + { + RVO::Scenario scenario; + setupIntersectionScenario(scenario); + scenario.setupScenario(sim_); + } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + rclcpp::shutdown(); + return 0; +} diff --git a/simulation/context_gamma_planner/customized_rvo2_examples/src/customized_rvo2_examples/multiple_agent_avoid_node.cpp b/simulation/context_gamma_planner/customized_rvo2_examples/src/customized_rvo2_examples/multiple_agent_avoid_node.cpp new file mode 100644 index 00000000000..3ef421205b6 --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2_examples/src/customized_rvo2_examples/multiple_agent_avoid_node.cpp @@ -0,0 +1,41 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "customized_rvo2_examples/example_scenarios.hpp" +#include "customized_rvo2_examples/rvo2_ros2.hpp" +#include "customized_rvo2_examples/scenario.hpp" +#include "rclcpp/rclcpp.hpp" + +class TestNode : public RVO2ROS2Component +{ +public: + explicit TestNode(const rclcpp::NodeOptions & options) : RVO2ROS2Component(options, 1.0f) + { + RVO::Scenario scenario; + setupMultipleAgentAvoidScenario(scenario); + scenario.setupScenario(sim_); + } +}; +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + rclcpp::shutdown(); + return 0; +} diff --git a/simulation/context_gamma_planner/customized_rvo2_examples/src/customized_rvo2_examples/road_demo_node.cpp b/simulation/context_gamma_planner/customized_rvo2_examples/src/customized_rvo2_examples/road_demo_node.cpp new file mode 100644 index 00000000000..0355c865613 --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2_examples/src/customized_rvo2_examples/road_demo_node.cpp @@ -0,0 +1,41 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "customized_rvo2_examples/example_scenarios.hpp" +#include "customized_rvo2_examples/rvo2_ros2.hpp" +#include "customized_rvo2_examples/scenario.hpp" +#include "rclcpp/rclcpp.hpp" + +class TestNode : public RVO2ROS2Component +{ +public: + explicit TestNode(const rclcpp::NodeOptions & options) : RVO2ROS2Component(options, 1.0f) + { + RVO::Scenario scenario; + setupRoadScenario(scenario); + scenario.setupScenario(sim_); + } +}; +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + rclcpp::shutdown(); + return 0; +} diff --git a/simulation/context_gamma_planner/customized_rvo2_examples/src/customized_rvo2_examples/single_agent_avoid_node.cpp b/simulation/context_gamma_planner/customized_rvo2_examples/src/customized_rvo2_examples/single_agent_avoid_node.cpp new file mode 100644 index 00000000000..2178ab4c9eb --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2_examples/src/customized_rvo2_examples/single_agent_avoid_node.cpp @@ -0,0 +1,41 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "customized_rvo2_examples/example_scenarios.hpp" +#include "customized_rvo2_examples/rvo2_ros2.hpp" +#include "customized_rvo2_examples/scenario.hpp" +#include "rclcpp/rclcpp.hpp" + +class TestNode : public RVO2ROS2Component +{ +public: + explicit TestNode(const rclcpp::NodeOptions & options) : RVO2ROS2Component(options, 1.0f) + { + RVO::Scenario scenario; + setupSingleAgentAvoidScenario(scenario); + scenario.setupScenario(sim_); + } +}; +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + auto component = std::make_shared(options); + rclcpp::spin(component); + rclcpp::shutdown(); + return 0; +} diff --git a/simulation/context_gamma_planner/customized_rvo2_examples/test/src/test_examples.cpp b/simulation/context_gamma_planner/customized_rvo2_examples/test/src/test_examples.cpp new file mode 100644 index 00000000000..7ed42c3c6bb --- /dev/null +++ b/simulation/context_gamma_planner/customized_rvo2_examples/test/src/test_examples.cpp @@ -0,0 +1,187 @@ +// Copyright 2021 Tier IV, Inc All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +//#include +#include "customized_rvo2_examples/example_scenarios.hpp" +#include "customized_rvo2_examples/scenario.hpp" + +class TestExamples : public ::testing::Test +{ +protected: + virtual void SetUp() { simulator_ = std::make_unique(0.025f); } + virtual void TearDown() { simulator_.release(); } + +public: + std::unique_ptr simulator_; +}; + +TEST_F(TestExamples, agent_ignore) +{ + RVO::Scenario scenario; + setupAgentIgnoreScenario(scenario); + scenario.setupScenario(simulator_.get()); + auto update = [this]() { + while (simulator_->getGlobalTime() <= 10) { + simulator_->update(); + } + }; + EXPECT_NO_THROW(update()); +} + +TEST_F(TestExamples, block) +{ + RVO::Scenario scenario; + setupBlockScenario(scenario); + scenario.setupScenario(simulator_.get()); + auto update = [this]() { + while (simulator_->getGlobalTime() <= 10) { + simulator_->update(); + } + }; + EXPECT_NO_THROW(update()); +} +TEST_F(TestExamples, circle) +{ + RVO::Scenario scenario; + setupCircleScenario(scenario); + scenario.setupScenario(simulator_.get()); + auto update = [this]() { + while (simulator_->getGlobalTime() <= 10) { + simulator_->update(); + } + }; + EXPECT_NO_THROW(update()); +} + +TEST_F(TestExamples, curve) +{ + RVO::Scenario scenario; + setupCurveScenario(scenario); + scenario.setupScenario(simulator_.get()); + auto update = [this]() { + while (simulator_->getGlobalTime() <= 10) { + simulator_->update(); + } + }; + EXPECT_NO_THROW(update()); +} + +TEST_F(TestExamples, intersection) +{ + RVO::Scenario scenario; + setupIntersectionScenario(scenario); + scenario.setupScenario(simulator_.get()); + auto update = [this]() { + while (simulator_->getGlobalTime() <= 10) { + simulator_->update(); + } + }; + EXPECT_NO_THROW(update()); +} + +TEST_F(TestExamples, multiple_agent_avoid) +{ + RVO::Scenario scenario; + setupMultipleAgentAvoidScenario(scenario); + scenario.setupScenario(simulator_.get()); + auto update = [this]() { + while (simulator_->getGlobalTime() <= 10) { + simulator_->update(); + } + }; + EXPECT_NO_THROW(update()); +} + +TEST_F(TestExamples, single_agent_avoid) +{ + RVO::Scenario scenario; + setupSingleAgentAvoidScenario(scenario); + scenario.setupScenario(simulator_.get()); + auto update = [this]() { + while (simulator_->getGlobalTime() <= 10) { + simulator_->update(); + } + }; + EXPECT_NO_THROW(update()); +} + +TEST_F(TestExamples, road) +{ + RVO::Scenario scenario; + setupRoadScenario(scenario); + scenario.setupScenario(simulator_.get()); + auto update = [this]() { + while (simulator_->getGlobalTime() <= 10) { + simulator_->update(); + } + }; + EXPECT_NO_THROW(update()); +} + +TEST_F(TestExamples, critical_corner_case) +{ + RVO::Scenario scenario; + setupCriticalCornerCaseScenario(scenario); + scenario.setupScenario(simulator_.get()); + auto update = [this]() { + while (simulator_->getGlobalTime() <= 20) { + simulator_->update(); + } + }; + EXPECT_NO_THROW(update()); +} + +TEST_F(TestExamples, grid_obstacle) +{ + RVO::Scenario scenario; + setupGridObstacleScenario(scenario); + scenario.setupScenario(simulator_.get()); + auto update = [this]() { + while (simulator_->getGlobalTime() <= 20) { + simulator_->update(); + } + }; + EXPECT_NO_THROW(update()); +} + +TEST_F(TestExamples, dynamic_velocity) +{ + RVO::Scenario scenario; + setupDynamicVelocityScenario(scenario); + scenario.setupScenario(simulator_.get()); + auto update = [this]() { + simulator_->getAgents().at("agent1")->setMaxSpeed(5.0); + EXPECT_DOUBLE_EQ(simulator_->getAgents().at("agent1")->getMaxSpeed(), 5.0); + + while (simulator_->getGlobalTime() <= 20) { + const float max_speed = simulator_->getGlobalTime(); + simulator_->getAgents().at("agent1")->setMaxSpeed(max_speed); + simulator_->update(); + const auto current_speed = simulator_->getAgents().at("agent1")->getVelocity(); + EXPECT_LE( + std::sqrt(current_speed.x() * current_speed.x() + current_speed.y() * current_speed.y()), + max_speed + 1e-3); + } + }; + EXPECT_NO_THROW(update()); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file