diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp index b7f84987e0499..8daa723e6fc83 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp @@ -25,6 +25,7 @@ #include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" +#include "interest_objects_marker_interface/interest_objects_marker_interface.hpp" #include @@ -48,6 +49,7 @@ namespace behavior_path_planner using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; +using interest_objects_marker_interface::InterestObjectsMarkerInterface; using tier4_planning_msgs::msg::LaneChangeDebugMsg; using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; @@ -133,6 +135,8 @@ class LaneChangeInterface : public SceneModuleInterface void setObjectDebugVisualization() const; + void setObjectDebugVisualizationTmp(); + void updateSteeringFactorPtr(const BehaviorModuleOutput & output); void updateSteeringFactorPtr( @@ -142,6 +146,7 @@ class LaneChangeInterface : public SceneModuleInterface mutable LaneChangeDebugMsgArray lane_change_debug_msg_array_; std::unique_ptr prev_approved_path_; + InterestObjectsMarkerInterface interest_objects_marker_interface_; void clearAbortApproval() { is_abort_path_approved_ = false; } diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index 1f1eed3f95ec2..f848b05523ddc 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -53,6 +53,7 @@ behaviortree_cpp_v3 freespace_planning_algorithms geometry_msgs + interest_objects_marker_interface interpolation lane_departure_checker lanelet2_extension diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 05b7c5d7dec92..4969d05e4ff05 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -40,7 +40,8 @@ LaneChangeInterface::LaneChangeInterface( : SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{std::move(parameters)}, module_type_{std::move(module_type)}, - prev_approved_path_{std::make_unique()} + prev_approved_path_{std::make_unique()}, + interest_objects_marker_interface_{&node, name} { steering_factor_interface_ptr_ = std::make_unique(&node, name); } @@ -220,6 +221,8 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); module_type_->updateLaneChangeStatus(); setObjectDebugVisualization(); + setObjectDebugVisualizationTmp(); + interest_objects_marker_interface_.publishMarkerArray(); // change turn signal when the vehicle reaches at the end of the path for waiting lane change out.turn_signal_info = getCurrentTurnSignalInfo(*out.path, out.turn_signal_info); @@ -308,6 +311,15 @@ void LaneChangeInterface::setObjectDebugVisualization() const } } +void LaneChangeInterface::setObjectDebugVisualizationTmp() +{ + const auto debug_data = module_type_->getDebugData(); + for (const auto & [uuid, data] : debug_data) { + interest_objects_marker_interface_.insertObjectStatus(data.current_obj_pose, 1.5, data.is_safe); + } + return; +} + std::shared_ptr LaneChangeInterface::get_debug_msg_array() const { const auto debug_data = module_type_->getDebugData(); diff --git a/planning/interest_objects_marker_interface/CMakeLists.txt b/planning/interest_objects_marker_interface/CMakeLists.txt new file mode 100644 index 0000000000000..199d895f72248 --- /dev/null +++ b/planning/interest_objects_marker_interface/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.5) +project(interest_objects_marker_interface) + +### Compile options +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_add_library(interest_objects_marker_interface SHARED + src/interest_objects_marker_interface.cpp +) + +# Test +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() diff --git a/planning/interest_objects_marker_interface/README.md b/planning/interest_objects_marker_interface/README.md new file mode 100644 index 0000000000000..7c909013c45e5 --- /dev/null +++ b/planning/interest_objects_marker_interface/README.md @@ -0,0 +1,11 @@ +# Interest Objects Marker Interface + +## Purpose + +## Inner-workings / Algorithms + +## Inputs / Outputs + +## Assumptions / Known limits + +## Future extensions / Unimplemented parts diff --git a/planning/interest_objects_marker_interface/include/interest_objects_marker_interface/interest_objects_marker_interface.hpp b/planning/interest_objects_marker_interface/include/interest_objects_marker_interface/interest_objects_marker_interface.hpp new file mode 100644 index 0000000000000..9494adba90594 --- /dev/null +++ b/planning/interest_objects_marker_interface/include/interest_objects_marker_interface/interest_objects_marker_interface.hpp @@ -0,0 +1,61 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 INTEREST_OBJECTS_MARKER_INTERFACE__INTEREST_OBJECTS_MARKER_INTERFACE_HPP_ +#define INTEREST_OBJECTS_MARKER_INTERFACE__INTEREST_OBJECTS_MARKER_INTERFACE_HPP_ +#include "rclcpp/rclcpp.hpp" + +#include + +#include +#include + +#include +#include +#include + +namespace interest_objects_marker_interface +{ +using geometry_msgs::msg::Pose; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + +struct ObjectStatus +{ + Pose pose{}; + double height{0.0}; + bool safe{false}; +}; + +class InterestObjectsMarkerInterface +{ +public: + InterestObjectsMarkerInterface(rclcpp::Node * node, const std::string & name); + void insertObjectStatus(const Pose & pose, const double obj_height, const bool safe); + void publishMarkerArray(); + +private: + rclcpp::Publisher::SharedPtr pub_marker_; + + std::vector obj_status_array_; + + std::string name_; + std::string topic_namespace_ = "/planning/interest_objects_marker"; + + mutable std::mutex mutex_; +}; + +} // namespace interest_objects_marker_interface + +#endif // INTEREST_OBJECTS_MARKER_INTERFACE__INTEREST_OBJECTS_MARKER_INTERFACE_HPP_ diff --git a/planning/interest_objects_marker_interface/package.xml b/planning/interest_objects_marker_interface/package.xml new file mode 100644 index 0000000000000..4526fcd01e38b --- /dev/null +++ b/planning/interest_objects_marker_interface/package.xml @@ -0,0 +1,26 @@ + + + interest_objects_marker_interface + 0.1.0 + The interest_objects_marker_interface package + + Fumiya Watanabe + + Apache License 2.0 + + Fumiya Watanabe + + ament_cmake_auto + + geometry_msgs + rclcpp + tier4_autoware_utils + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/interest_objects_marker_interface/src/interest_objects_marker_interface.cpp b/planning/interest_objects_marker_interface/src/interest_objects_marker_interface.cpp new file mode 100644 index 0000000000000..924dd203effea --- /dev/null +++ b/planning/interest_objects_marker_interface/src/interest_objects_marker_interface.cpp @@ -0,0 +1,85 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 "interest_objects_marker_interface/interest_objects_marker_interface.hpp" +namespace +{ +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using interest_objects_marker_interface::ObjectStatus; +using visualization_msgs::msg::Marker; + +using tier4_autoware_utils::createDefaultMarker; +using tier4_autoware_utils::createMarkerColor; +using tier4_autoware_utils::createMarkerScale; + +Marker createArrowMarker(const size_t & id, const ObjectStatus & status, const std::string & name) +{ + const float r = status.safe ? 0.0f : 0.75f; + const float g = status.safe ? 0.75f : 0.0f; + const float b = 0.0f; + + Marker marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), name, id, Marker::ARROW, + createMarkerScale(0.25, 0.5, 0.5), createMarkerColor(r, g, b, 0.999)); + + Point src, dst; + src = status.pose.position; + src.z += status.height + 1.0; + dst = status.pose.position; + dst.z += status.height; + + marker.points.push_back(src); + marker.points.push_back(dst); + + return marker; +} +} // namespace + +namespace interest_objects_marker_interface +{ + +InterestObjectsMarkerInterface::InterestObjectsMarkerInterface( + rclcpp::Node * node, const std::string & name) +: name_{name} +{ + // Publisher + pub_marker_ = node->create_publisher(topic_namespace_ + "/" + name, 1); +} + +void InterestObjectsMarkerInterface::insertObjectStatus( + const Pose & pose, const double obj_height, const bool safe) +{ + ObjectStatus status; + status.pose = pose; + status.height = obj_height; + status.safe = safe; + + obj_status_array_.push_back(status); +} + +void InterestObjectsMarkerInterface::publishMarkerArray() +{ + MarkerArray marker_array; + for (size_t i = 0; i < obj_status_array_.size(); ++i) { + const auto obj = obj_status_array_.at(i); + const Marker marker = createArrowMarker(i, obj, name_); + marker_array.markers.push_back(marker); + } + + pub_marker_->publish(marker_array); + obj_status_array_.clear(); +} + +} // namespace interest_objects_marker_interface