From cb5cd89c43e6fe5e589946c9e79bff47e3d87703 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=BF=83=E5=88=9A?= <90366790+liuXinGangChina@users.noreply.github.com> Date: Wed, 19 Feb 2025 12:53:49 +0800 Subject: [PATCH 1/8] feat(autoware_gnss_poser): porting from universe to core (#166) Signed-off-by: liuXinGangChina Signed-off-by: mitsudome-r --- sensing/autoware_gnss_poser/CMakeLists.txt | 39 ++ sensing/autoware_gnss_poser/README.md | 45 ++ .../config/gnss_poser.param.yaml | 8 + .../autoware/gnss_poser/gnss_poser_node.hpp | 104 +++++ .../launch/gnss_poser.launch.xml | 18 + sensing/autoware_gnss_poser/package.xml | 43 ++ .../schema/gnss_poser.schema.json | 65 +++ .../src/gnss_poser_node.cpp | 417 ++++++++++++++++++ 8 files changed, 739 insertions(+) create mode 100644 sensing/autoware_gnss_poser/CMakeLists.txt create mode 100644 sensing/autoware_gnss_poser/README.md create mode 100644 sensing/autoware_gnss_poser/config/gnss_poser.param.yaml create mode 100644 sensing/autoware_gnss_poser/include/autoware/gnss_poser/gnss_poser_node.hpp create mode 100644 sensing/autoware_gnss_poser/launch/gnss_poser.launch.xml create mode 100644 sensing/autoware_gnss_poser/package.xml create mode 100644 sensing/autoware_gnss_poser/schema/gnss_poser.schema.json create mode 100644 sensing/autoware_gnss_poser/src/gnss_poser_node.cpp diff --git a/sensing/autoware_gnss_poser/CMakeLists.txt b/sensing/autoware_gnss_poser/CMakeLists.txt new file mode 100644 index 0000000000..94317dd21a --- /dev/null +++ b/sensing/autoware_gnss_poser/CMakeLists.txt @@ -0,0 +1,39 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_gnss_poser) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +## Find non-ROS library +find_package(PkgConfig) +find_path(GeographicLib_INCLUDE_DIR GeographicLib/Config.h + PATH_SUFFIXES GeographicLib +) + +set(GeographicLib_INCLUDE_DIRS ${GeographicLib_INCLUDE_DIR}) +find_library(GeographicLib_LIBRARIES + NAMES Geographic +) + +set(GNSS_POSER_HEADERS + include/autoware/gnss_poser/gnss_poser_node.hpp +) + +ament_auto_add_library(gnss_poser_node SHARED + src/gnss_poser_node.cpp + ${GNSS_POSER_HEADERS} +) + +target_link_libraries(gnss_poser_node + Geographic +) + +rclcpp_components_register_node(gnss_poser_node + PLUGIN "autoware::gnss_poser::GNSSPoser" + EXECUTABLE gnss_poser +) + +ament_auto_package(INSTALL_TO_SHARE + config + launch +) diff --git a/sensing/autoware_gnss_poser/README.md b/sensing/autoware_gnss_poser/README.md new file mode 100644 index 0000000000..464498c270 --- /dev/null +++ b/sensing/autoware_gnss_poser/README.md @@ -0,0 +1,45 @@ +# gnss_poser + +## Overview + +The `gnss_poser` is a node that subscribes gnss sensing messages and calculates vehicle pose with covariance. + +## Design + +This node subscribes to NavSatFix to publish the pose of **base_link**. The data in NavSatFix represents the antenna's position. Therefore, it performs a coordinate transformation using the tf from `base_link` to the antenna's position. The frame_id of the antenna's position refers to NavSatFix's `header.frame_id`. +(**Note that `header.frame_id` in NavSatFix indicates the antenna's frame_id, not the Earth or reference ellipsoid.** [See also NavSatFix definition.](https://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/NavSatFix.html)) + +If the transformation from `base_link` to the antenna cannot be obtained, it outputs the pose of the antenna position without performing coordinate transformation. + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ------------------------------ | ------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------ | +| `/map/map_projector_info` | `autoware_map_msgs::msg::MapProjectorInfo` | map projection info | +| `~/input/fix` | `sensor_msgs::msg::NavSatFix` | gnss status message | +| `~/input/autoware_orientation` | `autoware_sensing_msgs::msg::GnssInsOrientationStamped` | orientation [click here for more details](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_sensing_msgs) | + +### Output + +| Name | Type | Description | +| ------------------------ | ------------------------------------------------ | -------------------------------------------------------------- | +| `~/output/pose` | `geometry_msgs::msg::PoseStamped` | vehicle pose calculated from gnss sensing data | +| `~/output/gnss_pose_cov` | `geometry_msgs::msg::PoseWithCovarianceStamped` | vehicle pose with covariance calculated from gnss sensing data | +| `~/output/gnss_fixed` | `autoware_internal_debug_msgs::msg::BoolStamped` | gnss fix status | + +## Parameters + +Parameters in below table + +| Name | Type | Default | Description | +| -------------------------- | --------- | ---------------- | ---------------------------------------------------------------------------------------------------------------------------------- | +| `base_frame` | `string` | `base_link` | frame id for base_frame | +| `gnss_base_frame` | `string` | `gnss_base_link` | frame id for gnss_base_frame | +| `map_frame` | `string` | `map` | frame id for map_frame | +| `use_gnss_ins_orientation` | `boolean` | `true` | use Gnss-Ins orientation | +| `gnss_pose_pub_method` | `integer` | `0` | 0: Instant Value 1: Average Value 2: Median Value. If `buffer_epoch` is set to 0, `gnss_pose_pub_method` loses affect. Range: 0~2. | +| `buff_epoch` | `integer` | `1` | Buffer epoch. Range: 0~inf. | + +All above parameters can be changed in config file [gnss_poser.param.yaml](./config/gnss_poser.param.yaml "Click here to open config file") . diff --git a/sensing/autoware_gnss_poser/config/gnss_poser.param.yaml b/sensing/autoware_gnss_poser/config/gnss_poser.param.yaml new file mode 100644 index 0000000000..1c00b43c20 --- /dev/null +++ b/sensing/autoware_gnss_poser/config/gnss_poser.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + base_frame: base_link + gnss_base_frame: gnss_base_link + map_frame: map + buff_epoch: 1 + use_gnss_ins_orientation: true + gnss_pose_pub_method: 0 diff --git a/sensing/autoware_gnss_poser/include/autoware/gnss_poser/gnss_poser_node.hpp b/sensing/autoware_gnss_poser/include/autoware/gnss_poser/gnss_poser_node.hpp new file mode 100644 index 0000000000..a9ada9e960 --- /dev/null +++ b/sensing/autoware_gnss_poser/include/autoware/gnss_poser/gnss_poser_node.hpp @@ -0,0 +1,104 @@ +// Copyright 2020 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__GNSS_POSER__GNSS_POSER_NODE_HPP_ +#define AUTOWARE__GNSS_POSER__GNSS_POSER_NODE_HPP_ + +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include +#include + +#include + +namespace autoware::gnss_poser +{ +class GNSSPoser : public rclcpp::Node +{ +public: + explicit GNSSPoser(const rclcpp::NodeOptions & node_options); + +private: + void callback_map_projector_info( + const autoware_map_msgs::msg::MapProjectorInfo::ConstSharedPtr msg); + void callback_nav_sat_fix(const sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_fix_msg_ptr); + void callback_gnss_ins_orientation_stamped( + const autoware_sensing_msgs::msg::GnssInsOrientationStamped::ConstSharedPtr msg); + + static bool is_fixed(const sensor_msgs::msg::NavSatStatus & nav_sat_status_msg); + static bool can_get_covariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg); + static geometry_msgs::msg::Point get_median_position( + const boost::circular_buffer & position_buffer); + static geometry_msgs::msg::Point get_average_position( + const boost::circular_buffer & position_buffer); + static geometry_msgs::msg::Quaternion get_quaternion_by_heading(const int heading); + static geometry_msgs::msg::Quaternion get_quaternion_by_position_difference( + const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Point & prev_point); + + bool get_transform( + const std::string & target_frame, const std::string & source_frame, + const geometry_msgs::msg::TransformStamped::SharedPtr transform_stamped_ptr); + bool get_static_transform( + const std::string & target_frame, const std::string & source_frame, + const geometry_msgs::msg::TransformStamped::SharedPtr transform_stamped_ptr, + const builtin_interfaces::msg::Time & stamp); + void publish_tf( + const std::string & frame_id, const std::string & child_frame_id, + const geometry_msgs::msg::PoseStamped & pose_msg); + + tf2::BufferCore tf2_buffer_; + tf2_ros::TransformListener tf2_listener_; + tf2_ros::TransformBroadcaster tf2_broadcaster_; + + rclcpp::Subscription::SharedPtr sub_map_projector_info_; + rclcpp::Subscription::SharedPtr nav_sat_fix_sub_; + rclcpp::Subscription::SharedPtr + autoware_orientation_sub_; + + rclcpp::Publisher::SharedPtr pose_pub_; + rclcpp::Publisher::SharedPtr pose_cov_pub_; + rclcpp::Publisher::SharedPtr fixed_pub_; + + autoware_map_msgs::msg::MapProjectorInfo projector_info_; + const std::string base_frame_; + const std::string gnss_base_frame_; + const std::string map_frame_; + bool received_map_projector_info_ = false; + bool use_gnss_ins_orientation_; + + boost::circular_buffer position_buffer_; + + autoware_sensing_msgs::msg::GnssInsOrientationStamped::SharedPtr + msg_gnss_ins_orientation_stamped_; + int gnss_pose_pub_method_; +}; +} // namespace autoware::gnss_poser + +#endif // AUTOWARE__GNSS_POSER__GNSS_POSER_NODE_HPP_ diff --git a/sensing/autoware_gnss_poser/launch/gnss_poser.launch.xml b/sensing/autoware_gnss_poser/launch/gnss_poser.launch.xml new file mode 100644 index 0000000000..ef2e2661a1 --- /dev/null +++ b/sensing/autoware_gnss_poser/launch/gnss_poser.launch.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/sensing/autoware_gnss_poser/package.xml b/sensing/autoware_gnss_poser/package.xml new file mode 100644 index 0000000000..6746f625ac --- /dev/null +++ b/sensing/autoware_gnss_poser/package.xml @@ -0,0 +1,43 @@ + + + + autoware_gnss_poser + 0.0.0 + The ROS 2 autoware_gnss_poser package + Xingang Liu + Yamato Ando + Masahiro Sakamoto + Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto + Apache License 2.0 + Ryo Watanabe + + ament_cmake_auto + autoware_cmake + + libboost-dev + + autoware_geography_utils + autoware_internal_debug_msgs + autoware_map_msgs + autoware_sensing_msgs + geographic_msgs + geographiclib + geometry_msgs + rclcpp + rclcpp_components + sensor_msgs + tf2 + tf2_geometry_msgs + tf2_ros + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/sensing/autoware_gnss_poser/schema/gnss_poser.schema.json b/sensing/autoware_gnss_poser/schema/gnss_poser.schema.json new file mode 100644 index 0000000000..d6e104b743 --- /dev/null +++ b/sensing/autoware_gnss_poser/schema/gnss_poser.schema.json @@ -0,0 +1,65 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Gnss Poser Node", + "type": "object", + "definitions": { + "gnss_poser": { + "type": "object", + "properties": { + "base_frame": { + "type": "string", + "default": "base_link", + "description": "frame id for base_frame" + }, + "gnss_base_frame": { + "type": "string", + "default": "gnss_base_link", + "description": "frame id for gnss_base_frame" + }, + "map_frame": { + "type": "string", + "default": "map", + "description": "frame id for map_frame" + }, + "use_gnss_ins_orientation": { + "type": "boolean", + "default": "true", + "description": "use Gnss-Ins orientation" + }, + "gnss_pose_pub_method": { + "type": "integer", + "default": "0", + "minimum": 0, + "maximum": 2, + "description": "0: Instant Value 1: Average Value 2: Median Value. If 0 is chosen buffer_epoch parameter loses affect." + }, + "buff_epoch": { + "type": "integer", + "default": "1", + "minimum": 0, + "description": "Buffer epoch" + } + }, + "required": [ + "base_frame", + "gnss_base_frame", + "map_frame", + "use_gnss_ins_orientation", + "gnss_pose_pub_method", + "buff_epoch" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/gnss_poser" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/sensing/autoware_gnss_poser/src/gnss_poser_node.cpp b/sensing/autoware_gnss_poser/src/gnss_poser_node.cpp new file mode 100644 index 0000000000..7ced9bd842 --- /dev/null +++ b/sensing/autoware_gnss_poser/src/gnss_poser_node.cpp @@ -0,0 +1,417 @@ +// Copyright 2020 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 "autoware/gnss_poser/gnss_poser_node.hpp" + +#include +#include + +#include + +#include +#include +#include +#include + +namespace autoware::gnss_poser +{ +GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) +: rclcpp::Node("gnss_poser", node_options), + tf2_listener_(tf2_buffer_), + tf2_broadcaster_(*this), + base_frame_(declare_parameter("base_frame")), + gnss_base_frame_(declare_parameter("gnss_base_frame")), + map_frame_(declare_parameter("map_frame")), + use_gnss_ins_orientation_(declare_parameter("use_gnss_ins_orientation")), + msg_gnss_ins_orientation_stamped_( + std::make_shared()), + gnss_pose_pub_method_(static_cast(declare_parameter("gnss_pose_pub_method"))) +{ + // Subscribe to map_projector_info topic + sub_map_projector_info_ = create_subscription( + "/map/map_projector_info", rclcpp::QoS{1}.transient_local(), + std::bind(&GNSSPoser::callback_map_projector_info, this, std::placeholders::_1)); + + // Set up position buffer + int buff_epoch = static_cast(declare_parameter("buff_epoch")); + position_buffer_.set_capacity(buff_epoch); + + // Set subscribers and publishers + nav_sat_fix_sub_ = create_subscription( + "fix", rclcpp::QoS{1}, + std::bind(&GNSSPoser::callback_nav_sat_fix, this, std::placeholders::_1)); + autoware_orientation_sub_ = + create_subscription( + "autoware_orientation", rclcpp::QoS{1}, + std::bind(&GNSSPoser::callback_gnss_ins_orientation_stamped, this, std::placeholders::_1)); + + pose_pub_ = create_publisher("gnss_pose", rclcpp::QoS{1}); + pose_cov_pub_ = create_publisher( + "gnss_pose_cov", rclcpp::QoS{1}); + fixed_pub_ = + create_publisher("gnss_fixed", rclcpp::QoS{1}); + + // Set msg_gnss_ins_orientation_stamped_ with temporary values (not to publish zero value + // covariances) + msg_gnss_ins_orientation_stamped_->orientation.rmse_rotation_x = 1.0; + msg_gnss_ins_orientation_stamped_->orientation.rmse_rotation_y = 1.0; + msg_gnss_ins_orientation_stamped_->orientation.rmse_rotation_z = 1.0; +} + +void GNSSPoser::callback_map_projector_info( + const autoware_map_msgs::msg::MapProjectorInfo::ConstSharedPtr msg) +{ + projector_info_ = *msg; + received_map_projector_info_ = true; +} + +void GNSSPoser::callback_nav_sat_fix( + const sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_fix_msg_ptr) +{ + // Return immediately if map_projector_info has not been received yet. + if (!received_map_projector_info_) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), + "map_projector_info has not been received yet. Check if the map_projection_loader is " + "successfully launched."); + return; + } + + if (projector_info_.projector_type == autoware_map_msgs::msg::MapProjectorInfo::LOCAL) { + RCLCPP_ERROR_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), + "map_projector_info is local projector type. Unable to convert GNSS pose."); + return; + } + + // check fixed topic + const bool is_status_fixed = is_fixed(nav_sat_fix_msg_ptr->status); + + // publish is_fixed topic + autoware_internal_debug_msgs::msg::BoolStamped is_fixed_msg; + is_fixed_msg.stamp = this->now(); + is_fixed_msg.data = is_status_fixed; + fixed_pub_->publish(is_fixed_msg); + + if (!is_status_fixed) { + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), + "Not Fixed Topic. Skipping Calculate."); + return; + } + + // get position + geographic_msgs::msg::GeoPoint gps_point; + gps_point.latitude = nav_sat_fix_msg_ptr->latitude; + gps_point.longitude = nav_sat_fix_msg_ptr->longitude; + gps_point.altitude = nav_sat_fix_msg_ptr->altitude; + geometry_msgs::msg::Point position = + autoware::geography_utils::project_forward(gps_point, projector_info_); + position.z = autoware::geography_utils::convert_height( + position.z, gps_point.latitude, gps_point.longitude, + autoware_map_msgs::msg::MapProjectorInfo::WGS84, projector_info_.vertical_datum); + + geometry_msgs::msg::Pose gnss_antenna_pose{}; + + // publish pose immediately + if (!gnss_pose_pub_method_) { + gnss_antenna_pose.position = position; + } else { + // fill position buffer + position_buffer_.push_front(position); + if (!position_buffer_.full()) { + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), + "Buffering Position. Output Skipped."); + return; + } + // publish average pose or median pose of position buffer + gnss_antenna_pose.position = (gnss_pose_pub_method_ == 1) + ? get_average_position(position_buffer_) + : get_median_position(position_buffer_); + } + + // calc gnss antenna orientation + geometry_msgs::msg::Quaternion orientation; + if (use_gnss_ins_orientation_) { + orientation = msg_gnss_ins_orientation_stamped_->orientation.orientation; + } else { + static auto prev_position = gnss_antenna_pose.position; + orientation = get_quaternion_by_position_difference(gnss_antenna_pose.position, prev_position); + prev_position = gnss_antenna_pose.position; + } + + gnss_antenna_pose.orientation = orientation; + + tf2::Transform tf_map2gnss_antenna{}; + tf2::fromMsg(gnss_antenna_pose, tf_map2gnss_antenna); + + // get TF from gnss_antenna to base_link + auto tf_gnss_antenna2base_link_msg_ptr = std::make_shared(); + + const std::string gnss_frame = nav_sat_fix_msg_ptr->header.frame_id; + get_static_transform( + gnss_frame, base_frame_, tf_gnss_antenna2base_link_msg_ptr, nav_sat_fix_msg_ptr->header.stamp); + tf2::Transform tf_gnss_antenna2base_link{}; + tf2::fromMsg(tf_gnss_antenna2base_link_msg_ptr->transform, tf_gnss_antenna2base_link); + + // transform pose from gnss_antenna(in map frame) to base_link(in map frame) + tf2::Transform tf_map2base_link{}; + tf_map2base_link = tf_map2gnss_antenna * tf_gnss_antenna2base_link; + + geometry_msgs::msg::PoseStamped gnss_base_pose_msg{}; + gnss_base_pose_msg.header.stamp = nav_sat_fix_msg_ptr->header.stamp; + gnss_base_pose_msg.header.frame_id = map_frame_; + tf2::toMsg(tf_map2base_link, gnss_base_pose_msg.pose); + + // publish gnss_base_link pose in map frame + pose_pub_->publish(gnss_base_pose_msg); + + // publish gnss_base_link pose_cov in map frame + geometry_msgs::msg::PoseWithCovarianceStamped gnss_base_pose_cov_msg; + gnss_base_pose_cov_msg.header = gnss_base_pose_msg.header; + gnss_base_pose_cov_msg.pose.pose = gnss_base_pose_msg.pose; + gnss_base_pose_cov_msg.pose.covariance[7 * 0] = + can_get_covariance(*nav_sat_fix_msg_ptr) ? nav_sat_fix_msg_ptr->position_covariance[0] : 10.0; + gnss_base_pose_cov_msg.pose.covariance[7 * 1] = + can_get_covariance(*nav_sat_fix_msg_ptr) ? nav_sat_fix_msg_ptr->position_covariance[4] : 10.0; + gnss_base_pose_cov_msg.pose.covariance[7 * 2] = + can_get_covariance(*nav_sat_fix_msg_ptr) ? nav_sat_fix_msg_ptr->position_covariance[8] : 10.0; + + if (use_gnss_ins_orientation_) { + gnss_base_pose_cov_msg.pose.covariance[7 * 3] = + std::pow(msg_gnss_ins_orientation_stamped_->orientation.rmse_rotation_x, 2); + gnss_base_pose_cov_msg.pose.covariance[7 * 4] = + std::pow(msg_gnss_ins_orientation_stamped_->orientation.rmse_rotation_y, 2); + gnss_base_pose_cov_msg.pose.covariance[7 * 5] = + std::pow(msg_gnss_ins_orientation_stamped_->orientation.rmse_rotation_z, 2); + } else { + gnss_base_pose_cov_msg.pose.covariance[7 * 3] = 0.1; + gnss_base_pose_cov_msg.pose.covariance[7 * 4] = 0.1; + gnss_base_pose_cov_msg.pose.covariance[7 * 5] = 1.0; + } + + pose_cov_pub_->publish(gnss_base_pose_cov_msg); + + // broadcast map to gnss_base_link + publish_tf(map_frame_, gnss_base_frame_, gnss_base_pose_msg); +} + +void GNSSPoser::callback_gnss_ins_orientation_stamped( + const autoware_sensing_msgs::msg::GnssInsOrientationStamped::ConstSharedPtr msg) +{ + *msg_gnss_ins_orientation_stamped_ = *msg; +} + +bool GNSSPoser::is_fixed(const sensor_msgs::msg::NavSatStatus & nav_sat_status_msg) +{ + return nav_sat_status_msg.status >= sensor_msgs::msg::NavSatStatus::STATUS_FIX; +} + +bool GNSSPoser::can_get_covariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg) +{ + return nav_sat_fix_msg.position_covariance_type > + sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN; +} + +geometry_msgs::msg::Point GNSSPoser::get_median_position( + const boost::circular_buffer & position_buffer) +{ + auto get_median = [](std::vector array) { + std::sort(std::begin(array), std::end(array)); + const size_t median_index = array.size() / 2; + double median = (array.size() % 2) + ? (array.at(median_index)) + : ((array.at(median_index) + array.at(median_index - 1)) / 2); + return median; + }; + + std::vector array_x; + std::vector array_y; + std::vector array_z; + for (const auto & position : position_buffer) { + array_x.push_back(position.x); + array_y.push_back(position.y); + array_z.push_back(position.z); + } + + geometry_msgs::msg::Point median_point; + median_point.x = get_median(array_x); + median_point.y = get_median(array_y); + median_point.z = get_median(array_z); + return median_point; +} + +geometry_msgs::msg::Point GNSSPoser::get_average_position( + const boost::circular_buffer & position_buffer) +{ + std::vector array_x; + std::vector array_y; + std::vector array_z; + for (const auto & position : position_buffer) { + array_x.push_back(position.x); + array_y.push_back(position.y); + array_z.push_back(position.z); + } + + geometry_msgs::msg::Point average_point; + average_point.x = + std::reduce(array_x.begin(), array_x.end()) / static_cast(array_x.size()); + average_point.y = + std::reduce(array_y.begin(), array_y.end()) / static_cast(array_y.size()); + average_point.z = + std::reduce(array_z.begin(), array_z.end()) / static_cast(array_z.size()); + return average_point; +} + +geometry_msgs::msg::Quaternion GNSSPoser::get_quaternion_by_heading(const int heading) +{ + int heading_conv = 0; + // convert heading[0(North)~360] to yaw[-M_PI(West)~M_PI] + if (heading >= 0 && heading <= 27000000) { + heading_conv = 9000000 - heading; + } else { + heading_conv = 45000000 - heading; + } + const double yaw = (heading_conv * 1e-5) * M_PI / 180.0; + + tf2::Quaternion quaternion; + quaternion.setRPY(0, 0, yaw); + + return tf2::toMsg(quaternion); +} + +geometry_msgs::msg::Quaternion GNSSPoser::get_quaternion_by_position_difference( + const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Point & prev_point) +{ + const double yaw = std::atan2(point.y - prev_point.y, point.x - prev_point.x); + tf2::Quaternion quaternion; + quaternion.setRPY(0, 0, yaw); + return tf2::toMsg(quaternion); +} + +bool GNSSPoser::get_transform( + const std::string & target_frame, const std::string & source_frame, + const geometry_msgs::msg::TransformStamped::SharedPtr transform_stamped_ptr) +{ + if (target_frame == source_frame) { + transform_stamped_ptr->header.stamp = this->now(); + transform_stamped_ptr->header.frame_id = target_frame; + transform_stamped_ptr->child_frame_id = source_frame; + transform_stamped_ptr->transform.translation.x = 0.0; + transform_stamped_ptr->transform.translation.y = 0.0; + transform_stamped_ptr->transform.translation.z = 0.0; + transform_stamped_ptr->transform.rotation.x = 0.0; + transform_stamped_ptr->transform.rotation.y = 0.0; + transform_stamped_ptr->transform.rotation.z = 0.0; + transform_stamped_ptr->transform.rotation.w = 1.0; + return true; + } + + try { + *transform_stamped_ptr = + tf2_buffer_.lookupTransform(target_frame, source_frame, tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), ex.what()); + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), + "Please publish TF " << target_frame.c_str() << " to " << source_frame.c_str()); + + transform_stamped_ptr->header.stamp = this->now(); + transform_stamped_ptr->header.frame_id = target_frame; + transform_stamped_ptr->child_frame_id = source_frame; + transform_stamped_ptr->transform.translation.x = 0.0; + transform_stamped_ptr->transform.translation.y = 0.0; + transform_stamped_ptr->transform.translation.z = 0.0; + transform_stamped_ptr->transform.rotation.x = 0.0; + transform_stamped_ptr->transform.rotation.y = 0.0; + transform_stamped_ptr->transform.rotation.z = 0.0; + transform_stamped_ptr->transform.rotation.w = 1.0; + return false; + } + return true; +} + +bool GNSSPoser::get_static_transform( + const std::string & target_frame, const std::string & source_frame, + const geometry_msgs::msg::TransformStamped::SharedPtr transform_stamped_ptr, + const builtin_interfaces::msg::Time & stamp) +{ + if (target_frame == source_frame) { + transform_stamped_ptr->header.stamp = stamp; + transform_stamped_ptr->header.frame_id = target_frame; + transform_stamped_ptr->child_frame_id = source_frame; + transform_stamped_ptr->transform.translation.x = 0.0; + transform_stamped_ptr->transform.translation.y = 0.0; + transform_stamped_ptr->transform.translation.z = 0.0; + transform_stamped_ptr->transform.rotation.x = 0.0; + transform_stamped_ptr->transform.rotation.y = 0.0; + transform_stamped_ptr->transform.rotation.z = 0.0; + transform_stamped_ptr->transform.rotation.w = 1.0; + return true; + } + + try { + *transform_stamped_ptr = tf2_buffer_.lookupTransform( + target_frame, source_frame, + tf2::TimePoint(std::chrono::seconds(stamp.sec) + std::chrono::nanoseconds(stamp.nanosec))); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), ex.what()); + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), + "Please publish TF " << target_frame.c_str() << " to " << source_frame.c_str()); + + transform_stamped_ptr->header.stamp = stamp; + transform_stamped_ptr->header.frame_id = target_frame; + transform_stamped_ptr->child_frame_id = source_frame; + transform_stamped_ptr->transform.translation.x = 0.0; + transform_stamped_ptr->transform.translation.y = 0.0; + transform_stamped_ptr->transform.translation.z = 0.0; + transform_stamped_ptr->transform.rotation.x = 0.0; + transform_stamped_ptr->transform.rotation.y = 0.0; + transform_stamped_ptr->transform.rotation.z = 0.0; + transform_stamped_ptr->transform.rotation.w = 1.0; + return false; + } + return true; +} + +void GNSSPoser::publish_tf( + const std::string & frame_id, const std::string & child_frame_id, + const geometry_msgs::msg::PoseStamped & pose_msg) +{ + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped.header.frame_id = frame_id; + transform_stamped.child_frame_id = child_frame_id; + transform_stamped.header.stamp = pose_msg.header.stamp; + + transform_stamped.transform.translation.x = pose_msg.pose.position.x; + transform_stamped.transform.translation.y = pose_msg.pose.position.y; + transform_stamped.transform.translation.z = pose_msg.pose.position.z; + + tf2::Quaternion tf_quaternion; + tf2::fromMsg(pose_msg.pose.orientation, tf_quaternion); + transform_stamped.transform.rotation.x = tf_quaternion.x(); + transform_stamped.transform.rotation.y = tf_quaternion.y(); + transform_stamped.transform.rotation.z = tf_quaternion.z(); + transform_stamped.transform.rotation.w = tf_quaternion.w(); + + tf2_broadcaster_.sendTransform(transform_stamped); +} +} // namespace autoware::gnss_poser + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::gnss_poser::GNSSPoser) From 69946285e56e9be222659b4be938d10927c5bdd0 Mon Sep 17 00:00:00 2001 From: Yukinari Hisaki <42021302+yhisaki@users.noreply.github.com> Date: Wed, 19 Feb 2025 21:12:47 +0900 Subject: [PATCH 2/8] feat(autoware_trajectory): add curvature_utils (#205) Signed-off-by: Y.Hisaki --- .../examples/example_trajectory_point.cpp | 5 +++ .../trajectory/utils/curvature_utils.hpp | 37 +++++++++++++++++++ .../test/test_trajectory_container.cpp | 7 ++++ 3 files changed, 49 insertions(+) create mode 100644 common/autoware_trajectory/include/autoware/trajectory/utils/curvature_utils.hpp diff --git a/common/autoware_trajectory/examples/example_trajectory_point.cpp b/common/autoware_trajectory/examples/example_trajectory_point.cpp index 3aead35108..fa0792f875 100644 --- a/common/autoware_trajectory/examples/example_trajectory_point.cpp +++ b/common/autoware_trajectory/examples/example_trajectory_point.cpp @@ -16,6 +16,7 @@ #include "autoware/trajectory/point.hpp" #include "autoware/trajectory/utils/closest.hpp" #include "autoware/trajectory/utils/crossed.hpp" +#include "autoware/trajectory/utils/curvature_utils.hpp" #include "lanelet2_core/primitives/LineString.h" #include @@ -133,6 +134,10 @@ int main() } plt.scatter(Args(x, y), Kwargs("label"_a = "Restored", "color"_a = "orange", "marker"_a = "x")); } + { + auto max_curvature = autoware::trajectory::max_curvature(*trajectory); + std::cout << "Max curvature: " << max_curvature << std::endl; + } plt.axis(Args("equal")); plt.legend(); diff --git a/common/autoware_trajectory/include/autoware/trajectory/utils/curvature_utils.hpp b/common/autoware_trajectory/include/autoware/trajectory/utils/curvature_utils.hpp new file mode 100644 index 0000000000..2845cf6339 --- /dev/null +++ b/common/autoware_trajectory/include/autoware/trajectory/utils/curvature_utils.hpp @@ -0,0 +1,37 @@ +// Copyright 2025 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__TRAJECTORY__UTILS__CURVATURE_UTILS_HPP_ +#define AUTOWARE__TRAJECTORY__UTILS__CURVATURE_UTILS_HPP_ + +#include "autoware/trajectory/forward.hpp" + +#include + +namespace autoware::trajectory +{ +template +double max_curvature(const Trajectory & trajectory) +{ + double max_curvature = 0.0; + for (const auto & base : trajectory.get_internal_bases()) { + const auto curvature = trajectory.curvature(base); + max_curvature = std::max(max_curvature, curvature); + } + return max_curvature; +} + +} // namespace autoware::trajectory + +#endif // AUTOWARE__TRAJECTORY__UTILS__CURVATURE_UTILS_HPP_ diff --git a/common/autoware_trajectory/test/test_trajectory_container.cpp b/common/autoware_trajectory/test/test_trajectory_container.cpp index bdbf5eb60f..ce7962a525 100644 --- a/common/autoware_trajectory/test/test_trajectory_container.cpp +++ b/common/autoware_trajectory/test/test_trajectory_container.cpp @@ -14,6 +14,7 @@ #include "autoware/trajectory/path_point_with_lane_id.hpp" #include "autoware/trajectory/utils/closest.hpp" #include "autoware/trajectory/utils/crossed.hpp" +#include "autoware/trajectory/utils/curvature_utils.hpp" #include "autoware/trajectory/utils/find_intervals.hpp" #include "lanelet2_core/primitives/LineString.h" @@ -187,3 +188,9 @@ TEST_F(TrajectoryTest, find_interval) EXPECT_LT(intervals[0].start, intervals[0].end); EXPECT_NEAR(intervals[0].end, trajectory->length(), 0.1); } + +TEST_F(TrajectoryTest, max_curvature) +{ + double max_curvature = autoware::trajectory::max_curvature(*trajectory); + EXPECT_LT(0, max_curvature); +} From 7618ecf33ae8e11c8413c220e0b6556e5b5310ba Mon Sep 17 00:00:00 2001 From: Yukinari Hisaki <42021302+yhisaki@users.noreply.github.com> Date: Wed, 19 Feb 2025 21:13:05 +0900 Subject: [PATCH 3/8] chore(autoware_trajectory): resolve clang-tidy warning of example file (#206) Signed-off-by: Y.Hisaki --- common/autoware_trajectory/examples/example_interpolator.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/common/autoware_trajectory/examples/example_interpolator.cpp b/common/autoware_trajectory/examples/example_interpolator.cpp index f34d291377..ddc6fb0a51 100644 --- a/common/autoware_trajectory/examples/example_interpolator.cpp +++ b/common/autoware_trajectory/examples/example_interpolator.cpp @@ -35,6 +35,7 @@ int main() std::random_device seed_gen; std::mt19937 engine(seed_gen()); std::uniform_real_distribution<> dist(-1.0, 1.0); + values.reserve(bases.size()); for (size_t i = 0; i < bases.size(); ++i) { values.push_back(dist(engine)); } From 1b80891535a1af3ee8036ad2829f5d8ab0686a6c Mon Sep 17 00:00:00 2001 From: Yukinari Hisaki <42021302+yhisaki@users.noreply.github.com> Date: Wed, 19 Feb 2025 21:13:15 +0900 Subject: [PATCH 4/8] fix(autoware_trajectory): fix a bug of example file (#204) Signed-off-by: Y.Hisaki --- .../examples/example_find_intervals.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/common/autoware_trajectory/examples/example_find_intervals.cpp b/common/autoware_trajectory/examples/example_find_intervals.cpp index 69d0ed2a38..dd093edd71 100644 --- a/common/autoware_trajectory/examples/example_find_intervals.cpp +++ b/common/autoware_trajectory/examples/example_find_intervals.cpp @@ -22,10 +22,10 @@ using Trajectory = autoware::trajectory::Trajectory; -autoware_internal::msg::PathPointWithLaneId path_point_with_lane_id( +autoware_internal_planning_msgs::msg::PathPointWithLaneId path_point_with_lane_id( double x, double y, uint8_t lane_id) { - autoware_internal::msg::PathPointWithLaneId point; + autoware_internal_planning_msgs::msg::PathPointWithLaneId point; point.point.pose.position.x = x; point.point.pose.position.y = y; point.lane_ids.emplace_back(lane_id); @@ -37,7 +37,7 @@ int main() pybind11::scoped_interpreter guard{}; auto plt = matplotlibcpp17::pyplot::import(); - std::vector points{ + std::vector points{ path_point_with_lane_id(0.41, 0.69, 0), path_point_with_lane_id(0.66, 1.09, 0), path_point_with_lane_id(0.93, 1.41, 0), path_point_with_lane_id(1.26, 1.71, 0), path_point_with_lane_id(1.62, 1.90, 0), path_point_with_lane_id(1.96, 1.98, 0), @@ -56,7 +56,7 @@ int main() } const auto intervals = autoware::trajectory::find_intervals( - *trajectory, [](const autoware_internal::msg::PathPointWithLaneId & point) { + *trajectory, [](const autoware_internal_planning_msgs::msg::PathPointWithLaneId & point) { return point.lane_ids[0] == 1; }); From 9eff11e04cfd89295d47e35ee6b37f1c474c69c7 Mon Sep 17 00:00:00 2001 From: Yukinari Hisaki <42021302+yhisaki@users.noreply.github.com> Date: Wed, 19 Feb 2025 21:14:35 +0900 Subject: [PATCH 5/8] feat(autoware_trajecotry): add a conversion function from point trajectory to pose trajectory (#207) feat(autoware_trajecotry): add conversion function from point trajectory to pose trajectory Signed-off-by: Y.Hisaki --- .../include/autoware/trajectory/point.hpp | 3 +++ .../include/autoware/trajectory/pose.hpp | 3 +++ common/autoware_trajectory/src/pose.cpp | 27 +++++++++++++++++++ 3 files changed, 33 insertions(+) diff --git a/common/autoware_trajectory/include/autoware/trajectory/point.hpp b/common/autoware_trajectory/include/autoware/trajectory/point.hpp index af47c404f7..cc69ffc2ae 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/point.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/point.hpp @@ -36,6 +36,9 @@ namespace autoware::trajectory template <> class Trajectory { + template + friend class Trajectory; + using PointType = geometry_msgs::msg::Point; protected: diff --git a/common/autoware_trajectory/include/autoware/trajectory/pose.hpp b/common/autoware_trajectory/include/autoware/trajectory/pose.hpp index 911db11bf8..a7030e352b 100644 --- a/common/autoware_trajectory/include/autoware/trajectory/pose.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/pose.hpp @@ -49,6 +49,9 @@ class Trajectory : public Trajectory & point_trajectory); + bool build(const std::vector & points); /** diff --git a/common/autoware_trajectory/src/pose.cpp b/common/autoware_trajectory/src/pose.cpp index 0dc3287769..37dca2f24a 100644 --- a/common/autoware_trajectory/src/pose.cpp +++ b/common/autoware_trajectory/src/pose.cpp @@ -15,6 +15,7 @@ #include "autoware/trajectory/pose.hpp" #include "autoware/trajectory/detail/helpers.hpp" +#include "autoware/trajectory/forward.hpp" #include "autoware/trajectory/interpolator/spherical_linear.hpp" #include @@ -45,6 +46,32 @@ Trajectory & Trajectory::operator=(const Trajectory & rhs) return *this; } +Trajectory::Trajectory(const Trajectory & point_trajectory) +: Trajectory() +{ + x_interpolator_ = point_trajectory.x_interpolator_->clone(); + y_interpolator_ = point_trajectory.y_interpolator_->clone(); + z_interpolator_ = point_trajectory.z_interpolator_->clone(); + bases_ = point_trajectory.get_internal_bases(); + start_ = point_trajectory.start_; + end_ = point_trajectory.end_; + + // build mock orientations + std::vector orientations(bases_.size()); + for (size_t i = 0; i < bases_.size(); ++i) { + orientations[i].w = 1.0; + } + bool success = orientation_interpolator_->build(bases_, orientations); + + if (!success) { + throw std::runtime_error( + "Failed to build orientation interpolator."); // This Exception should not be thrown. + } + + // align orientation with trajectory direction + align_orientation_with_trajectory_direction(); +} + bool Trajectory::build(const std::vector & points) { std::vector path_points; From 68e35fe5219f432bec6e7a3155d60a57bfc02ef1 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Thu, 20 Feb 2025 13:14:12 +0900 Subject: [PATCH 6/8] feat: port autoware_pyplot from Autoware Universe to Autoware Core (#199) Signed-off-by: mitsudome-r --- testing/autoware_pyplot/CMakeLists.txt | 36 +++ testing/autoware_pyplot/README.md | 43 ++++ .../include/autoware/pyplot/axes.hpp | 141 ++++++++++++ .../include/autoware/pyplot/common.hpp | 44 ++++ .../include/autoware/pyplot/figure.hpp | 62 ++++++ .../include/autoware/pyplot/legend.hpp | 32 +++ .../include/autoware/pyplot/loader.hpp | 28 +++ .../include/autoware/pyplot/patches.hpp | 57 +++++ .../include/autoware/pyplot/pyplot.hpp | 164 ++++++++++++++ .../include/autoware/pyplot/quiver.hpp | 32 +++ .../include/autoware/pyplot/text.hpp | 40 ++++ testing/autoware_pyplot/package.xml | 27 +++ testing/autoware_pyplot/src/axes.cpp | 156 +++++++++++++ testing/autoware_pyplot/src/common.cpp | 32 +++ testing/autoware_pyplot/src/figure.cpp | 66 ++++++ testing/autoware_pyplot/src/legend.cpp | 28 +++ testing/autoware_pyplot/src/patches.cpp | 45 ++++ testing/autoware_pyplot/src/pyplot.cpp | 208 ++++++++++++++++++ testing/autoware_pyplot/src/quiver.cpp | 28 +++ testing/autoware_pyplot/src/text.cpp | 35 +++ testing/autoware_pyplot/test/test_pyplot.cpp | 64 ++++++ 21 files changed, 1368 insertions(+) create mode 100644 testing/autoware_pyplot/CMakeLists.txt create mode 100644 testing/autoware_pyplot/README.md create mode 100644 testing/autoware_pyplot/include/autoware/pyplot/axes.hpp create mode 100644 testing/autoware_pyplot/include/autoware/pyplot/common.hpp create mode 100644 testing/autoware_pyplot/include/autoware/pyplot/figure.hpp create mode 100644 testing/autoware_pyplot/include/autoware/pyplot/legend.hpp create mode 100644 testing/autoware_pyplot/include/autoware/pyplot/loader.hpp create mode 100644 testing/autoware_pyplot/include/autoware/pyplot/patches.hpp create mode 100644 testing/autoware_pyplot/include/autoware/pyplot/pyplot.hpp create mode 100644 testing/autoware_pyplot/include/autoware/pyplot/quiver.hpp create mode 100644 testing/autoware_pyplot/include/autoware/pyplot/text.hpp create mode 100644 testing/autoware_pyplot/package.xml create mode 100644 testing/autoware_pyplot/src/axes.cpp create mode 100644 testing/autoware_pyplot/src/common.cpp create mode 100644 testing/autoware_pyplot/src/figure.cpp create mode 100644 testing/autoware_pyplot/src/legend.cpp create mode 100644 testing/autoware_pyplot/src/patches.cpp create mode 100644 testing/autoware_pyplot/src/pyplot.cpp create mode 100644 testing/autoware_pyplot/src/quiver.cpp create mode 100644 testing/autoware_pyplot/src/text.cpp create mode 100644 testing/autoware_pyplot/test/test_pyplot.cpp diff --git a/testing/autoware_pyplot/CMakeLists.txt b/testing/autoware_pyplot/CMakeLists.txt new file mode 100644 index 0000000000..5871929676 --- /dev/null +++ b/testing/autoware_pyplot/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 3.14) + +project(autoware_pyplot) + +find_package(autoware_cmake REQUIRED) + +find_package( + Python3 + COMPONENTS Interpreter Development + REQUIRED) +find_package(pybind11 REQUIRED) + +autoware_package() + +ament_auto_add_library(${PROJECT_NAME} STATIC + DIRECTORY src +) +target_compile_options(${PROJECT_NAME} PUBLIC "-fPIC") +target_link_libraries(${PROJECT_NAME} ${Python3_LIBRARIES} pybind11::embed) + +# NOTE(soblin): this is workaround for propagating the include of "Python.h" to user modules to avoid "'Python.h' not found" +ament_export_include_directories(${Python3_INCLUDE_DIRS}) + +if(BUILD_TESTING) + find_package(ament_cmake_ros REQUIRED) + + file(GLOB_RECURSE test_files test/*.cpp) + + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} ${test_files}) + + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) +endif() + +ament_auto_package() diff --git a/testing/autoware_pyplot/README.md b/testing/autoware_pyplot/README.md new file mode 100644 index 0000000000..ded3387fef --- /dev/null +++ b/testing/autoware_pyplot/README.md @@ -0,0 +1,43 @@ +# autoware_pyplot + +This package provides C++ interface for the notable `matplotlib` using `pybind11` backend for + +- creating scientific plots and images illustrating the function inputs/outputs +- debugging the output and internal data of a function before unit testing in a more lightweight manner than planning_simulator + +## usage + +In your main function, setup the python context and import `matplotlib` + +```cpp +#include +#include + +// in main... + py::scoped_interpreter guard{}; + auto plt = autoware::pyplot::import(); +``` + +Then you can use major functionalities of `matplotlib` almost in the same way as native python code. + +```cpp +{ + plt.plot(Args(std::vector({1, 3, 2, 4})), Kwargs("color"_a = "blue", "linewidth"_a = 1.0)); + plt.xlabel(Args("x-title")); + plt.ylabel(Args("y-title")); + plt.title(Args("title")); + plt.xlim(Args(0, 5)); + plt.ylim(Args(0, 5)); + plt.grid(Args(true)); + plt.savefig(Args("test_single_plot.png")); +} + +{ + auto [fig, axes] = plt.subplots(1, 2); + auto & ax1 = axes[0]; + auto & ax2 = axes[1]; + + ax1.set_aspect(Args("equal")); + ax2.set_aspect(Args("equal")); +} +``` diff --git a/testing/autoware_pyplot/include/autoware/pyplot/axes.hpp b/testing/autoware_pyplot/include/autoware/pyplot/axes.hpp new file mode 100644 index 0000000000..53eb2daf21 --- /dev/null +++ b/testing/autoware_pyplot/include/autoware/pyplot/axes.hpp @@ -0,0 +1,141 @@ +// Copyright 2024 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__PYPLOT__AXES_HPP_ +#define AUTOWARE__PYPLOT__AXES_HPP_ + +#include +#include +#include + +#include + +namespace autoware::pyplot +{ +inline namespace axes +{ +class DECL_VISIBILITY Axes : public PyObjectWrapper +{ +public: + explicit Axes(const pybind11::object & object); + explicit Axes(pybind11::object && object); + + PyObjectWrapper add_patch( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()) const; + + PyObjectWrapper bar_label( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()) const; + + PyObjectWrapper cla( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()) const; + + PyObjectWrapper contour( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()) const; + + PyObjectWrapper errorbar( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()) const; + + PyObjectWrapper fill( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()) const; + + PyObjectWrapper fill_between( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()) const; + + std::tuple get_xlim() const; + + std::tuple get_ylim() const; + + PyObjectWrapper grid( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()) const; + + PyObjectWrapper imshow( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()) const; + + legend::Legend legend( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()) const; + + PyObjectWrapper plot( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()) const; + + quiver::Quiver quiver( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()) const; + + PyObjectWrapper set_aspect( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()) const; + + PyObjectWrapper set_title( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()) const; + + PyObjectWrapper set_xlabel( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()) const; + + PyObjectWrapper set_xlim( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()) const; + + PyObjectWrapper set_ylabel( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()) const; + + PyObjectWrapper set_ylim( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()) const; + + PyObjectWrapper text( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()) const; + +private: + void load_attrs(); + + pybind11::object add_patch_attr; + pybind11::object cla_attr; + pybind11::object contour_attr; + pybind11::object contourf_attr; + pybind11::object fill_attr; + pybind11::object fill_between_attr; + pybind11::object get_xlim_attr; + pybind11::object get_ylim_attr; + pybind11::object grid_attr; + pybind11::object imshow_attr; + pybind11::object legend_attr; + pybind11::object quiver_attr; + pybind11::object plot_attr; + pybind11::object scatter_attr; + pybind11::object set_aspect_attr; + pybind11::object set_title_attr; + pybind11::object set_xlabel_attr; + pybind11::object set_xlim_attr; + pybind11::object set_ylabel_attr; + pybind11::object set_ylim_attr; + pybind11::object text_attr; +}; +} // namespace axes +} // namespace autoware::pyplot +#endif // AUTOWARE__PYPLOT__AXES_HPP_ diff --git a/testing/autoware_pyplot/include/autoware/pyplot/common.hpp b/testing/autoware_pyplot/include/autoware/pyplot/common.hpp new file mode 100644 index 0000000000..123bd82029 --- /dev/null +++ b/testing/autoware_pyplot/include/autoware/pyplot/common.hpp @@ -0,0 +1,44 @@ +// Copyright 2024 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__PYPLOT__COMMON_HPP_ +#define AUTOWARE__PYPLOT__COMMON_HPP_ + +#include + +namespace autoware::pyplot +{ + +#ifdef _WIN32 +#define DECL_VISIBILITY __declspec(dllexport) +#else +#define DECL_VISIBILITY __attribute__((visibility("hidden"))) +#endif + +inline namespace common +{ +class DECL_VISIBILITY PyObjectWrapper +{ +public: + explicit PyObjectWrapper(const pybind11::object & object); + explicit PyObjectWrapper(pybind11::object && object); + pybind11::object unwrap() const { return self_; } + +protected: + pybind11::object self_; +}; +} // namespace common +} // namespace autoware::pyplot + +#endif // AUTOWARE__PYPLOT__COMMON_HPP_ diff --git a/testing/autoware_pyplot/include/autoware/pyplot/figure.hpp b/testing/autoware_pyplot/include/autoware/pyplot/figure.hpp new file mode 100644 index 0000000000..d438682d5b --- /dev/null +++ b/testing/autoware_pyplot/include/autoware/pyplot/figure.hpp @@ -0,0 +1,62 @@ +// Copyright 2024 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__PYPLOT__FIGURE_HPP_ +#define AUTOWARE__PYPLOT__FIGURE_HPP_ + +#include +#include + +namespace autoware::pyplot +{ +inline namespace figure +{ +class DECL_VISIBILITY Figure : public PyObjectWrapper +{ +public: + explicit Figure(const pybind11::object & object); + explicit Figure(pybind11::object && object); + + axes::Axes add_axes( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()) const; + + axes::Axes add_subplot( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()) const; + + PyObjectWrapper colorbar( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()) const; + + PyObjectWrapper savefig( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()) const; + + PyObjectWrapper tight_layout( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()) const; + +private: + void load_attrs(); + + pybind11::object add_axes_attr; + pybind11::object add_subplot_attr; + pybind11::object colorbar_attr; + pybind11::object savefig_attr; + pybind11::object tight_layout_attr; +}; +} // namespace figure +} // namespace autoware::pyplot +#endif // AUTOWARE__PYPLOT__FIGURE_HPP_ diff --git a/testing/autoware_pyplot/include/autoware/pyplot/legend.hpp b/testing/autoware_pyplot/include/autoware/pyplot/legend.hpp new file mode 100644 index 0000000000..853f1e2884 --- /dev/null +++ b/testing/autoware_pyplot/include/autoware/pyplot/legend.hpp @@ -0,0 +1,32 @@ +// Copyright 2024 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__PYPLOT__LEGEND_HPP_ +#define AUTOWARE__PYPLOT__LEGEND_HPP_ + +#include + +namespace autoware::pyplot +{ +inline namespace legend +{ +class DECL_VISIBILITY Legend : public PyObjectWrapper +{ +public: + explicit Legend(const pybind11::object & object); + explicit Legend(pybind11::object && object); +}; +} // namespace legend +} // namespace autoware::pyplot +#endif // AUTOWARE__PYPLOT__LEGEND_HPP_ diff --git a/testing/autoware_pyplot/include/autoware/pyplot/loader.hpp b/testing/autoware_pyplot/include/autoware/pyplot/loader.hpp new file mode 100644 index 0000000000..df44c257d5 --- /dev/null +++ b/testing/autoware_pyplot/include/autoware/pyplot/loader.hpp @@ -0,0 +1,28 @@ +// Copyright 2024 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__PYPLOT__LOADER_HPP_ +#define AUTOWARE__PYPLOT__LOADER_HPP_ + +namespace autoware::pyplot +{ + +#define LOAD_FUNC_ATTR(obj, mod) \ + do { \ + obj##_attr = mod.attr(#obj); \ + } while (0) + +} // namespace autoware::pyplot + +#endif // AUTOWARE__PYPLOT__LOADER_HPP_ diff --git a/testing/autoware_pyplot/include/autoware/pyplot/patches.hpp b/testing/autoware_pyplot/include/autoware/pyplot/patches.hpp new file mode 100644 index 0000000000..0c6c545942 --- /dev/null +++ b/testing/autoware_pyplot/include/autoware/pyplot/patches.hpp @@ -0,0 +1,57 @@ +// Copyright 2024 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__PYPLOT__PATCHES_HPP_ +#define AUTOWARE__PYPLOT__PATCHES_HPP_ + +#include + +namespace autoware::pyplot +{ +inline namespace patches +{ +class DECL_VISIBILITY Circle : public PyObjectWrapper +{ +public: + explicit Circle( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()); +}; + +class DECL_VISIBILITY Ellipse : public PyObjectWrapper +{ +public: + explicit Ellipse( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()); +}; + +class DECL_VISIBILITY Rectangle : public PyObjectWrapper +{ +public: + explicit Rectangle( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()); +}; + +class DECL_VISIBILITY Polygon : public PyObjectWrapper +{ +public: + explicit Polygon( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()); +}; +} // namespace patches +} // namespace autoware::pyplot +#endif // AUTOWARE__PYPLOT__PATCHES_HPP_ diff --git a/testing/autoware_pyplot/include/autoware/pyplot/pyplot.hpp b/testing/autoware_pyplot/include/autoware/pyplot/pyplot.hpp new file mode 100644 index 0000000000..83e0febeb2 --- /dev/null +++ b/testing/autoware_pyplot/include/autoware/pyplot/pyplot.hpp @@ -0,0 +1,164 @@ +// Copyright 2024 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__PYPLOT__PYPLOT_HPP_ +#define AUTOWARE__PYPLOT__PYPLOT_HPP_ + +#include +#include +#include + +#include +#include +#include + +namespace autoware::pyplot +{ +struct DECL_VISIBILITY PyPlot +{ +public: + explicit PyPlot(const pybind11::module & mod_); + + axes::Axes axes(const pybind11::dict & kwargs = pybind11::dict()); + + PyObjectWrapper axis( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()); + + PyObjectWrapper cla( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()); + + PyObjectWrapper clf( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()); + + figure::Figure figure( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()); + + axes::Axes gca( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()); + + figure::Figure gcf( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()); + + PyObjectWrapper gci( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()); + + PyObjectWrapper grid( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()); + + PyObjectWrapper imshow( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()); + + PyObjectWrapper legend( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()); + + PyObjectWrapper plot( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()); + + PyObjectWrapper quiver( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()); + + PyObjectWrapper savefig( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()); + + PyObjectWrapper scatter( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()); + + PyObjectWrapper show( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()); + + axes::Axes subplot(const pybind11::dict & kwargs = pybind11::dict()); + axes::Axes subplot(int cri); + + std::tuple subplots(const pybind11::dict & kwargs = pybind11::dict()); + std::tuple> subplots( + int r, int c, const pybind11::dict & kwargs = pybind11::dict()); + + PyObjectWrapper title( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()); + + PyObjectWrapper xlabel( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()); + + PyObjectWrapper xlim( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()); + + PyObjectWrapper ylabel( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()); + + PyObjectWrapper ylim( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()); + +private: + void load_attrs(); + pybind11::module mod; + pybind11::object axes_attr; + pybind11::object cla_attr; + pybind11::object clf_attr; + pybind11::object figure_attr; + pybind11::object gca_attr; + pybind11::object gcf_attr; + pybind11::object gci_attr; + pybind11::object grid_attr; + pybind11::object imshow_attr; + pybind11::object legend_attr; + pybind11::object plot_attr; + pybind11::object quiver_attr; + pybind11::object savefig_attr; + pybind11::object scatter_attr; + pybind11::object show_attr; + pybind11::object subplot_attr; + pybind11::object subplots_attr; + pybind11::object title_attr; + pybind11::object xlabel_attr; + pybind11::object xlim_attr; + pybind11::object ylabel_attr; + pybind11::object ylim_attr; +}; + +PyPlot import(); + +} // namespace autoware::pyplot + +template +pybind11::tuple Args(ArgsT &&... args) +{ + return pybind11::make_tuple(std::forward(args)...); +} + +using Kwargs = pybind11::dict; + +namespace py = pybind11; +using namespace py::literals; + +#endif // AUTOWARE__PYPLOT__PYPLOT_HPP_ diff --git a/testing/autoware_pyplot/include/autoware/pyplot/quiver.hpp b/testing/autoware_pyplot/include/autoware/pyplot/quiver.hpp new file mode 100644 index 0000000000..443c0540d9 --- /dev/null +++ b/testing/autoware_pyplot/include/autoware/pyplot/quiver.hpp @@ -0,0 +1,32 @@ +// Copyright 2024 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__PYPLOT__QUIVER_HPP_ +#define AUTOWARE__PYPLOT__QUIVER_HPP_ + +#include + +namespace autoware::pyplot +{ +inline namespace quiver +{ +class DECL_VISIBILITY Quiver : public PyObjectWrapper +{ +public: + explicit Quiver(const pybind11::object & object); + explicit Quiver(pybind11::object && object); +}; +} // namespace quiver +} // namespace autoware::pyplot +#endif // AUTOWARE__PYPLOT__QUIVER_HPP_ diff --git a/testing/autoware_pyplot/include/autoware/pyplot/text.hpp b/testing/autoware_pyplot/include/autoware/pyplot/text.hpp new file mode 100644 index 0000000000..27f892e904 --- /dev/null +++ b/testing/autoware_pyplot/include/autoware/pyplot/text.hpp @@ -0,0 +1,40 @@ +// Copyright 2024 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__PYPLOT__TEXT_HPP_ +#define AUTOWARE__PYPLOT__TEXT_HPP_ + +#include + +namespace autoware::pyplot +{ +inline namespace text +{ +class DECL_VISIBILITY Text : public PyObjectWrapper +{ +public: + explicit Text(const pybind11::object & object); + explicit Text(pybind11::object && object); + + PyObjectWrapper set_rotation( + const pybind11::tuple & args = pybind11::tuple(), + const pybind11::dict & kwargs = pybind11::dict()) const; + +private: + void load_attrs(); + pybind11::object set_rotation_attr; +}; +} // namespace text +} // namespace autoware::pyplot +#endif // AUTOWARE__PYPLOT__TEXT_HPP_ diff --git a/testing/autoware_pyplot/package.xml b/testing/autoware_pyplot/package.xml new file mode 100644 index 0000000000..e2a5b26efe --- /dev/null +++ b/testing/autoware_pyplot/package.xml @@ -0,0 +1,27 @@ + + + + autoware_pyplot + 0.0.0 + C++ interface for matplotlib based on pybind11 + Mamoru Sobue + Yukinari Hisaki + Apache License 2.0 + + Mamoru Sobue + + ament_cmake_auto + autoware_cmake + + pybind11-dev + python3-dev + python3-matplotlib + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/testing/autoware_pyplot/src/axes.cpp b/testing/autoware_pyplot/src/axes.cpp new file mode 100644 index 0000000000..95f871ea61 --- /dev/null +++ b/testing/autoware_pyplot/src/axes.cpp @@ -0,0 +1,156 @@ +// Copyright 2024 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 + +namespace autoware::pyplot +{ +inline namespace axes +{ +Axes::Axes(const pybind11::object & object) : PyObjectWrapper(object) +{ + load_attrs(); +} +Axes::Axes(pybind11::object && object) : PyObjectWrapper(object) +{ + load_attrs(); +} + +PyObjectWrapper Axes::add_patch(const pybind11::tuple & args, const pybind11::dict & kwargs) const +{ + return PyObjectWrapper{add_patch_attr(*args, **kwargs)}; +} + +PyObjectWrapper Axes::cla(const pybind11::tuple & args, const pybind11::dict & kwargs) const +{ + return PyObjectWrapper{cla_attr(*args, **kwargs)}; +} + +PyObjectWrapper Axes::contour(const pybind11::tuple & args, const pybind11::dict & kwargs) const +{ + return PyObjectWrapper{contour_attr(*args, **kwargs)}; +} + +PyObjectWrapper Axes::fill(const pybind11::tuple & args, const pybind11::dict & kwargs) const +{ + return PyObjectWrapper{fill_attr(*args, **kwargs)}; +} + +PyObjectWrapper Axes::fill_between( + const pybind11::tuple & args, const pybind11::dict & kwargs) const +{ + return PyObjectWrapper{fill_between_attr(*args, **kwargs)}; +} + +std::tuple Axes::get_xlim() const +{ + const pybind11::list ret = get_xlim_attr(); + return {ret[0].cast(), ret[1].cast()}; +} + +std::tuple Axes::get_ylim() const +{ + const pybind11::list ret = get_ylim_attr(); + return {ret[0].cast(), ret[1].cast()}; +} + +PyObjectWrapper Axes::grid(const pybind11::tuple & args, const pybind11::dict & kwargs) const +{ + return PyObjectWrapper{grid_attr(*args, **kwargs)}; +} + +PyObjectWrapper Axes::imshow(const pybind11::tuple & args, const pybind11::dict & kwargs) const +{ + return PyObjectWrapper{imshow_attr(*args, **kwargs)}; +} + +legend::Legend Axes::legend(const pybind11::tuple & args, const pybind11::dict & kwargs) const +{ + return legend::Legend{legend_attr(*args, **kwargs)}; +} + +PyObjectWrapper Axes::plot(const pybind11::tuple & args, const pybind11::dict & kwargs) const +{ + return PyObjectWrapper{plot_attr(*args, **kwargs)}; +} + +quiver::Quiver Axes::quiver(const pybind11::tuple & args, const pybind11::dict & kwargs) const +{ + return Quiver{quiver_attr(*args, **kwargs)}; +} + +PyObjectWrapper Axes::set_aspect(const pybind11::tuple & args, const pybind11::dict & kwargs) const +{ + return PyObjectWrapper{set_aspect_attr(*args, **kwargs)}; +} + +PyObjectWrapper Axes::set_title(const pybind11::tuple & args, const pybind11::dict & kwargs) const +{ + return PyObjectWrapper{set_title_attr(*args, **kwargs)}; +} + +PyObjectWrapper Axes::set_xlabel(const pybind11::tuple & args, const pybind11::dict & kwargs) const +{ + return PyObjectWrapper{set_xlabel_attr(*args, **kwargs)}; +} + +PyObjectWrapper Axes::set_xlim(const pybind11::tuple & args, const pybind11::dict & kwargs) const +{ + return PyObjectWrapper{set_xlim_attr(*args, **kwargs)}; +} + +PyObjectWrapper Axes::set_ylabel(const pybind11::tuple & args, const pybind11::dict & kwargs) const +{ + return PyObjectWrapper{set_ylabel_attr(*args, **kwargs)}; +} + +PyObjectWrapper Axes::set_ylim(const pybind11::tuple & args, const pybind11::dict & kwargs) const +{ + return PyObjectWrapper{set_ylim_attr(*args, **kwargs)}; +} + +PyObjectWrapper Axes::text(const pybind11::tuple & args, const pybind11::dict & kwargs) const +{ + return PyObjectWrapper{text_attr(*args, **kwargs)}; +} + +void Axes::load_attrs() +{ + LOAD_FUNC_ATTR(add_patch, self_); + LOAD_FUNC_ATTR(cla, self_); + LOAD_FUNC_ATTR(contour, self_); + LOAD_FUNC_ATTR(contourf, self_); + LOAD_FUNC_ATTR(fill, self_); + LOAD_FUNC_ATTR(fill_between, self_); + LOAD_FUNC_ATTR(get_xlim, self_); + LOAD_FUNC_ATTR(get_ylim, self_); + LOAD_FUNC_ATTR(grid, self_); + LOAD_FUNC_ATTR(imshow, self_); + LOAD_FUNC_ATTR(legend, self_); + LOAD_FUNC_ATTR(quiver, self_); + LOAD_FUNC_ATTR(plot, self_); + LOAD_FUNC_ATTR(scatter, self_); + LOAD_FUNC_ATTR(set_aspect, self_); + LOAD_FUNC_ATTR(set_title, self_); + LOAD_FUNC_ATTR(set_xlabel, self_); + LOAD_FUNC_ATTR(set_xlim, self_); + LOAD_FUNC_ATTR(set_ylabel, self_); + LOAD_FUNC_ATTR(set_ylim, self_); + LOAD_FUNC_ATTR(text, self_); +} +} // namespace axes +} // namespace autoware::pyplot diff --git a/testing/autoware_pyplot/src/common.cpp b/testing/autoware_pyplot/src/common.cpp new file mode 100644 index 0000000000..5f1226cb5b --- /dev/null +++ b/testing/autoware_pyplot/src/common.cpp @@ -0,0 +1,32 @@ +// Copyright 2024 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 + +namespace autoware::pyplot +{ +inline namespace common +{ +PyObjectWrapper::PyObjectWrapper(const pybind11::object & object) +{ + self_ = object; +} +PyObjectWrapper::PyObjectWrapper(pybind11::object && object) +{ + self_ = std::move(object); +} +} // namespace common +} // namespace autoware::pyplot diff --git a/testing/autoware_pyplot/src/figure.cpp b/testing/autoware_pyplot/src/figure.cpp new file mode 100644 index 0000000000..07aacf8dd5 --- /dev/null +++ b/testing/autoware_pyplot/src/figure.cpp @@ -0,0 +1,66 @@ +// Copyright 2024 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 + +namespace autoware::pyplot +{ +inline namespace figure +{ +Figure::Figure(const pybind11::object & object) : PyObjectWrapper(object) +{ + load_attrs(); +} +Figure::Figure(pybind11::object && object) : PyObjectWrapper(object) +{ + load_attrs(); +} + +void Figure::load_attrs() +{ + LOAD_FUNC_ATTR(add_axes, self_); + LOAD_FUNC_ATTR(add_subplot, self_); + LOAD_FUNC_ATTR(colorbar, self_); + LOAD_FUNC_ATTR(savefig, self_); + LOAD_FUNC_ATTR(tight_layout, self_); +} + +axes::Axes Figure::add_axes(const pybind11::tuple & args, const pybind11::dict & kwargs) const +{ + return axes::Axes{add_axes_attr(*args, **kwargs)}; +} + +axes::Axes Figure::add_subplot(const pybind11::tuple & args, const pybind11::dict & kwargs) const +{ + return axes::Axes{add_subplot_attr(*args, **kwargs)}; +} + +PyObjectWrapper Figure::colorbar(const pybind11::tuple & args, const pybind11::dict & kwargs) const +{ + return PyObjectWrapper{colorbar_attr(*args, **kwargs)}; +} + +PyObjectWrapper Figure::savefig(const pybind11::tuple & args, const pybind11::dict & kwargs) const +{ + return PyObjectWrapper{savefig_attr(*args, **kwargs)}; +} + +PyObjectWrapper Figure::tight_layout( + const pybind11::tuple & args, const pybind11::dict & kwargs) const +{ + return PyObjectWrapper{tight_layout_attr(*args, **kwargs)}; +} +} // namespace figure +} // namespace autoware::pyplot diff --git a/testing/autoware_pyplot/src/legend.cpp b/testing/autoware_pyplot/src/legend.cpp new file mode 100644 index 0000000000..e5a128725f --- /dev/null +++ b/testing/autoware_pyplot/src/legend.cpp @@ -0,0 +1,28 @@ +// Copyright 2024 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 + +namespace autoware::pyplot +{ +inline namespace legend +{ +Legend::Legend(const pybind11::object & object) : PyObjectWrapper(object) +{ +} +Legend::Legend(pybind11::object && object) : PyObjectWrapper(object) +{ +} +} // namespace legend +} // namespace autoware::pyplot diff --git a/testing/autoware_pyplot/src/patches.cpp b/testing/autoware_pyplot/src/patches.cpp new file mode 100644 index 0000000000..4df4de27e8 --- /dev/null +++ b/testing/autoware_pyplot/src/patches.cpp @@ -0,0 +1,45 @@ +// Copyright 2024 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 + +namespace autoware::pyplot +{ +inline namespace patches +{ +Circle::Circle(const pybind11::tuple & args, const pybind11::dict & kwargs) +: PyObjectWrapper(pybind11::module::import("matplotlib.patches").attr("Circle")) +{ + self_ = self_(*args, **kwargs); +} + +Ellipse::Ellipse(const pybind11::tuple & args, const pybind11::dict & kwargs) +: PyObjectWrapper(pybind11::module::import("matplotlib.patches").attr("Ellipse")) +{ + self_ = self_(*args, **kwargs); +} + +Rectangle::Rectangle(const pybind11::tuple & args, const pybind11::dict & kwargs) +: PyObjectWrapper(pybind11::module::import("matplotlib.patches").attr("Rectangle")) +{ + self_ = self_(*args, **kwargs); +} + +Polygon::Polygon(const pybind11::tuple & args, const pybind11::dict & kwargs) +: PyObjectWrapper(pybind11::module::import("matplotlib.patches").attr("Polygon")) +{ + self_ = self_(*args, **kwargs); +} +} // namespace patches +} // namespace autoware::pyplot diff --git a/testing/autoware_pyplot/src/pyplot.cpp b/testing/autoware_pyplot/src/pyplot.cpp new file mode 100644 index 0000000000..624a1b1db0 --- /dev/null +++ b/testing/autoware_pyplot/src/pyplot.cpp @@ -0,0 +1,208 @@ +// Copyright 2024 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 + +namespace autoware::pyplot +{ +PyPlot::PyPlot(const pybind11::module & mod_) : mod(mod_) +{ + load_attrs(); +} + +void PyPlot::load_attrs() +{ + LOAD_FUNC_ATTR(axes, mod); + LOAD_FUNC_ATTR(cla, mod); + LOAD_FUNC_ATTR(clf, mod); + LOAD_FUNC_ATTR(figure, mod); + LOAD_FUNC_ATTR(gca, mod); + LOAD_FUNC_ATTR(gcf, mod); + LOAD_FUNC_ATTR(gci, mod); + LOAD_FUNC_ATTR(grid, mod); + LOAD_FUNC_ATTR(imshow, mod); + LOAD_FUNC_ATTR(legend, mod); + LOAD_FUNC_ATTR(plot, mod); + LOAD_FUNC_ATTR(quiver, mod); + LOAD_FUNC_ATTR(savefig, mod); + LOAD_FUNC_ATTR(scatter, mod); + LOAD_FUNC_ATTR(show, mod); + LOAD_FUNC_ATTR(subplot, mod); + LOAD_FUNC_ATTR(subplots, mod); + LOAD_FUNC_ATTR(title, mod); + LOAD_FUNC_ATTR(xlabel, mod); + LOAD_FUNC_ATTR(xlim, mod); + LOAD_FUNC_ATTR(ylabel, mod); + LOAD_FUNC_ATTR(ylim, mod); +} + +axes::Axes PyPlot::axes(const pybind11::dict & kwargs) +{ + return axes::Axes{axes_attr(**kwargs)}; +} + +PyObjectWrapper PyPlot::cla(const pybind11::tuple & args, const pybind11::dict & kwargs) +{ + return PyObjectWrapper{cla_attr(*args, **kwargs)}; +} + +PyObjectWrapper PyPlot::clf(const pybind11::tuple & args, const pybind11::dict & kwargs) +{ + return PyObjectWrapper{clf_attr(*args, **kwargs)}; +} + +figure::Figure PyPlot::figure(const pybind11::tuple & args, const pybind11::dict & kwargs) +{ + return figure::Figure{figure_attr(*args, **kwargs)}; +} + +axes::Axes PyPlot::gca(const pybind11::tuple & args, const pybind11::dict & kwargs) +{ + return axes::Axes{gca_attr(*args, **kwargs)}; +} + +figure::Figure PyPlot::gcf(const pybind11::tuple & args, const pybind11::dict & kwargs) +{ + return figure::Figure{gcf_attr(*args, **kwargs)}; +} + +PyObjectWrapper PyPlot::gci(const pybind11::tuple & args, const pybind11::dict & kwargs) +{ + return PyObjectWrapper{gci_attr(*args, **kwargs)}; +} + +PyObjectWrapper PyPlot::grid(const pybind11::tuple & args, const pybind11::dict & kwargs) +{ + return PyObjectWrapper{grid_attr(*args, **kwargs)}; +} + +PyObjectWrapper PyPlot::imshow(const pybind11::tuple & args, const pybind11::dict & kwargs) +{ + return PyObjectWrapper{imshow_attr(*args, **kwargs)}; +} + +PyObjectWrapper PyPlot::legend(const pybind11::tuple & args, const pybind11::dict & kwargs) +{ + return PyObjectWrapper{legend_attr(*args, **kwargs)}; +} + +PyObjectWrapper PyPlot::plot(const pybind11::tuple & args, const pybind11::dict & kwargs) +{ + return PyObjectWrapper{plot_attr(*args, **kwargs)}; +} + +PyObjectWrapper PyPlot::quiver(const pybind11::tuple & args, const pybind11::dict & kwargs) +{ + return PyObjectWrapper{quiver_attr(*args, **kwargs)}; +} + +PyObjectWrapper PyPlot::scatter(const pybind11::tuple & args, const pybind11::dict & kwargs) +{ + return PyObjectWrapper{scatter_attr(*args, **kwargs)}; +} + +PyObjectWrapper PyPlot::savefig(const pybind11::tuple & args, const pybind11::dict & kwargs) +{ + return PyObjectWrapper{savefig_attr(*args, **kwargs)}; +} + +PyObjectWrapper PyPlot::show(const pybind11::tuple & args, const pybind11::dict & kwargs) +{ + return PyObjectWrapper{show_attr(*args, **kwargs)}; +} + +axes::Axes PyPlot::subplot(const pybind11::dict & kwargs) +{ + return axes::Axes(subplot_attr(**kwargs)); +} + +axes::Axes PyPlot::subplot(int cri) +{ + return axes::Axes(subplot_attr(cri)); +} + +std::tuple PyPlot::subplots(const pybind11::dict & kwargs) +{ + pybind11::list ret = subplots_attr(**kwargs); + pybind11::object fig = ret[0]; + pybind11::object ax = ret[1]; + return {figure::Figure(fig), axes::Axes(ax)}; +} + +std::tuple> PyPlot::subplots( + int r, int c, const pybind11::dict & kwargs) +{ + // subplots() returns [][] (if r > 1 && c > 1) else [] + // return []axes in row-major + // NOTE: equal to Axes.flat + pybind11::tuple args = pybind11::make_tuple(r, c); + pybind11::list ret = subplots_attr(*args, **kwargs); + std::vector axes; + pybind11::object fig = ret[0]; + figure::Figure figure(fig); + if (r == 1 && c == 1) { + // python returns Axes + axes.emplace_back(ret[1]); + } else if (r == 1 || c == 1) { + // python returns []Axes + pybind11::list axs = ret[1]; + for (int i = 0; i < r * c; ++i) axes.emplace_back(axs[i]); + } else { + // python returns [][]Axes + pybind11::list axs = ret[1]; + for (pybind11::size_t i = 0; i < axs.size(); ++i) { + pybind11::list ax_i = axs[i]; + for (unsigned j = 0; j < ax_i.size(); ++j) axes.emplace_back(ax_i[j]); + } + } + return {figure, axes}; +} + +PyObjectWrapper PyPlot::title(const pybind11::tuple & args, const pybind11::dict & kwargs) +{ + return PyObjectWrapper{title_attr(*args, **kwargs)}; +} + +PyObjectWrapper PyPlot::xlabel(const pybind11::tuple & args, const pybind11::dict & kwargs) +{ + return PyObjectWrapper{xlabel_attr(*args, **kwargs)}; +} + +PyObjectWrapper PyPlot::xlim(const pybind11::tuple & args, const pybind11::dict & kwargs) +{ + return PyObjectWrapper{xlim_attr(*args, **kwargs)}; +} + +PyObjectWrapper PyPlot::ylabel(const pybind11::tuple & args, const pybind11::dict & kwargs) +{ + return PyObjectWrapper{ylabel_attr(*args, **kwargs)}; +} + +PyObjectWrapper PyPlot::ylim(const pybind11::tuple & args, const pybind11::dict & kwargs) +{ + return PyObjectWrapper{ylim_attr(*args, **kwargs)}; +} + +PyPlot import() +{ + auto mod = pybind11::module::import("matplotlib.pyplot"); + auto g_pyplot = PyPlot(mod); + return g_pyplot; +} + +} // namespace autoware::pyplot diff --git a/testing/autoware_pyplot/src/quiver.cpp b/testing/autoware_pyplot/src/quiver.cpp new file mode 100644 index 0000000000..72614fb622 --- /dev/null +++ b/testing/autoware_pyplot/src/quiver.cpp @@ -0,0 +1,28 @@ +// Copyright 2024 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 + +namespace autoware::pyplot +{ +inline namespace quiver +{ +Quiver::Quiver(const pybind11::object & object) : PyObjectWrapper(object) +{ +} +Quiver::Quiver(pybind11::object && object) : PyObjectWrapper(object) +{ +} +} // namespace quiver +} // namespace autoware::pyplot diff --git a/testing/autoware_pyplot/src/text.cpp b/testing/autoware_pyplot/src/text.cpp new file mode 100644 index 0000000000..024c7c2daf --- /dev/null +++ b/testing/autoware_pyplot/src/text.cpp @@ -0,0 +1,35 @@ +// Copyright 2024 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 + +namespace autoware::pyplot +{ +inline namespace text +{ +Text::Text(const pybind11::object & object) : PyObjectWrapper(object) +{ +} + +Text::Text(pybind11::object && object) : PyObjectWrapper(object) +{ +} + +void Text::load_attrs() +{ + LOAD_FUNC_ATTR(set_rotation, self_); +} +} // namespace text +} // namespace autoware::pyplot diff --git a/testing/autoware_pyplot/test/test_pyplot.cpp b/testing/autoware_pyplot/test/test_pyplot.cpp new file mode 100644 index 0000000000..2055709c61 --- /dev/null +++ b/testing/autoware_pyplot/test/test_pyplot.cpp @@ -0,0 +1,64 @@ +// Copyright 2024 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 +/* + very weirdly, you must include to pass vector. Although the code + compiles without it, the executable crashes at runtime + */ +#include + +#include + +TEST(PyPlot, single_plot) +{ + // NOTE: somehow, running multiple tests simultaneously causes the python interpreter to crash + py::scoped_interpreter guard{}; + auto plt = autoware::pyplot::import(); + { + plt.plot(Args(std::vector({1, 3, 2, 4})), Kwargs("color"_a = "blue", "linewidth"_a = 1.0)); + plt.xlabel(Args("x-title")); + plt.ylabel(Args("y-title")); + plt.title(Args("title")); + plt.xlim(Args(0, 5)); + plt.ylim(Args(0, 5)); + plt.grid(Args(true)); + plt.savefig(Args("test_single_plot.png")); + } + { + auto [fig, axes] = plt.subplots(1, 2); + auto & ax1 = axes[0]; + auto & ax2 = axes[1]; + + auto c = + autoware::pyplot::Circle(Args(py::make_tuple(0, 0), 0.5), Kwargs("fc"_a = "g", "ec"_a = "r")); + ax1.add_patch(Args(c.unwrap())); + + auto e = autoware::pyplot::Ellipse( + Args(py::make_tuple(-0.25, 0), 0.5, 0.25), Kwargs("fc"_a = "b", "ec"_a = "y")); + ax1.add_patch(Args(e.unwrap())); + + auto r = autoware::pyplot::Rectangle( + Args(py::make_tuple(0, 0), 0.25, 0.5), Kwargs("ec"_a = "#000000", "fill"_a = false)); + ax2.add_patch(Args(r.unwrap())); + + ax1.set_aspect(Args("equal")); + ax2.set_aspect(Args("equal")); + plt.savefig(Args("test_double_plot.svg")); + } +} From 1f4723862cc5dbde73a800aa1fda30871ca92ae3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=BF=83=E5=88=9A?= <90366790+liuXinGangChina@users.noreply.github.com> Date: Fri, 21 Feb 2025 09:16:54 +0800 Subject: [PATCH 7/8] feat(autoware_objects_of_interest_marker_interface): porting from universe to core (#168) Signed-off-by: liuXinGangChina --- .../CMakeLists.txt | 27 ++++ .../README.md | 46 +++++++ .../coloring.hpp | 31 +++++ .../marker_data.hpp | 34 +++++ .../marker_utils.hpp | 86 +++++++++++++ .../objects_of_interest_marker_interface.hpp | 103 ++++++++++++++++ .../package.xml | 31 +++++ .../src/coloring.cpp | 54 ++++++++ .../src/marker_utils.cpp | 116 ++++++++++++++++++ .../objects_of_interest_marker_interface.cpp | 92 ++++++++++++++ 10 files changed, 620 insertions(+) create mode 100644 planning/autoware_objects_of_interest_marker_interface/CMakeLists.txt create mode 100644 planning/autoware_objects_of_interest_marker_interface/README.md create mode 100644 planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/coloring.hpp create mode 100644 planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/marker_data.hpp create mode 100644 planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/marker_utils.hpp create mode 100644 planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp create mode 100644 planning/autoware_objects_of_interest_marker_interface/package.xml create mode 100644 planning/autoware_objects_of_interest_marker_interface/src/coloring.cpp create mode 100644 planning/autoware_objects_of_interest_marker_interface/src/marker_utils.cpp create mode 100644 planning/autoware_objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp diff --git a/planning/autoware_objects_of_interest_marker_interface/CMakeLists.txt b/planning/autoware_objects_of_interest_marker_interface/CMakeLists.txt new file mode 100644 index 0000000000..da1161c007 --- /dev/null +++ b/planning/autoware_objects_of_interest_marker_interface/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.5) +project(autoware_objects_of_interest_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(autoware_objects_of_interest_marker_interface SHARED + src/coloring.cpp + src/objects_of_interest_marker_interface.cpp + src/marker_utils.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/autoware_objects_of_interest_marker_interface/README.md b/planning/autoware_objects_of_interest_marker_interface/README.md new file mode 100644 index 0000000000..0e4aefe898 --- /dev/null +++ b/planning/autoware_objects_of_interest_marker_interface/README.md @@ -0,0 +1,46 @@ +# Objects Of Interest Marker Interface + +## Overview + +`autoware_objects_of_interest_marker_interface` is a collection of object visualization function packages. + +## Design + +This package implement a library to manage and visualize the object information by construct and publish it as marker array to rviz. + +For a object to be visualized, it has three import characteristics. + +- pose the position of the object +- shape the shape of the Bounding box of the object +- color the color of the Bounding box of the object + +## Usage + +### init + +include the header file to use then init the library + +```cpp +#include + +autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface + objects_of_interest_marker_interface_{this, "obstacle_cruise_planner"}; +``` + +### insert + +insert object information to the 'objects_of_interest_marker_interface' manager + +```cpp +using autoware::objects_of_interest_marker_interface::ColorName; +objects_of_interest_marker_interface_.insertObjectData( + stopped_obstacle.pose, stopped_obstacle.shape, ColorName::RED); +``` + +### publish + +publish object information to the rviz to visualize + +```cpp +objects_of_interest_marker_interface_.publishMarkerArray(); +``` diff --git a/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/coloring.hpp b/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/coloring.hpp new file mode 100644 index 0000000000..b155191817 --- /dev/null +++ b/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/coloring.hpp @@ -0,0 +1,31 @@ +// 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 AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__COLORING_HPP_ +#define AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__COLORING_HPP_ +#include "autoware/objects_of_interest_marker_interface/marker_data.hpp" + +#include + +#include + +namespace autoware::objects_of_interest_marker_interface::coloring +{ +std_msgs::msg::ColorRGBA getGreen(const float alpha); +std_msgs::msg::ColorRGBA getAmber(const float alpha); +std_msgs::msg::ColorRGBA getRed(const float alpha); +std_msgs::msg::ColorRGBA getGray(const float alpha); +} // namespace autoware::objects_of_interest_marker_interface::coloring + +#endif // AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__COLORING_HPP_ diff --git a/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/marker_data.hpp b/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/marker_data.hpp new file mode 100644 index 0000000000..53a5cb091d --- /dev/null +++ b/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/marker_data.hpp @@ -0,0 +1,34 @@ +// 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 AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_DATA_HPP_ +#define AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_DATA_HPP_ + +#include +#include +#include + +namespace autoware::objects_of_interest_marker_interface +{ +struct ObjectMarkerData +{ + geometry_msgs::msg::Pose pose{}; + autoware_perception_msgs::msg::Shape shape{}; + std_msgs::msg::ColorRGBA color; +}; + +enum class ColorName { GRAY, GREEN, AMBER, RED }; +} // namespace autoware::objects_of_interest_marker_interface + +#endif // AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_DATA_HPP_ diff --git a/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/marker_utils.hpp b/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/marker_utils.hpp new file mode 100644 index 0000000000..1e484ca888 --- /dev/null +++ b/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/marker_utils.hpp @@ -0,0 +1,86 @@ +// 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 AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_UTILS_HPP_ +#define AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_UTILS_HPP_ +#include "autoware/objects_of_interest_marker_interface/coloring.hpp" +#include "autoware/objects_of_interest_marker_interface/marker_data.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace autoware::objects_of_interest_marker_interface::marker_utils +{ +/** + * @brief Create arrow marker from object marker data + * @param id Marker id + * @param data Object marker data + * @param name Module name + * @param height_offset Height offset of arrow marker + * @param arrow_length Length of arrow marker + */ +visualization_msgs::msg::Marker createArrowMarker( + const size_t id, const ObjectMarkerData & data, const std::string & name, + const double height_offset, const double arrow_length = 1.0); + +/** + * @brief Create circle marker from object marker data + * @param id Marker id + * @param data Object marker data + * @param name Module name + * @param radius Radius of circle marker + * @param height_offset Height offset of circle marker + * @param line_width Line width of circle marker + */ +visualization_msgs::msg::Marker createCircleMarker( + const size_t id, const ObjectMarkerData & data, const std::string & name, const double radius, + const double height_offset, const double line_width = 0.1); + +/** + * @brief Create text marker visualizing module name + * @param id Marker id + * @param data Object marker data + * @param name Module name + * @param height_offset Height offset of target marker + * @param text_size Text size + */ +visualization_msgs::msg::Marker createNameTextMarker( + const size_t id, const ObjectMarkerData & data, const std::string & name, + const double height_offset, const double text_size = 0.5); + +/** + * @brief Create target marker from object marker data + * @param id Marker id + * @param data Object marker data + * @param name Module name + * @param height_offset Height offset of target marker + * @param arrow_length Length of arrow marker + * @param line_width Line width of circle marker + */ +visualization_msgs::msg::MarkerArray createTargetMarker( + const size_t id, const ObjectMarkerData & data, const std::string & name, + const double height_offset, const double arrow_length = 1.0, const double line_width = 0.1); +} // namespace autoware::objects_of_interest_marker_interface::marker_utils + +#endif // AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_UTILS_HPP_ diff --git a/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp b/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp new file mode 100644 index 0000000000..619df5f719 --- /dev/null +++ b/planning/autoware_objects_of_interest_marker_interface/include/autoware/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp @@ -0,0 +1,103 @@ +// 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 AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__OBJECTS_OF_INTEREST_MARKER_INTERFACE_HPP_ +#define AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__OBJECTS_OF_INTEREST_MARKER_INTERFACE_HPP_ +#include "autoware/objects_of_interest_marker_interface/coloring.hpp" +#include "autoware/objects_of_interest_marker_interface/marker_data.hpp" +#include "autoware/objects_of_interest_marker_interface/marker_utils.hpp" + +#include + +#include +#include +#include +#include + +#include +#include + +namespace autoware::objects_of_interest_marker_interface +{ +class ObjectsOfInterestMarkerInterface +{ +public: + /** + * @brief Constructor + * @param node Node that publishes marker + * @param name Module name + */ + ObjectsOfInterestMarkerInterface(rclcpp::Node * node, const std::string & name); + + /** + * @brief Insert object data to visualize + * @param pose Object pose + * @param shape Object shape + * @param color_name Color name + */ + void insertObjectData( + const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape, + const ColorName & color_name); + + /** + * @brief Insert object data to visualize with custom color data + * @param pose Object pose + * @param shape Object shape + * @param color Color data with alpha + */ + void insertObjectDataWithCustomColor( + const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape, + const std_msgs::msg::ColorRGBA & color); + + /** + * @brief Publish interest objects marker + */ + void publishMarkerArray(); + + /** + * @brief Set height offset of markers + * @param offset Height offset of markers + */ + void setHeightOffset(const double offset); + + /** + * @brief Get color data from color name + * @param color_name Color name + * @param alpha Alpha + */ + static std_msgs::msg::ColorRGBA getColor(const ColorName & color_name, const float alpha = 0.99f); + + /** + * @brief Get module name including this interface + */ + std::string getName() const { return name_; } + + /** + * @brief Get height offset + */ + double getHeightOffset() const { return height_offset_; } + +private: + rclcpp::Publisher::SharedPtr pub_marker_; + + double height_offset_{0.5}; + std::vector obj_marker_data_array_; + + std::string name_; + std::string topic_namespace_ = "/planning/debug/objects_of_interest"; +}; + +} // namespace autoware::objects_of_interest_marker_interface + +#endif // AUTOWARE__OBJECTS_OF_INTEREST_MARKER_INTERFACE__OBJECTS_OF_INTEREST_MARKER_INTERFACE_HPP_ diff --git a/planning/autoware_objects_of_interest_marker_interface/package.xml b/planning/autoware_objects_of_interest_marker_interface/package.xml new file mode 100644 index 0000000000..27b07972d9 --- /dev/null +++ b/planning/autoware_objects_of_interest_marker_interface/package.xml @@ -0,0 +1,31 @@ + + + autoware_objects_of_interest_marker_interface + 0.0.0 + The autoware_objects_of_interest_marker_interface package + + Xingang Liu + Fumiya Watanabe + Kosuke Takeuchi + Zulfaqar Azmi + + Apache License 2.0 + + Fumiya Watanabe + + ament_cmake_auto + + autoware_perception_msgs + autoware_utils + geometry_msgs + rclcpp + std_msgs + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/autoware_objects_of_interest_marker_interface/src/coloring.cpp b/planning/autoware_objects_of_interest_marker_interface/src/coloring.cpp new file mode 100644 index 0000000000..2d47c3ce15 --- /dev/null +++ b/planning/autoware_objects_of_interest_marker_interface/src/coloring.cpp @@ -0,0 +1,54 @@ +// 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 "autoware/objects_of_interest_marker_interface/coloring.hpp" + +namespace +{ +std_msgs::msg::ColorRGBA convertFromColorCode(const uint64_t code, const float alpha) +{ + const float r = static_cast(code >> 16) / 255.0; + const float g = static_cast((code << 48) >> 56) / 255.0; + const float b = static_cast((code << 56) >> 56) / 255.0; + + return autoware_utils::create_marker_color(r, g, b, alpha); +} +} // namespace + +namespace autoware::objects_of_interest_marker_interface::coloring +{ +std_msgs::msg::ColorRGBA getGreen(const float alpha) +{ + constexpr uint64_t code = 0x00e676; + return convertFromColorCode(code, alpha); +} + +std_msgs::msg::ColorRGBA getAmber(const float alpha) +{ + constexpr uint64_t code = 0xffea00; + return convertFromColorCode(code, alpha); +} + +std_msgs::msg::ColorRGBA getRed(const float alpha) +{ + constexpr uint64_t code = 0xff3d00; + return convertFromColorCode(code, alpha); +} + +std_msgs::msg::ColorRGBA getGray(const float alpha) +{ + constexpr uint64_t code = 0xbdbdbd; + return convertFromColorCode(code, alpha); +} +} // namespace autoware::objects_of_interest_marker_interface::coloring diff --git a/planning/autoware_objects_of_interest_marker_interface/src/marker_utils.cpp b/planning/autoware_objects_of_interest_marker_interface/src/marker_utils.cpp new file mode 100644 index 0000000000..931400545e --- /dev/null +++ b/planning/autoware_objects_of_interest_marker_interface/src/marker_utils.cpp @@ -0,0 +1,116 @@ +// 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 "autoware/objects_of_interest_marker_interface/marker_utils.hpp" + +#include + +namespace autoware::objects_of_interest_marker_interface::marker_utils +{ +using geometry_msgs::msg::Point; + +using std_msgs::msg::ColorRGBA; + +using autoware_utils::create_default_marker; +using autoware_utils::create_marker_scale; + +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + +Marker createArrowMarker( + const size_t id, const ObjectMarkerData & data, const std::string & name, + const double height_offset, const double arrow_length) +{ + const double line_width = 0.25 * arrow_length; + const double head_width = 0.5 * arrow_length; + const double head_height = 0.5 * arrow_length; + + Marker marker = create_default_marker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), name + "_arrow", id, Marker::ARROW, + create_marker_scale(line_width, head_width, head_height), data.color); + + const double height = 0.5 * data.shape.dimensions.z; + + Point src, dst; + src = data.pose.position; + src.z += height + height_offset + arrow_length; + dst = data.pose.position; + dst.z += height + height_offset; + + marker.points.push_back(src); + marker.points.push_back(dst); + + return marker; +} + +Marker createCircleMarker( + const size_t id, const ObjectMarkerData & data, const std::string & name, const double radius, + const double height_offset, const double line_width) +{ + Marker marker = create_default_marker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), name, id, Marker::LINE_STRIP, + create_marker_scale(line_width, 0.0, 0.0), data.color); + + const double height = 0.5 * data.shape.dimensions.z; + + constexpr size_t num_points = 20; + for (size_t i = 0; i < num_points; ++i) { + Point point; + const double ratio = static_cast(i) / static_cast(num_points); + const double theta = 2 * autoware_utils::pi * ratio; + point.x = data.pose.position.x + radius * autoware_utils::cos(theta); + point.y = data.pose.position.y + radius * autoware_utils::sin(theta); + point.z = data.pose.position.z + height + height_offset; + marker.points.push_back(point); + } + marker.points.push_back(marker.points.front()); + + return marker; +} + +visualization_msgs::msg::Marker createNameTextMarker( + const size_t id, const ObjectMarkerData & data, const std::string & name, + const double height_offset, const double text_size) +{ + Marker marker = create_default_marker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), name + "_name_text", id, Marker::TEXT_VIEW_FACING, + create_marker_scale(0.0, 0.0, text_size), coloring::getGray(data.color.a)); + + marker.text = name; + + const double height = 0.5 * data.shape.dimensions.z; + marker.pose = data.pose; + marker.pose.position.z += height + height_offset; + + return marker; +} + +MarkerArray createTargetMarker( + const size_t id, const ObjectMarkerData & data, const std::string & name, + const double height_offset, const double arrow_length, const double line_width) +{ + MarkerArray marker_array; + marker_array.markers.push_back(createArrowMarker(id, data, name, height_offset, arrow_length)); + marker_array.markers.push_back(createCircleMarker( + id, data, name + "_circle1", 0.5 * arrow_length, height_offset + 0.75 * arrow_length, + line_width)); + marker_array.markers.push_back(createCircleMarker( + id, data, name + "_circle2", 0.75 * arrow_length, height_offset + 0.75 * arrow_length, + line_width)); + marker_array.markers.push_back( + createNameTextMarker(id, data, name, height_offset + 1.5 * arrow_length, 0.5 * arrow_length)); + + return marker_array; +} +} // namespace autoware::objects_of_interest_marker_interface::marker_utils diff --git a/planning/autoware_objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp b/planning/autoware_objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp new file mode 100644 index 0000000000..055a836f6b --- /dev/null +++ b/planning/autoware_objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp @@ -0,0 +1,92 @@ +// 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 "autoware/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp" + +#include +#include +#include + +#include + +namespace autoware::objects_of_interest_marker_interface +{ +using autoware_perception_msgs::msg::Shape; +using geometry_msgs::msg::Pose; +using std_msgs::msg::ColorRGBA; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + +ObjectsOfInterestMarkerInterface::ObjectsOfInterestMarkerInterface( + rclcpp::Node * node, const std::string & name) +: name_{name} +{ + // Publisher + pub_marker_ = node->create_publisher(topic_namespace_ + "/" + name, 1); +} + +void ObjectsOfInterestMarkerInterface::insertObjectData( + const Pose & pose, const Shape & shape, const ColorName & color_name) +{ + insertObjectDataWithCustomColor(pose, shape, getColor(color_name)); +} + +void ObjectsOfInterestMarkerInterface::insertObjectDataWithCustomColor( + const Pose & pose, const Shape & shape, const ColorRGBA & color) +{ + ObjectMarkerData data; + data.pose = pose; + data.shape = shape; + data.color = color; + + obj_marker_data_array_.push_back(data); +} + +void ObjectsOfInterestMarkerInterface::publishMarkerArray() +{ + MarkerArray marker_array; + for (size_t i = 0; i < obj_marker_data_array_.size(); ++i) { + const auto data = obj_marker_data_array_.at(i); + const MarkerArray target_marker = + marker_utils::createTargetMarker(i, data, getName(), getHeightOffset()); + marker_array.markers.insert( + marker_array.markers.end(), target_marker.markers.begin(), target_marker.markers.end()); + } + pub_marker_->publish(marker_array); + obj_marker_data_array_.clear(); +} + +void ObjectsOfInterestMarkerInterface::setHeightOffset(const double offset) +{ + height_offset_ = offset; +} + +ColorRGBA ObjectsOfInterestMarkerInterface::getColor( + const ColorName & color_name, const float alpha) +{ + switch (color_name) { + case ColorName::GRAY: + return coloring::getGray(alpha); + case ColorName::GREEN: + return coloring::getGreen(alpha); + case ColorName::AMBER: + return coloring::getAmber(alpha); + case ColorName::RED: + return coloring::getRed(alpha); + default: + return coloring::getGray(alpha); + } +} + +} // namespace autoware::objects_of_interest_marker_interface From f5d1eb53ce3c8af58ad16fad256ed767ecd9001c Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Fri, 21 Feb 2025 02:51:58 +0000 Subject: [PATCH 8/8] chore: update CODEOWNERS (#202) Signed-off-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> Co-authored-by: github-actions --- .github/CODEOWNERS | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 75ce4e675e..a4f927e669 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,4 +1,5 @@ ### Automatically generated from package.xml ### +common/autoware_component_interface_specs/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-core-global-codeowners common/autoware_geography_utils/** anh.nguyen.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp @autowarefoundation/autoware-core-global-codeowners common/autoware_interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-core-global-codeowners common/autoware_kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-core-global-codeowners @@ -11,6 +12,8 @@ common/autoware_trajectory/** mamoru.sobue@tier4.jp takayuki.murooka@tier4.jp yu demos/autoware_test_node/** mfc@autoware.org @autowarefoundation/autoware-core-global-codeowners localization/autoware_ekf_localizer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp @autowarefoundation/autoware-core-global-codeowners localization/autoware_localization_util/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp lxg19892021@gmail.com masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp @autowarefoundation/autoware-core-global-codeowners +sensing/autoware_gnss_poser/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp lxg19892021@gmail.com masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp @autowarefoundation/autoware-core-global-codeowners +testing/autoware_pyplot/** mamoru.sobue@tier4.jp yukinari.hisaki.2@tier4.jp @autowarefoundation/autoware-core-global-codeowners testing/autoware_testing/** adam.dabrowski@robotec.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-core-global-codeowners ### Copied from .github/CODEOWNERS-manual ###