diff --git a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/planning.hpp b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/planning.hpp new file mode 100644 index 0000000000000..4d4530dc38b75 --- /dev/null +++ b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/planning.hpp @@ -0,0 +1,46 @@ +// 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 AUTOWARE_AD_API_SPECS__PLANNING_HPP_ +#define AUTOWARE_AD_API_SPECS__PLANNING_HPP_ + +#include + +#include +#include + +namespace autoware_ad_api::planning +{ + +struct VelocityFactors +{ + using Message = autoware_adapi_v1_msgs::msg::VelocityFactorArray; + static constexpr char name[] = "/api/planning/velocity_factors"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; +}; + +struct SteeringFactors +{ + using Message = autoware_adapi_v1_msgs::msg::SteeringFactorArray; + static constexpr char name[] = "/api/planning/steering_factors"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; +}; + +} // namespace autoware_ad_api::planning + +#endif // AUTOWARE_AD_API_SPECS__PLANNING_HPP_ diff --git a/common/component_interface_specs/include/component_interface_specs/localization.hpp b/common/component_interface_specs/include/component_interface_specs/localization.hpp index a1a39f5cd57af..488a18b40c99c 100644 --- a/common/component_interface_specs/include/component_interface_specs/localization.hpp +++ b/common/component_interface_specs/include/component_interface_specs/localization.hpp @@ -19,6 +19,7 @@ #include #include +#include namespace localization_interface { @@ -38,6 +39,15 @@ struct InitializationState static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; }; +struct KinematicState +{ + using Message = nav_msgs::msg::Odometry; + static constexpr char name[] = "/localization/kinematic_state"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; +}; + } // namespace localization_interface #endif // COMPONENT_INTERFACE_SPECS__LOCALIZATION_HPP_ diff --git a/common/component_interface_specs/include/component_interface_specs/planning.hpp b/common/component_interface_specs/include/component_interface_specs/planning.hpp index c3106cf7b60ad..fecee4426cc11 100644 --- a/common/component_interface_specs/include/component_interface_specs/planning.hpp +++ b/common/component_interface_specs/include/component_interface_specs/planning.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -62,6 +63,15 @@ struct Route static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; }; +struct Trajectory +{ + using Message = autoware_auto_planning_msgs::msg::Trajectory; + static constexpr char name[] = "/planning/scenario_planning/trajectory"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; +}; + } // namespace planning_interface #endif // COMPONENT_INTERFACE_SPECS__PLANNING_HPP_ diff --git a/system/default_ad_api/CMakeLists.txt b/system/default_ad_api/CMakeLists.txt index e0656a15b82f3..6221a370eab72 100644 --- a/system/default_ad_api/CMakeLists.txt +++ b/system/default_ad_api/CMakeLists.txt @@ -10,6 +10,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/localization.cpp src/motion.cpp src/operation_mode.cpp + src/planning.cpp src/routing.cpp src/utils/route_conversion.cpp src/compatibility/autoware_state.cpp @@ -22,6 +23,7 @@ rclcpp_components_register_nodes(${PROJECT_NAME} "default_ad_api::LocalizationNode" "default_ad_api::MotionNode" "default_ad_api::OperationModeNode" + "default_ad_api::PlanningNode" "default_ad_api::RoutingNode" ) diff --git a/system/default_ad_api/launch/default_ad_api.launch.py b/system/default_ad_api/launch/default_ad_api.launch.py index d3182a9070c2a..50211ede05e86 100644 --- a/system/default_ad_api/launch/default_ad_api.launch.py +++ b/system/default_ad_api/launch/default_ad_api.launch.py @@ -47,6 +47,7 @@ def generate_launch_description(): create_api_node("localization", "LocalizationNode"), create_api_node("motion", "MotionNode"), create_api_node("operation_mode", "OperationModeNode"), + create_api_node("planning", "PlanningNode"), create_api_node("routing", "RoutingNode"), ] container = ComposableNodeContainer( diff --git a/system/default_ad_api/src/planning.cpp b/system/default_ad_api/src/planning.cpp new file mode 100644 index 0000000000000..2fa7ae5b97749 --- /dev/null +++ b/system/default_ad_api/src/planning.cpp @@ -0,0 +1,151 @@ +// 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 "planning.hpp" + +#include +#include +#include + +namespace default_ad_api +{ + +template +void concat(std::vector & v1, const std::vector & v2) +{ + v1.insert(v1.end(), v2.begin(), v2.end()); +} + +template +std::vector::SharedPtr> init_factors( + rclcpp::Node * node, std::vector & factors, + const std::vector & topics) +{ + const auto callback = [&factors](const int index) { + return [&factors, index](const typename T::ConstSharedPtr msg) { factors[index] = msg; }; + }; + + std::vector::SharedPtr> subs; + for (size_t index = 0; index < topics.size(); ++index) { + subs.push_back(node->create_subscription(topics[index], rclcpp::QoS(1), callback(index))); + } + factors.resize(topics.size()); + return subs; +} + +template +T merge_factors(const rclcpp::Time stamp, const std::vector & factors) +{ + T message; + message.header.stamp = stamp; + message.header.frame_id = "map"; + + for (const auto & factor : factors) { + if (factor) { + concat(message.factors, factor->factors); + } + } + return message; +} + +PlanningNode::PlanningNode(const rclcpp::NodeOptions & options) : Node("planning", options) +{ + // TODO(Takagi, Isamu): remove default value + stop_distance_ = declare_parameter("stop_distance", 1.0); + stop_duration_ = declare_parameter("stop_duration", 1.0); + stop_checker_ = std::make_unique(this, stop_duration_ + 1.0); + + std::vector velocity_factor_topics = { + "/planning/velocity_factors/blind_spot", + "/planning/velocity_factors/crosswalk", + "/planning/velocity_factors/detection_area", + "/planning/velocity_factors/intersection", + "/planning/velocity_factors/merge_from_private", + "/planning/velocity_factors/no_stopping_area", + "/planning/velocity_factors/obstacle_stop", + "/planning/velocity_factors/obstacle_cruise", + "/planning/velocity_factors/occlusion_spot", + "/planning/velocity_factors/stop_line", + "/planning/velocity_factors/surround_obstacle", + "/planning/velocity_factors/traffic_light", + "/planning/velocity_factors/virtual_traffic_light", + "/planning/velocity_factors/walkway"}; + + std::vector steering_factor_topics = { + "/planning/steering_factor/avoidance", "/planning/steering_factor/intersection", + "/planning/steering_factor/lane_change", "/planning/steering_factor/pull_out", + "/planning/steering_factor/pull_over"}; + + sub_velocity_factors_ = + init_factors(this, velocity_factors_, velocity_factor_topics); + sub_steering_factors_ = + init_factors(this, steering_factors_, steering_factor_topics); + + const auto adaptor = component_interface_utils::NodeAdaptor(this); + adaptor.init_pub(pub_velocity_factors_); + adaptor.init_pub(pub_steering_factors_); + adaptor.init_sub(sub_kinematic_state_, this, &PlanningNode::on_kinematic_state); + adaptor.init_sub(sub_trajectory_, this, &PlanningNode::on_trajectory); + + const auto rate = rclcpp::Rate(5); + timer_ = rclcpp::create_timer(this, get_clock(), rate.period(), [this]() { on_timer(); }); +} + +void PlanningNode::on_trajectory(const Trajectory::ConstSharedPtr msg) { trajectory_ = msg; } + +void PlanningNode::on_kinematic_state(const KinematicState::ConstSharedPtr msg) +{ + kinematic_state_ = msg; + + geometry_msgs::msg::TwistStamped twist; + twist.header = msg->header; + twist.twist = msg->twist.twist; + stop_checker_->addTwist(twist); +} + +void PlanningNode::on_timer() +{ + using autoware_adapi_v1_msgs::msg::VelocityFactor; + auto velocity = merge_factors(now(), velocity_factors_); + auto steering = merge_factors(now(), steering_factors_); + + // Set the distance if it is nan. + if (trajectory_ && kinematic_state_) { + for (auto & factor : velocity.factors) { + if (std::isnan(factor.distance)) { + const auto & curr_point = kinematic_state_->pose.pose.position; + const auto & stop_point = factor.pose.position; + const auto & points = trajectory_->points; + factor.distance = motion_utils::calcSignedArcLength(points, curr_point, stop_point); + } + } + } + + // Set the status if it is unknown. + const auto is_vehicle_stopped = stop_checker_->isVehicleStopped(stop_duration_); + for (auto & factor : velocity.factors) { + if ((factor.status == VelocityFactor::UNKNOWN) && (!std::isnan(factor.distance))) { + const auto is_stopped = is_vehicle_stopped && (factor.distance < stop_distance_); + factor.status = is_stopped ? VelocityFactor::STOPPED : VelocityFactor::APPROACHING; + } + } + + pub_velocity_factors_->publish(velocity); + pub_steering_factors_->publish(steering); +} + +} // namespace default_ad_api + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::PlanningNode) diff --git a/system/default_ad_api/src/planning.hpp b/system/default_ad_api/src/planning.hpp new file mode 100644 index 0000000000000..03b45fd3b5d41 --- /dev/null +++ b/system/default_ad_api/src/planning.hpp @@ -0,0 +1,67 @@ +// 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_HPP_ +#define PLANNING_HPP_ + +#include +#include +#include +#include +#include + +#include +#include + +// This file should be included after messages. +#include "utils/types.hpp" + +namespace default_ad_api +{ + +class PlanningNode : public rclcpp::Node +{ +public: + explicit PlanningNode(const rclcpp::NodeOptions & options); + +private: + using VelocityFactorArray = autoware_adapi_v1_msgs::msg::VelocityFactorArray; + using SteeringFactorArray = autoware_adapi_v1_msgs::msg::SteeringFactorArray; + Pub pub_velocity_factors_; + Pub pub_steering_factors_; + Sub sub_trajectory_; + Sub sub_kinematic_state_; + std::vector::SharedPtr> sub_velocity_factors_; + std::vector::SharedPtr> sub_steering_factors_; + std::vector velocity_factors_; + std::vector steering_factors_; + rclcpp::TimerBase::SharedPtr timer_; + + using VehicleStopChecker = motion_utils::VehicleStopCheckerBase; + using Trajectory = planning_interface::Trajectory::Message; + using KinematicState = localization_interface::KinematicState::Message; + void on_trajectory(const Trajectory::ConstSharedPtr msg); + void on_kinematic_state(const KinematicState::ConstSharedPtr msg); + void on_timer(); + + double stop_distance_; + double stop_duration_; + std::unique_ptr stop_checker_; + Trajectory::ConstSharedPtr trajectory_; + KinematicState::ConstSharedPtr kinematic_state_; +}; + +} // namespace default_ad_api + +#endif // PLANNING_HPP_ diff --git a/system/default_ad_api_helpers/ad_api_visualizers/ad_api_visualizers/__init__.py b/system/default_ad_api_helpers/ad_api_visualizers/ad_api_visualizers/__init__.py new file mode 100644 index 0000000000000..e69de29bb2d1d diff --git a/system/default_ad_api_helpers/ad_api_visualizers/ad_api_visualizers/planning_factors.py b/system/default_ad_api_helpers/ad_api_visualizers/ad_api_visualizers/planning_factors.py new file mode 100755 index 0000000000000..194551d56dcd5 --- /dev/null +++ b/system/default_ad_api_helpers/ad_api_visualizers/ad_api_visualizers/planning_factors.py @@ -0,0 +1,159 @@ +#!/usr/bin/env python3 + +from autoware_adapi_v1_msgs.msg import SteeringFactor +from autoware_adapi_v1_msgs.msg import SteeringFactorArray +from autoware_adapi_v1_msgs.msg import VelocityFactor +from autoware_adapi_v1_msgs.msg import VelocityFactorArray +from geometry_msgs.msg import Pose +import numpy +import rclpy +import rclpy.duration +import rclpy.node +import tf_transformations +from visualization_msgs.msg import Marker +from visualization_msgs.msg import MarkerArray + +velocity_type_text = { + VelocityFactor.UNKNOWN: "unknown", + VelocityFactor.SURROUNDING_OBSTACLE: "surrounding obstacle", + VelocityFactor.ROUTE_OBSTACLE: "route obstacle", + VelocityFactor.INTERSECTION: "intersection", + VelocityFactor.CROSSWALK: "crosswalk", + VelocityFactor.REAR_CHECK: "rear check", + VelocityFactor.USER_DEFINED_DETECTION_AREA: "detection area", + VelocityFactor.NO_STOPPING_AREA: "no stopping area", + VelocityFactor.STOP_SIGN: "stop sign", + VelocityFactor.TRAFFIC_SIGNAL: "traffic signal", + VelocityFactor.V2I_GATE_CONTROL_ENTER: "v2i enter", + VelocityFactor.V2I_GATE_CONTROL_LEAVE: "v2i leave", + VelocityFactor.MERGE: "merge", + VelocityFactor.SIDEWALK: "sidewalk", + VelocityFactor.LANE_CHANGE: "lane change", + VelocityFactor.AVOIDANCE: "avoidance", + VelocityFactor.EMERGENCY_STOP_OPERATION: "emergency stop operation", +} + +steering_type_text = { + SteeringFactor.INTERSECTION: "intersection", + SteeringFactor.LANE_CHANGE: "lane change", + SteeringFactor.AVOIDANCE_PATH_CHANGE: "avoidance change", + SteeringFactor.AVOIDANCE_PATH_RETURN: "avoidance return", + SteeringFactor.STATION: "station", + SteeringFactor.PULL_OUT: "pull out", + SteeringFactor.PULL_OVER: "pull over", + SteeringFactor.EMERGENCY_OPERATION: "emergency operation", +} + +velocity_status_color = { + VelocityFactor.UNKNOWN: (1.0, 1.0, 1.0), + VelocityFactor.APPROACHING: (1.0, 0.0, 0.0), + VelocityFactor.STOPPED: (1.0, 1.0, 0.0), +} + +steering_status_color = { + SteeringFactor.UNKNOWN: (1.0, 1.0, 1.0), + SteeringFactor.APPROACHING: (1.0, 0.0, 0.0), + SteeringFactor.TRYING: (1.0, 1.0, 0.0), + SteeringFactor.TURNING: (1.0, 1.0, 1.0), +} + + +class PlanningFactorVisualizer(rclpy.node.Node): + def __init__(self): + super().__init__("planning_factor_visualizer") + self.front_offset = self.declare_parameter("front_offset", 0.0).value + self.pub_velocity = self.create_publisher(MarkerArray, "/visualizer/velocity_factors", 1) + self.pub_steering = self.create_publisher(MarkerArray, "/visualizer/steering_factors", 1) + self.sub_velocity = self.create_subscription(VelocityFactorArray, "/api/planning/velocity_factors", self.on_velocity_factor, 1) # fmt: skip + self.sub_steering = self.create_subscription(SteeringFactorArray, "/api/planning/steering_factors", self.on_steering_factor, 1) # fmt: skip + + def on_velocity_factor(self, msg): + markers = MarkerArray() + for index, factor in enumerate(msg.factors): + color = velocity_status_color[factor.status] + text = velocity_type_text[factor.type] + markers.markers.append(self.create_wall(index, msg.header, factor.pose, color)) + markers.markers.append(self.create_text(index, msg.header, factor.pose, text)) + self.pub_velocity.publish(markers) + + def on_steering_factor(self, msg): + markers = MarkerArray() + for index, factor in enumerate(msg.factors): + index1 = index * 2 + 0 + index2 = index * 2 + 1 + color = steering_status_color[factor.status] + text = steering_type_text[factor.type] + markers.markers.append(self.create_wall(index1, msg.header, factor.pose[0], color)) + markers.markers.append(self.create_wall(index2, msg.header, factor.pose[1], color)) + markers.markers.append(self.create_text(index1, msg.header, factor.pose[0], text)) + markers.markers.append(self.create_text(index2, msg.header, factor.pose[1], text)) + self.pub_steering.publish(markers) + + def offset_pose(self, pose): + q = pose.orientation + q = numpy.array([q.x, q.y, q.z, q.w]) + p = numpy.array([self.front_offset, 0, 0, 1]) + r = tf_transformations.quaternion_matrix(q) + x = numpy.dot(r, p) + result = Pose() + result.position.x = pose.position.x + x[0] + result.position.y = pose.position.y + x[1] + result.position.z = pose.position.z + x[2] + result.orientation = pose.orientation + return result + + def create_wall(self, index, header, pose, color): + pose = self.offset_pose(pose) + marker = Marker() + marker.ns = "wall" + marker.id = index + marker.lifetime = rclpy.duration.Duration(nanoseconds=0.3 * 1e9).to_msg() + marker.header = header + marker.action = Marker.ADD + marker.type = Marker.CUBE + marker.pose.position.x = pose.position.x + marker.pose.position.y = pose.position.y + marker.pose.position.z = pose.position.z + 1.0 + marker.pose.orientation = pose.orientation + marker.scale.x = 0.1 + marker.scale.y = 5.0 + marker.scale.z = 2.0 + marker.color.a = 0.7 + marker.color.r = color[0] + marker.color.g = color[1] + marker.color.b = color[2] + return marker + + def create_text(self, index, header, pose, text): + pose = self.offset_pose(pose) + marker = Marker() + marker.ns = "type" + marker.id = index + marker.lifetime = rclpy.duration.Duration(nanoseconds=0.3 * 1e9).to_msg() + marker.header = header + marker.action = Marker.ADD + marker.type = Marker.TEXT_VIEW_FACING + marker.text = text + marker.pose.position.x = pose.position.x + marker.pose.position.y = pose.position.y + marker.pose.position.z = pose.position.z + 2.0 + marker.pose.orientation = pose.orientation + marker.scale.z = 1.0 + marker.color.a = 1.0 + marker.color.r = 1.0 + marker.color.g = 1.0 + marker.color.b = 1.0 + return marker + + +def main(args=None): + try: + rclpy.init(args=args) + rclpy.spin(PlanningFactorVisualizer()) + rclpy.shutdown() + except KeyboardInterrupt: + pass + + +if __name__ == "__main__": + main() diff --git a/system/default_ad_api_helpers/ad_api_visualizers/launch/ad_api_visualizers.launch.xml b/system/default_ad_api_helpers/ad_api_visualizers/launch/ad_api_visualizers.launch.xml new file mode 100644 index 0000000000000..26fb4720ca435 --- /dev/null +++ b/system/default_ad_api_helpers/ad_api_visualizers/launch/ad_api_visualizers.launch.xml @@ -0,0 +1,5 @@ + + + + + diff --git a/system/default_ad_api_helpers/ad_api_visualizers/package.xml b/system/default_ad_api_helpers/ad_api_visualizers/package.xml new file mode 100644 index 0000000000000..642e07d724710 --- /dev/null +++ b/system/default_ad_api_helpers/ad_api_visualizers/package.xml @@ -0,0 +1,25 @@ + + + + ad_api_visualizers + 0.1.0 + The ad_api_visualizers package + Takagi, Isamu + yabuta + Kah Hooi Tan + Apache License 2.0 + + autoware_adapi_v1_msgs + geometry_msgs + rclpy + tf_transformations + visualization_msgs + + ament_copyright + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/system/default_ad_api_helpers/ad_api_visualizers/resource/ad_api_visualizers b/system/default_ad_api_helpers/ad_api_visualizers/resource/ad_api_visualizers new file mode 100644 index 0000000000000..e69de29bb2d1d diff --git a/system/default_ad_api_helpers/ad_api_visualizers/setup.cfg b/system/default_ad_api_helpers/ad_api_visualizers/setup.cfg new file mode 100644 index 0000000000000..b0af17360079d --- /dev/null +++ b/system/default_ad_api_helpers/ad_api_visualizers/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/ad_api_visualizers +[install] +install_scripts=$base/lib/ad_api_visualizers diff --git a/system/default_ad_api_helpers/ad_api_visualizers/setup.py b/system/default_ad_api_helpers/ad_api_visualizers/setup.py new file mode 100644 index 0000000000000..5712980f8e3fb --- /dev/null +++ b/system/default_ad_api_helpers/ad_api_visualizers/setup.py @@ -0,0 +1,31 @@ +from warnings import simplefilter + +from pkg_resources import PkgResourcesDeprecationWarning +from setuptools import SetuptoolsDeprecationWarning +from setuptools import setup + +simplefilter("ignore", category=SetuptoolsDeprecationWarning) +simplefilter("ignore", category=PkgResourcesDeprecationWarning) + +package_name = "ad_api_visualizers" + +setup( + name=package_name, + version="0.0.0", + packages=[package_name], + data_files=[ + ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]), + (f"share/{package_name}", ["package.xml"]), + (f"share/{package_name}/launch", [f"launch/{package_name}.launch.xml"]), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="Takagi, Isamu", + maintainer_email="isamu.takagi@tier4.jp", + description="The ad_api_visualizers package", + license="Apache License 2.0", + tests_require=["pytest"], + entry_points={ + "console_scripts": ["planning_factors = ad_api_visualizers.planning_factors:main"], + }, +) diff --git a/system/default_ad_api_helpers/ad_api_visualizers/test/test_copyright.py b/system/default_ad_api_helpers/ad_api_visualizers/test/test_copyright.py new file mode 100644 index 0000000000000..95f03810541d2 --- /dev/null +++ b/system/default_ad_api_helpers/ad_api_visualizers/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, 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. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason="No copyright header has been placed in the generated source file.") +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" diff --git a/system/default_ad_api_helpers/ad_api_visualizers/test/test_pep257.py b/system/default_ad_api_helpers/ad_api_visualizers/test/test_pep257.py new file mode 100644 index 0000000000000..a2c3deb8eb3de --- /dev/null +++ b/system/default_ad_api_helpers/ad_api_visualizers/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found code style errors / warnings"