diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/marker_helper.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/marker_helper.hpp index d674b6c1caaf4..3ef74ffcbcef6 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/marker_helper.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/marker_helper.hpp @@ -19,6 +19,8 @@ #include +#include + #include namespace tier4_autoware_utils @@ -88,10 +90,15 @@ inline visualization_msgs::msg::Marker createDefaultMarker( inline void appendMarkerArray( const visualization_msgs::msg::MarkerArray & additional_marker_array, - visualization_msgs::msg::MarkerArray * marker_array) + visualization_msgs::msg::MarkerArray * marker_array, + const boost::optional & current_time = {}) { for (const auto & marker : additional_marker_array.markers) { marker_array->markers.push_back(marker); + + if (current_time) { + marker_array->markers.back().header.stamp = current_time.get(); + } } } } // namespace tier4_autoware_utils diff --git a/planning/motion_velocity_smoother/CMakeLists.txt b/planning/motion_velocity_smoother/CMakeLists.txt index f69a216ae68d5..2995442ee1b67 100644 --- a/planning/motion_velocity_smoother/CMakeLists.txt +++ b/planning/motion_velocity_smoother/CMakeLists.txt @@ -61,9 +61,3 @@ ament_auto_package( launch config ) - -install(PROGRAMS - scripts/trajectory_visualizer.py - scripts/closest_velocity_checker.py - DESTINATION lib/${PROJECT_NAME} -) diff --git a/planning/planning_debug_tools/CMakeLists.txt b/planning/planning_debug_tools/CMakeLists.txt new file mode 100644 index 0000000000000..7d5722d791959 --- /dev/null +++ b/planning/planning_debug_tools/CMakeLists.txt @@ -0,0 +1,54 @@ +cmake_minimum_required(VERSION 3.5) +project(planning_debug_tools) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) + +rosidl_generate_interfaces( + planning_debug_tools + "msg/TrajectoryDebugInfo.msg" + DEPENDENCIES builtin_interfaces +) + +ament_auto_add_library(trajectory_analyzer_node SHARED + src/trajectory_analyzer.cpp +) + +ament_auto_add_library(stop_reason_visualizer_node SHARED + src/stop_reason_visualizer.cpp +) + +if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) + rosidl_target_interfaces(trajectory_analyzer_node + planning_debug_tools "rosidl_typesupport_cpp") +else() + rosidl_get_typesupport_target( + cpp_typesupport_target planning_debug_tools "rosidl_typesupport_cpp") + target_link_libraries(trajectory_analyzer_node "${cpp_typesupport_target}") +endif() + + +rclcpp_components_register_node(trajectory_analyzer_node + PLUGIN "planning_debug_tools::TrajectoryAnalyzerNode" + EXECUTABLE trajectory_analyzer_exe +) + +rclcpp_components_register_node(stop_reason_visualizer_node + PLUGIN "planning_debug_tools::StopReasonVisualizerNode" + EXECUTABLE stop_reason_visualizer_exe +) + +ament_auto_package( + INSTALL_TO_SHARE + launch +) + +install(PROGRAMS + scripts/trajectory_visualizer.py + scripts/closest_velocity_checker.py + DESTINATION lib/${PROJECT_NAME} +) diff --git a/planning/planning_debug_tools/Readme.md b/planning/planning_debug_tools/Readme.md new file mode 100644 index 0000000000000..4d5ed1e95111f --- /dev/null +++ b/planning/planning_debug_tools/Readme.md @@ -0,0 +1,154 @@ +# Planning Debug Tools + +This package contains several planning-related debug tools. + +- **Trajectory analyzer**: visualizes the information (speed, curvature, yaw, etc) along the trajectory +- **Closest velocity checker**: prints the velocity information indicated by each modules + +## Trajectory analyzer + +The `trajectory_analyzer` visualizes the information (speed, curvature, yaw, etc) along the trajectory. This feature would be helpful for purposes such as "_investigating the reason why the vehicle decelerates here_". This feature employs the OSS [PlotJuggler](https://www.plotjuggler.io/). + +![how this works](https://user-images.githubusercontent.com/21360593/179361367-a9fa136c-cd65-4f3c-ad7c-f542346a8d37.mp4) + +## Stop reason visualizer + +This is to visualize stop factor and reason. +[see the details](./doc-stop-reason-visualizer.md) + +### How to use + +please launch the analyzer node + +```bash +ros2 launch planning_debug_tools trajectory_analyzer.launch.xml +``` + +and visualize the analyzed data on the plot juggler following below. + +#### setup PlotJuggler + +For the first time, please add the following code to reactive script and save it as the picture below! +(Looking for the way to automatically load the configuration file...) + +You can customize what you plot by editing this code. + +![image](./image/lua.png) + +in Global code + +```lua +behavior_path = '/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id/debug_info' +behavior_velocity = '/planning/scenario_planning/lane_driving/behavior_planning/path/debug_info' +motion_avoid = '/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/trajectory/debug_info' +motion_smoother_latacc = '/planning/scenario_planning/motion_velocity_smoother/debug/trajectory_lateral_acc_filtered/debug_info' +motion_smoother = '/planning/scenario_planning/trajectory/debug_info' +``` + +in function(tracker_time) + +```lua +PlotCurvatureOverArclength('k_behavior_path', behavior_path, tracker_time) +PlotCurvatureOverArclength('k_behavior_velocity', behavior_velocity, tracker_time) +PlotCurvatureOverArclength('k_motion_avoid', motion_avoid, tracker_time) +PlotCurvatureOverArclength('k_motion_smoother', motion_smoother, tracker_time) + +PlotVelocityOverArclength('v_behavior_path', behavior_path, tracker_time) +PlotVelocityOverArclength('v_behavior_velocity', behavior_velocity, tracker_time) +PlotVelocityOverArclength('v_motion_avoid', motion_avoid, tracker_time) +PlotVelocityOverArclength('v_motion_smoother_latacc', motion_smoother_latacc, tracker_time) +PlotVelocityOverArclength('v_motion_smoother', motion_smoother, tracker_time) + +PlotAccelerationOverArclength('a_behavior_path', behavior_path, tracker_time) +PlotAccelerationOverArclength('a_behavior_velocity', behavior_velocity, tracker_time) +PlotAccelerationOverArclength('a_motion_avoid', motion_avoid, tracker_time) +PlotAccelerationOverArclength('a_motion_smoother_latacc', motion_smoother_latacc, tracker_time) +PlotAccelerationOverArclength('a_motion_smoother', motion_smoother, tracker_time) + +PlotYawOverArclength('yaw_behavior_path', behavior_path, tracker_time) +PlotYawOverArclength('yaw_behavior_velocity', behavior_velocity, tracker_time) +PlotYawOverArclength('yaw_motion_avoid', motion_avoid, tracker_time) +PlotYawOverArclength('yaw_motion_smoother_latacc', motion_smoother_latacc, tracker_time) +PlotYawOverArclength('yaw_motion_smoother', motion_smoother, tracker_time) + +PlotCurrentVelocity('localization_kinematic_state', '/localization/kinematic_state', tracker_time) +``` + +in Function Library +![image](./image/script.png) + +```lua + +function PlotValue(name, path, timestamp, value) + new_series = ScatterXY.new(name) + index = 0 + while(true) do + series_k = TimeseriesView.find( string.format( "%s/"..value..".%d", path, index) ) + series_s = TimeseriesView.find( string.format( "%s/arclength.%d", path, index) ) + series_size = TimeseriesView.find( string.format( "%s/size", path) ) + + if series_k == nil or series_s == nil then break end + + k = series_k:atTime(timestamp) + s = series_s:atTime(timestamp) + size = series_size:atTime(timestamp) + + if index >= size then break end + + new_series:push_back(s,k) + index = index+1 + end + +function PlotCurvatureOverArclength(name, path, timestamp) + PlotValue(name, path, timestamp,"curvature") +end + +function PlotVelocityOverArclength(name, path, timestamp) + PlotValue(name, path, timestamp,"velocity") +end + +function PlotAccelerationOverArclength(name, path, timestamp) + PlotValue(name, path, timestamp,"acceleration") +end + +function PlotYawOverArclength(name, path, timestamp) + PlotValue(name, path, timestamp,"yaw") +end + +function PlotCurrentVelocity(name, kinematics_name, timestamp) + new_series = ScatterXY.new(name) + series_v = TimeseriesView.find( string.format( "%s/twist/twist/linear/x", kinematics_name)) + if series_v == nil then + print("error") + return + end + v = series_v:atTime(timestamp) + new_series:push_back(0.0, v) +end +``` + +Then, run the plot juggler. + +### How to customize the plot + +Add Path/PathWithLaneIds/Trajectory topics you want to plot in the `trajectory_analyzer.launch.xml`, then the analyzed topics for these messages will be published with `TrajectoryDebugINfo.msg` type. You can then visualize these data by editing the reactive script on the PlotJuggler. + +### Requirements + +The version of the plotJuggler must be > `3.5.0` + +## Closest velocity checker + +This node prints the velocity information indicated by planning/control modules on a terminal. For trajectories calculated by planning modules, the target velocity on the trajectory point which is closest to the ego vehicle is printed. For control commands calculated by control modules, the target velocity and acceleration is directly printed. This feature would be helpful for purposes such as "_investigating the reason why the vehicle does not move_". + +You can launch by + +```bash +ros2 run planning_debug_tools closest_velocity_checker.py +``` + +![closest-velocity-checker](image/closest-velocity-checker.png) + +## Trajectory visualizer + +The old version of the trajectory analyzer. It is written in Python and more flexible, but very slow. diff --git a/planning/planning_debug_tools/doc-stop-reason-visualizer.md b/planning/planning_debug_tools/doc-stop-reason-visualizer.md new file mode 100644 index 0000000000000..57fb5b30c2c3e --- /dev/null +++ b/planning/planning_debug_tools/doc-stop-reason-visualizer.md @@ -0,0 +1,21 @@ +## stop_reason_visualizer + +This module is to visualize stop factor quickly without selecting correct debug markers. +This is supposed to use with virtual wall marker like below. +![image](image/stop_reason_image.png) + +### How to use + +Run this node. + +```sh +ros2 run planning_debug_tools stop_reason_visualizer_exe +``` + +Add stop reason debug marker from rviz. + +![image](image/add_marker.png) + +Note: ros2 process can be sometimes deleted only from `killall stop_reason_visualizer_exe` + +[Reference](https://answers.ros.org/question/323329/how-to-kill-nodes-in-ros2/?answer=403184#post-id-403184) diff --git a/planning/planning_debug_tools/image/add_marker.png b/planning/planning_debug_tools/image/add_marker.png new file mode 100644 index 0000000000000..f2b9992d31368 Binary files /dev/null and b/planning/planning_debug_tools/image/add_marker.png differ diff --git a/planning/planning_debug_tools/image/closest-velocity-checker.png b/planning/planning_debug_tools/image/closest-velocity-checker.png new file mode 100644 index 0000000000000..2849648df4881 Binary files /dev/null and b/planning/planning_debug_tools/image/closest-velocity-checker.png differ diff --git a/planning/planning_debug_tools/image/lua.png b/planning/planning_debug_tools/image/lua.png new file mode 100644 index 0000000000000..7ae0e5e67da35 Binary files /dev/null and b/planning/planning_debug_tools/image/lua.png differ diff --git a/planning/planning_debug_tools/image/script.png b/planning/planning_debug_tools/image/script.png new file mode 100644 index 0000000000000..5cc8ca98ebccb Binary files /dev/null and b/planning/planning_debug_tools/image/script.png differ diff --git a/planning/planning_debug_tools/image/stop_reason_image.png b/planning/planning_debug_tools/image/stop_reason_image.png new file mode 100644 index 0000000000000..92c994ce94b2c Binary files /dev/null and b/planning/planning_debug_tools/image/stop_reason_image.png differ diff --git a/planning/planning_debug_tools/image/stop_reason_rviz.png b/planning/planning_debug_tools/image/stop_reason_rviz.png new file mode 100644 index 0000000000000..1e99b95976d6e Binary files /dev/null and b/planning/planning_debug_tools/image/stop_reason_rviz.png differ diff --git a/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp b/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp new file mode 100644 index 0000000000000..05aa858f92f0a --- /dev/null +++ b/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp @@ -0,0 +1,125 @@ +// Copyright 2021 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 PLANNING_DEBUG_TOOLS__TRAJECTORY_ANALYZER_HPP_ +#define PLANNING_DEBUG_TOOLS__TRAJECTORY_ANALYZER_HPP_ + +#include "motion_utils/trajectory/trajectory.hpp" +#include "planning_debug_tools/msg/trajectory_debug_info.hpp" +#include "planning_debug_tools/util.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp" + +#include "autoware_auto_planning_msgs/msg/path.hpp" +#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "tier4_debug_msgs/msg/float64_multi_array_stamped.hpp" + +#include +#include +#include +#include +#include + +namespace planning_debug_tools +{ +using autoware_auto_planning_msgs::msg::Path; +using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_auto_planning_msgs::msg::Trajectory; +using nav_msgs::msg::Odometry; +using planning_debug_tools::msg::TrajectoryDebugInfo; + +template +class TrajectoryAnalyzer +{ + using SubscriberType = typename rclcpp::Subscription::SharedPtr; + using PublisherType = rclcpp::Publisher::SharedPtr; + using T_ConstSharedPtr = typename T::ConstSharedPtr; + +public: + TrajectoryAnalyzer(rclcpp::Node * node, const std::string & sub_name) + : node_(node), name_(sub_name) + { + const auto pub_name = sub_name + "/debug_info"; + pub_ = node->create_publisher(pub_name, 1); + sub_ = node->create_subscription( + sub_name, 1, [this](const T_ConstSharedPtr msg) { run(msg->points); }); + } + ~TrajectoryAnalyzer() = default; + + void setKinematics(const Odometry::ConstSharedPtr input) { ego_kinematics_ = input; } + + // Note: the lambda used in the subscriber captures "this", so any operations that change the + // address of "this" are prohibited. + TrajectoryAnalyzer(const TrajectoryAnalyzer &) = delete; // copy + TrajectoryAnalyzer(TrajectoryAnalyzer &&) = delete; // move + auto operator=(const TrajectoryAnalyzer &) -> TrajectoryAnalyzer & = delete; // copy assignment + auto operator=(TrajectoryAnalyzer &&) -> TrajectoryAnalyzer & = delete; // move assignment + +public: + std::shared_ptr node_; + std::string name_; + PublisherType pub_; + SubscriberType sub_; + Odometry::ConstSharedPtr ego_kinematics_; + + template + void run(const P & points) + { + if (!ego_kinematics_) return; + if (points.size() < 3) return; + + const auto & ego_p = ego_kinematics_->pose.pose.position; + + TrajectoryDebugInfo data; + data.stamp = node_->now(); + data.size = points.size(); + data.curvature = calcCurvature(points); + const auto arclength_offset = motion_utils::calcSignedArcLength(points, 0, ego_p); + data.arclength = calcPathArcLengthArray(points, -arclength_offset); + data.velocity = getVelocityArray(points); + data.acceleration = getAccelerationArray(points); + data.yaw = getYawArray(points); + + if ( + data.size != data.arclength.size() || data.size != data.velocity.size() || + data.size != data.yaw.size()) { + RCLCPP_ERROR(node_->get_logger(), "computation failed."); + return; + } + + pub_->publish(data); + } +}; + +class TrajectoryAnalyzerNode : public rclcpp::Node +{ +public: + explicit TrajectoryAnalyzerNode(const rclcpp::NodeOptions & options); + ~TrajectoryAnalyzerNode() = default; + +private: + rclcpp::Subscription::SharedPtr sub_ego_kinematics_; + void onEgoKinematics(const Odometry::ConstSharedPtr msg); + + std::vector>> path_analyzers_; + std::vector>> path_wlid_analyzers_; + std::vector>> trajectory_analyzers_; +}; + +} // namespace planning_debug_tools + +#endif // PLANNING_DEBUG_TOOLS__TRAJECTORY_ANALYZER_HPP_ diff --git a/planning/planning_debug_tools/include/planning_debug_tools/util.hpp b/planning/planning_debug_tools/include/planning_debug_tools/util.hpp new file mode 100644 index 0000000000000..e3b4ab7639cb6 --- /dev/null +++ b/planning/planning_debug_tools/include/planning_debug_tools/util.hpp @@ -0,0 +1,134 @@ +// Copyright 2022 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 PLANNING_DEBUG_TOOLS__UTIL_HPP_ +#define PLANNING_DEBUG_TOOLS__UTIL_HPP_ + +#include "motion_utils/trajectory/trajectory.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp" + +#include "autoware_auto_planning_msgs/msg/path.hpp" +#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" + +#include + +namespace planning_debug_tools +{ + +using autoware_auto_planning_msgs::msg::PathPoint; +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using tier4_autoware_utils::calcDistance2d; +using tier4_autoware_utils::getPoint; +using tier4_autoware_utils::getRPY; + +double getVelocity(const PathPoint & p) { return p.longitudinal_velocity_mps; } +double getVelocity(const PathPointWithLaneId & p) { return p.point.longitudinal_velocity_mps; } +double getVelocity(const TrajectoryPoint & p) { return p.longitudinal_velocity_mps; } + +double getYaw(const PathPoint & p) { return getRPY(p.pose.orientation).z; } +double getYaw(const PathPointWithLaneId & p) { return getRPY(p.point.pose.orientation).z; } +double getYaw(const TrajectoryPoint & p) { return getRPY(p.pose.orientation).z; } + +template +inline std::vector getYawArray(const T & points) +{ + std::vector yaw_arr; + for (const auto & p : points) { + yaw_arr.push_back(getYaw(p)); + } + return yaw_arr; +} +template +inline std::vector getVelocityArray(const T & points) +{ + std::vector v_arr; + for (const auto & p : points) { + v_arr.push_back(getVelocity(p)); + } + return v_arr; +} + +template +inline std::vector getAccelerationArray(const T & points) +{ + std::vector segment_wise_a_arr; + for (size_t i = 0; i < points.size() - 1; ++i) { + const auto & prev_point = points.at(i); + const auto & next_point = points.at(i + 1); + + const double delta_s = tier4_autoware_utils::calcDistance2d(prev_point, next_point); + if (delta_s == 0.0) { + segment_wise_a_arr.push_back(0.0); + } else { + const double prev_vel = getVelocity(prev_point); + const double next_vel = getVelocity(next_point); + + const double acc = (std::pow(next_vel, 2) - std::pow(prev_vel, 2)) / 2.0 / delta_s; + + segment_wise_a_arr.push_back(acc); + } + } + + std::vector point_wise_a_arr; + for (size_t i = 0; i < points.size(); ++i) { + if (i == 0) { + point_wise_a_arr.push_back(segment_wise_a_arr.at(i)); + } else if (i == points.size() - 1 || i == points.size() - 2) { + // Ignore the last two acceleration values which are negative infinity since the path end + // velocity is always 0 by motion_velocity_smoother. NOTE: Path end velocity affects the last + // two acceleration values. + point_wise_a_arr.push_back(0.0); + } else { + point_wise_a_arr.push_back((segment_wise_a_arr.at(i - 1) + segment_wise_a_arr.at(i)) / 2.0); + } + } + + return point_wise_a_arr; +} + +template +std::vector calcPathArcLengthArray(const T & points, const double offset) +{ + std::vector out; + out.push_back(offset); + double sum = offset; + for (size_t i = 1; i < points.size(); ++i) { + sum += calcDistance2d(getPoint(points.at(i)), getPoint(points.at(i - 1))); + out.push_back(sum); + } + return out; +} + +template +inline std::vector calcCurvature(const T & points) +{ + std::vector curvature_arr; + curvature_arr.push_back(0.0); + for (size_t i = 1; i < points.size() - 1; ++i) { + const auto p1 = getPoint(points.at(i - 1)); + const auto p2 = getPoint(points.at(i)); + const auto p3 = getPoint(points.at(i + 1)); + curvature_arr.push_back(tier4_autoware_utils::calcCurvature(p1, p2, p3)); + } + curvature_arr.push_back(0.0); + return curvature_arr; +} + +} // namespace planning_debug_tools + +#endif // PLANNING_DEBUG_TOOLS__UTIL_HPP_ diff --git a/planning/planning_debug_tools/launch/stop_reason_visualizer.launch.xml b/planning/planning_debug_tools/launch/stop_reason_visualizer.launch.xml new file mode 100644 index 0000000000000..d4a92dec1c8e7 --- /dev/null +++ b/planning/planning_debug_tools/launch/stop_reason_visualizer.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/planning_debug_tools/launch/trajectory_analyzer.launch.xml b/planning/planning_debug_tools/launch/trajectory_analyzer.launch.xml new file mode 100644 index 0000000000000..638d0dceaa8aa --- /dev/null +++ b/planning/planning_debug_tools/launch/trajectory_analyzer.launch.xml @@ -0,0 +1,19 @@ + + + + + + + + diff --git a/planning/planning_debug_tools/msg/TrajectoryDebugInfo.msg b/planning/planning_debug_tools/msg/TrajectoryDebugInfo.msg new file mode 100644 index 0000000000000..c8d349b2f9540 --- /dev/null +++ b/planning/planning_debug_tools/msg/TrajectoryDebugInfo.msg @@ -0,0 +1,7 @@ +builtin_interfaces/Time stamp +uint32 size +float64[] arclength +float64[] curvature +float64[] velocity +float64[] acceleration +float64[] yaw diff --git a/planning/planning_debug_tools/package.xml b/planning/planning_debug_tools/package.xml new file mode 100644 index 0000000000000..9cee8bfdac066 --- /dev/null +++ b/planning/planning_debug_tools/package.xml @@ -0,0 +1,38 @@ + + + + planning_debug_tools + 0.1.0 + The planning_debug_tools package + Takamasa Horibe + Taiki Tanaka + Apache License 2.0 + + Takamasa Horibe + + ament_cmake_auto + eigen3_cmake_module + + autoware_cmake + rosidl_default_generators + + autoware_auto_planning_msgs + geometry_msgs + motion_utils + nav_msgs + rclcpp + rclcpp_components + tier4_autoware_utils + tier4_debug_msgs + tier4_planning_msgs + + ament_lint_auto + autoware_lint_common + + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + diff --git a/planning/motion_velocity_smoother/scripts/closest_velocity_checker.py b/planning/planning_debug_tools/scripts/closest_velocity_checker.py similarity index 100% rename from planning/motion_velocity_smoother/scripts/closest_velocity_checker.py rename to planning/planning_debug_tools/scripts/closest_velocity_checker.py diff --git a/planning/motion_velocity_smoother/scripts/trajectory_visualizer.py b/planning/planning_debug_tools/scripts/trajectory_visualizer.py similarity index 100% rename from planning/motion_velocity_smoother/scripts/trajectory_visualizer.py rename to planning/planning_debug_tools/scripts/trajectory_visualizer.py diff --git a/planning/planning_debug_tools/src/stop_reason_visualizer.cpp b/planning/planning_debug_tools/src/stop_reason_visualizer.cpp new file mode 100644 index 0000000000000..61e54f47caf5a --- /dev/null +++ b/planning/planning_debug_tools/src/stop_reason_visualizer.cpp @@ -0,0 +1,128 @@ +// Copyright 2022 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 +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace planning_debug_tools +{ +using std::placeholders::_1; +using tier4_planning_msgs::msg::StopReason; +using tier4_planning_msgs::msg::StopReasonArray; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + +class StopReasonVisualizerNode : public rclcpp::Node +{ +public: + explicit StopReasonVisualizerNode(const rclcpp::NodeOptions & options) + : Node("stop_reason_visualizer", options) + { + pub_stop_reasons_marker_ = create_publisher("~/debug/markers", 1); + sub_stop_reasons_ = create_subscription( + "/planning/scenario_planning/status/stop_reasons", rclcpp::QoS{1}, + std::bind(&StopReasonVisualizerNode::onStopReasonArray, this, _1)); + } + +private: + void onStopReasonArray(const StopReasonArray::ConstSharedPtr msg) + { + using tier4_autoware_utils::appendMarkerArray; + using tier4_autoware_utils::createDefaultMarker; + using tier4_autoware_utils::createMarkerColor; + using tier4_autoware_utils::createMarkerScale; + + MarkerArray all_marker_array; + const auto header = msg->header; + const double offset_z = 1.0; + for (auto stop_reason : msg->stop_reasons) { + std::string reason = stop_reason.reason; + if (reason.empty()) continue; + const auto current_time = this->now(); + int id = 0; + MarkerArray marker_array; + for (auto stop_factor : stop_reason.stop_factors) { + if (stop_factor.stop_factor_points.empty()) continue; + std::string prefix = reason + "[" + std::to_string(id) + "]"; + const auto stop_factor_point = stop_factor.stop_factor_points.front(); + // base stop pose marker + { + auto stop_point_marker = createDefaultMarker( + "map", current_time, prefix + ":stop_factor_point", id, Marker::SPHERE, + createMarkerScale(0.25, 0.25, 0.25), createMarkerColor(1.0, 0.0, 0.0, 0.999)); + stop_point_marker.pose.position = stop_factor_point; + marker_array.markers.emplace_back(stop_point_marker); + } + // attention ! marker + { + auto attention_text_marker = createDefaultMarker( + "map", current_time, prefix + ":attention text", id, + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), + createMarkerColor(1.0, 1.0, 1.0, 0.999)); + attention_text_marker.pose.position = stop_factor_point; + attention_text_marker.pose.position.z += offset_z; + attention_text_marker.text = "!"; + marker_array.markers.emplace_back(attention_text_marker); + } + // point to pose + { + auto stop_to_pose_marker = createDefaultMarker( + "map", current_time, prefix + ":stop_to_pose", id, Marker::LINE_STRIP, + createMarkerScale(0.02, 0.0, 0.0), createMarkerColor(0.0, 1.0, 1.0, 0.999)); + stop_to_pose_marker.points.emplace_back(stop_factor.stop_factor_points.front()); + stop_to_pose_marker.points.emplace_back(stop_factor.stop_pose.position); + marker_array.markers.emplace_back(stop_to_pose_marker); + } + // point to pose + { + auto stop_pose_marker = createDefaultMarker( + "map", current_time, prefix + ":stop_pose", id, Marker::ARROW, + createMarkerScale(0.4, 0.2, 0.2), createMarkerColor(1.0, 0.0, 0.0, 0.999)); + stop_pose_marker.pose = stop_factor.stop_pose; + marker_array.markers.emplace_back(stop_pose_marker); + } + // add view distance text + { + auto reason_text_marker = createDefaultMarker( + "map", current_time, prefix + ":reason", id, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.0, 0.0, 0.2), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + reason_text_marker.pose = stop_factor.stop_pose; + reason_text_marker.text = prefix; + marker_array.markers.emplace_back(reason_text_marker); + } + id++; + } + if (!marker_array.markers.empty()) + appendMarkerArray(marker_array, &all_marker_array, current_time); + } + pub_stop_reasons_marker_->publish(all_marker_array); + } + rclcpp::Publisher::SharedPtr pub_stop_reasons_marker_; + rclcpp::Subscription::SharedPtr sub_stop_reasons_; +}; +} // namespace planning_debug_tools + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(planning_debug_tools::StopReasonVisualizerNode) diff --git a/planning/planning_debug_tools/src/trajectory_analyzer.cpp b/planning/planning_debug_tools/src/trajectory_analyzer.cpp new file mode 100644 index 0000000000000..91a0426d79b6b --- /dev/null +++ b/planning/planning_debug_tools/src/trajectory_analyzer.cpp @@ -0,0 +1,62 @@ +// Copyright 2021 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 "planning_debug_tools/trajectory_analyzer.hpp" + +namespace planning_debug_tools +{ +TrajectoryAnalyzerNode::TrajectoryAnalyzerNode(const rclcpp::NodeOptions & options) +: Node("trajectory_analyzer", options) +{ + using TopicNames = std::vector; + const auto path_topics = declare_parameter("path_topics", TopicNames{}); + const auto path_wlid_topics = declare_parameter("path_wlid_topics", TopicNames{}); + const auto trajectory_topics = declare_parameter("trajectory_topics", TopicNames{}); + + for (const auto & s : path_topics) { + path_analyzers_.push_back(std::make_shared>(this, s)); + RCLCPP_INFO(get_logger(), "path_topics: %s", s.c_str()); + } + for (const auto & s : path_wlid_topics) { + path_wlid_analyzers_.push_back(std::make_shared>(this, s)); + RCLCPP_INFO(get_logger(), "path_wlid_topics: %s", s.c_str()); + } + + for (const auto & s : trajectory_topics) { + trajectory_analyzers_.push_back(std::make_shared>(this, s)); + RCLCPP_INFO(get_logger(), "trajectory_topics: %s", s.c_str()); + } + + using std::placeholders::_1; + sub_ego_kinematics_ = create_subscription( + "ego_kinematics", 1, std::bind(&TrajectoryAnalyzerNode::onEgoKinematics, this, _1)); +} + +void TrajectoryAnalyzerNode::onEgoKinematics(const Odometry::ConstSharedPtr msg) +{ + for (auto & a : path_analyzers_) { + a->setKinematics(msg); + } + for (auto & a : path_wlid_analyzers_) { + a->setKinematics(msg); + } + for (auto & a : trajectory_analyzers_) { + a->setKinematics(msg); + } +} + +} // namespace planning_debug_tools + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(planning_debug_tools::TrajectoryAnalyzerNode)