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..8cb35baf7a0f6 --- /dev/null +++ b/planning/planning_debug_tools/CMakeLists.txt @@ -0,0 +1,45 @@ +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 +) + +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 +) + +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/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..3e147b2e55dba --- /dev/null +++ b/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp @@ -0,0 +1,121 @@ +// 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); + + if (data.size != data.arclength.size() || data.size != data.velocity.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..24d47ac138a62 --- /dev/null +++ b/planning/planning_debug_tools/include/planning_debug_tools/util.hpp @@ -0,0 +1,87 @@ +// 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 tier4_autoware_utils::calcDistance2d; +using tier4_autoware_utils::getPoint; + +double getVelocity(const autoware_auto_planning_msgs::msg::PathPoint & p) +{ + return p.longitudinal_velocity_mps; +} +double getVelocity(const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) +{ + return p.point.longitudinal_velocity_mps; +} +double getVelocity(const autoware_auto_planning_msgs::msg::TrajectoryPoint & p) +{ + return p.longitudinal_velocity_mps; +} +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 +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/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..e7aa00f8c0e6e --- /dev/null +++ b/planning/planning_debug_tools/msg/TrajectoryDebugInfo.msg @@ -0,0 +1,5 @@ +builtin_interfaces/Time stamp +uint32 size +float64[] arclength +float64[] curvature +float64[] velocity diff --git a/planning/planning_debug_tools/package.xml b/planning/planning_debug_tools/package.xml new file mode 100644 index 0000000000000..f4897bb57c609 --- /dev/null +++ b/planning/planning_debug_tools/package.xml @@ -0,0 +1,35 @@ + + + + planning_debug_tools + 0.1.0 + The planning_debug_tools package + Takamasa Horibe + Apache License 2.0 + + 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/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)