From d6327b4d5cfa6268ef3b88fdc345bc4776a09855 Mon Sep 17 00:00:00 2001 From: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com> Date: Fri, 3 Dec 2021 14:00:46 +0900 Subject: [PATCH] feat: add twist_estimator packages (#57) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * release v0.4.0 * Avoid setting CMAKE_BUILD_TYPE=Release in each CMakeLists.txt (#720) * remove set CMAKE_BUILD_TYPE Release in each CMakeLists.txt * remove set CMAKE_BUILD_TYPE Release in ndt_pcl_modified * set compile options for debug in ndt_omp * Fix indent * add warning if -DCMAKE_BUILD_TYPE=Release is not set in ndt_omp Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> * remove ROS1 packages temporarily Signed-off-by: mitsudome-r * Revert "remove ROS1 packages temporarily" This reverts commit 2ba159d0875d817ae73a0df4ba12c4660c86acfe. Signed-off-by: mitsudome-r * add COLCON_IGNORE to ros1 packages Signed-off-by: mitsudome-r * Rename launch files to launch.xml (#28) * ROS2 Porting: gyro_odometer (#39) * Make compile - Fix cmake and package.xml - Update tf and msgs * Add publishers, subscribers and loggers - Fix launch file and Cmake to generate executable * Address PR comments: - Remove launch file xml line (not needed) - Convert Buffer and TransformListener to plain objects - Fix some typos * ported pose2twist from ROS1 to ROS2 (#19) * ported pose2twist from ROS1 to ROS2 * Update CMakeLists.txt updated CMakeLists to be consise using ament_auto * Update pose2twist_core.cpp updated the ~Pose2Twist() = default in header * Update package.xml sorted dependencies alphabetically * Update pose2twist_core.h added ~Pose2Twist() = default; * Update pose2twist_core.h Include guards replaced * edited pose2twist launch file to ROS2 * fixed package XML * Add geometry2 to repos (#76) * add geometry2 package temporarily until new release Signed-off-by: mitsudome-r * trigger-ci Signed-off-by: mitsudome-r * add tf2 dependency to the packages that use tf2_geometry_msgs Signed-off-by: mitsudome-r * Revert "Add geometry2 to repos (#76)" (#96) * Revert "Add geometry2 to repos (#76)" This reverts commit 2e5ba3e86d0597920c0adf23f066521ea8a1a5b6. * Re-add tf2 dependencies * Revert "Re-add tf2 dependencies" This reverts commit e23b0c8b0826cf9518924d33349f9de34b4975df. * Debug CI pipeline * Revert "Debug CI pipeline" This reverts commit 58f1eba550360d82c08230552abfb64b33b23e0f. * Explicitly install known versions of the geometry packages * No need to skip tf2 packages anymore Co-authored-by: Esteve Fernandez * Rename h files to hpp (#142) * Change includes * Rename files * Adjustments to make things compile * Other packages * Adjust copyright notice on 532 out of 699 source files (#143) * Use quotes for includes where appropriate (#144) * Use quotes for includes where appropriate * Fix lint tests * Make tests pass hopefully * Run uncrustify on the entire Pilot.Auto codebase (#151) * Run uncrustify on the entire Pilot.Auto codebase * Exclude open PRs * adding linters to pose2twist (#193) * adding linters to gyro_odometer (#192) * apply env_var to use_sim_time (#222) * Fix dt in pose2twist (#289) * fix dt Signed-off-by: Kosuke Murakami * apply format Signed-off-by: Kosuke Murakami * fix covariance (#875) (#267) Signed-off-by: Yamato Ando Co-authored-by: YamatoAndo * rm std_msgs (#359) * rm_std_msgs (#402) * Fix rolling build errors (#1225) * Add missing include files Signed-off-by: Kenji Miyake * Replace rclcpp::Duration Signed-off-by: Kenji Miyake * Use reference for exceptions Signed-off-by: Kenji Miyake * Use from_seconds Signed-off-by: Kenji Miyake * Sync public repo (#1228) * [simple_planning_simulator] add readme (#424) * add readme of simple_planning_simulator Signed-off-by: Takamasa Horibe * Update simulator/simple_planning_simulator/README.md * set transit_margin_time to intersect. planner (#460) * Fix pose2twist (#462) Signed-off-by: Takagi, Isamu * Ros2 vehicle info param server (#447) * add vehicle_info_param_server * update vehicle info * apply format * fix bug * skip unnecessary search * delete vehicle param file * fix bug * Ros2 fix topic name part2 (#425) * Fix topic name of traffic_light_classifier Signed-off-by: Takagi, Isamu * Fix topic name of traffic_light_visualization Signed-off-by: Takagi, Isamu * Fix topic name of traffic_light_ssd_fine_detector Signed-off-by: Takagi, Isamu * Fix topic name of traffic_light_map_based_detector Signed-off-by: Takagi, Isamu * Fix lint traffic_light_recognition Signed-off-by: Takagi, Isamu * Fix lint traffic_light_ssd_fine_detector Signed-off-by: Takagi, Isamu * Fix lint traffic_light_classifier Signed-off-by: Takagi, Isamu * Fix lint traffic_light_classifier Signed-off-by: Takagi, Isamu * Fix lint traffic_light_ssd_fine_detector Signed-off-by: Takagi, Isamu * Fix issues in hdd_reader (#466) * Fix some issues detected by Coverity Scan and Clang-Tidy * Update launch command * Add more `close(new_sock)` * Simplify the definitions of struct * fix: re-construct laneletMapLayer for reindex RTree (#463) * Rviz overlay render fix (#461) * Moved painiting in SteeringAngle plugin to update() Signed-off-by: Adam Dabrowski * super class now back to MFD Signed-off-by: Adam Dabrowski * uncrustified Signed-off-by: Adam Dabrowski * acquire data in mutex Signed-off-by: Adam Dabrowski * back to RTD as superclass Signed-off-by: Adam Dabrowski * Rviz overlay render in update (#465) * Moved painiting in SteeringAngle plugin to update() Signed-off-by: Adam Dabrowski * super class now back to MFD Signed-off-by: Adam Dabrowski * uncrustified Signed-off-by: Adam Dabrowski * acquire data in mutex Signed-off-by: Adam Dabrowski * removed unnecessary includes and some dead code Signed-off-by: Adam Dabrowski * Adepted remaining vehicle plugin classes to render-in-update concept. Returned to MFD superclass Signed-off-by: Adam Dabrowski * restored RTD superclass Signed-off-by: Adam Dabrowski Co-authored-by: Takamasa Horibe Co-authored-by: tkimura4 Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Kazuki Miyahara Co-authored-by: Makoto Tokunaga Co-authored-by: Adam Dąbrowski * Fix for rolling (#1226) * Replace doc by description Signed-off-by: Kenji Miyake * Replace ns by push-ros-namespace Signed-off-by: Kenji Miyake * Unify Apache-2.0 license name (#1242) * Remove use_sim_time for set_parameter (#1260) Signed-off-by: wep21 * Feature/gyro odometer add twist with cov input (#1586) * add description for gyro_odometer package #1819 * Fix -Wunused-parameter (#1836) * Fix -Wunused-parameter Signed-off-by: Kenji Miyake * Fix mistake Signed-off-by: Kenji Miyake * fix spell * Fix lint issues Signed-off-by: Kenji Miyake * Ignore flake8 warnings Signed-off-by: Kenji Miyake Co-authored-by: Hiroki OTA * Invoke code formatter at pre-commit (#1935) * Run ament_uncrustify at pre-commit * Reformat existing files * Fix copyright and cpplint errors Signed-off-by: Kenji Miyake Co-authored-by: Kenji Miyake * Feature/add stop filter ros2 (#1575) * revert imu yaw bias (#2040) (#2043) Co-authored-by: YamatoAndo * add sort-package-xml hook in pre-commit (#1881) * add sort xml hook in pre-commit * change retval to exit_status * rename * add prettier plugin-xml * use early return * add license note * add tier4 license * restore prettier * change license order * move local hooks to public repo * move prettier-xml to pre-commit-hooks-ros * update version for bug-fix * apply pre-commit * Change formatter to clang-format and black (#2332) * Revert "Temporarily comment out pre-commit hooks" This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3. * Replace ament_lint_common with autoware_lint_common Signed-off-by: Kenji Miyake * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake * Apply Black Signed-off-by: Kenji Miyake * Apply clang-format Signed-off-by: Kenji Miyake * Fix build errors Signed-off-by: Kenji Miyake * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake * Apply clang-format Signed-off-by: Kenji Miyake * Fix build errors Signed-off-by: Kenji Miyake * Add COLCON_IGNORE (#500) Signed-off-by: Kenji Miyake * Remove COLCON_IGNORE under localization (#553) * [gyro_odometer]add readme (#625) * add readme * update * update * Update localization/twist_estimator/gyro_odometer/README.md Co-authored-by: Kazuki Miyahara * add discription Co-authored-by: Takamasa Horibe Co-authored-by: Kazuki Miyahara * add readme in pose2twist (#662) * add readme Signed-off-by: Takamasa Horibe * Update localization/twist_estimator/pose2twist/src/README.md Co-authored-by: Kazuki Miyahara Co-authored-by: Kazuki Miyahara * fix readme (#669) Signed-off-by: Takamasa Horibe * convert VehicleReport to Twist msgs (#657) * add vehicle velocity converter package * clang format * add README * add heading_rate value * remove twist msg * clang format * set covariance value * fixed bug * [gyro_odometer] remove input twist (#706) * remove input twist * update README * fix: move README.md directory Co-authored-by: mitsudome-r Co-authored-by: Daichi Murakami Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Nikolai Morin Co-authored-by: Jilada Eccleston Co-authored-by: nik-tier4 <71747268+nik-tier4@users.noreply.github.com> Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Co-authored-by: Esteve Fernandez Co-authored-by: Kosuke Murakami Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: YamatoAndo Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Co-authored-by: Takamasa Horibe Co-authored-by: tkimura4 Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Kazuki Miyahara Co-authored-by: Makoto Tokunaga Co-authored-by: Adam Dąbrowski Co-authored-by: Hiroki OTA Co-authored-by: Takeshi Ishita Co-authored-by: Kenji Miyake Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> Co-authored-by: autoware-iv-sync-ci[bot] <87871706+autoware-iv-sync-ci[bot]@users.noreply.github.com> Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Co-authored-by: Sugatyon <32741405+Sugatyon@users.noreply.github.com> Co-authored-by: RyuYamamoto --- localization/gyro_odometer/CMakeLists.txt | 35 +++++ localization/gyro_odometer/README.md | 31 ++++ .../gyro_odometer/gyro_odometer_core.hpp | 60 ++++++++ .../launch/gyro_odometer.launch.xml | 22 +++ localization/gyro_odometer/package.xml | 27 ++++ .../gyro_odometer/src/gyro_odometer_core.cpp | 145 ++++++++++++++++++ .../gyro_odometer/src/gyro_odometer_node.cpp | 29 ++++ localization/pose2twist/CMakeLists.txt | 26 ++++ localization/pose2twist/README.md | 32 ++++ .../include/pose2twist/pose2twist_core.hpp | 40 +++++ .../pose2twist/launch/pose2twist.launch.xml | 11 ++ localization/pose2twist/package.xml | 22 +++ .../pose2twist/src/pose2twist_core.cpp | 128 ++++++++++++++++ .../pose2twist/src/pose2twist_node.cpp | 29 ++++ .../vehicle_velocity_converter/CMakeLists.txt | 34 ++++ .../vehicle_velocity_converter/README.md | 26 ++++ .../config/vehicle_velocity_converter.yaml | 42 +++++ .../vehicle_velocity_converter.hpp | 47 ++++++ .../vehicle_velocity_converter.launch.xml | 11 ++ .../vehicle_velocity_converter/package.xml | 21 +++ .../src/vehicle_velocity_converter.cpp | 51 ++++++ .../src/vehicle_velocity_converter_node.cpp | 25 +++ 22 files changed, 894 insertions(+) create mode 100644 localization/gyro_odometer/CMakeLists.txt create mode 100644 localization/gyro_odometer/README.md create mode 100644 localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp create mode 100644 localization/gyro_odometer/launch/gyro_odometer.launch.xml create mode 100644 localization/gyro_odometer/package.xml create mode 100644 localization/gyro_odometer/src/gyro_odometer_core.cpp create mode 100644 localization/gyro_odometer/src/gyro_odometer_node.cpp create mode 100644 localization/pose2twist/CMakeLists.txt create mode 100644 localization/pose2twist/README.md create mode 100644 localization/pose2twist/include/pose2twist/pose2twist_core.hpp create mode 100644 localization/pose2twist/launch/pose2twist.launch.xml create mode 100644 localization/pose2twist/package.xml create mode 100644 localization/pose2twist/src/pose2twist_core.cpp create mode 100644 localization/pose2twist/src/pose2twist_node.cpp create mode 100644 localization/vehicle_velocity_converter/CMakeLists.txt create mode 100644 localization/vehicle_velocity_converter/README.md create mode 100644 localization/vehicle_velocity_converter/config/vehicle_velocity_converter.yaml create mode 100644 localization/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp create mode 100644 localization/vehicle_velocity_converter/launch/vehicle_velocity_converter.launch.xml create mode 100644 localization/vehicle_velocity_converter/package.xml create mode 100644 localization/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp create mode 100644 localization/vehicle_velocity_converter/src/vehicle_velocity_converter_node.cpp diff --git a/localization/gyro_odometer/CMakeLists.txt b/localization/gyro_odometer/CMakeLists.txt new file mode 100644 index 0000000000000..4cf9cfc371af7 --- /dev/null +++ b/localization/gyro_odometer/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 3.5) +project(gyro_odometer) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +set(GYRO_ODOMETER_SRC + src/gyro_odometer_core.cpp) + +set(GYRO_ODOMETER_HEADERS + include/gyro_odometer/gyro_odometer_core.hpp) + +ament_auto_add_executable(${PROJECT_NAME} + src/gyro_odometer_node.cpp + ${GYRO_ODOMETER_SRC} + ${GYRO_ODOMETER_HEADERS} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE + launch +) diff --git a/localization/gyro_odometer/README.md b/localization/gyro_odometer/README.md new file mode 100644 index 0000000000000..e4471ed3e58e7 --- /dev/null +++ b/localization/gyro_odometer/README.md @@ -0,0 +1,31 @@ +# gyro_odometer + +## Purpose + +`gyro_odometer` is the package to estimate twist by combining imu and vehicle speed. + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ------------------------------- | ------------------------------------------------ | ---------------------------------- | +| `vehicle/twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | twist with covariance from vehicle | +| `imu` | `sensor_msgs::msg::Imu` | imu from sensor | + +### Output + +| Name | Type | Description | +| ----------------------- | ------------------------------------------------ | ------------------------------- | +| `twist` | `geometry_msgs::msg::TwistStamped` | estimated twist | +| `twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | estimated twist with covariance | + +## Parameters + +| Parameter | Type | Description | +| -------------- | ------ | ----------------- | +| `output_frame` | String | output's frame id | + +## Assumptions / Known limits + +TBD. diff --git a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp new file mode 100644 index 0000000000000..f95374a48a12d --- /dev/null +++ b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp @@ -0,0 +1,60 @@ +// Copyright 2015-2019 Autoware Foundation +// +// 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 GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ +#define GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ + +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include + +class GyroOdometer : public rclcpp::Node +{ +public: + GyroOdometer(); + ~GyroOdometer(); + +private: + void callbackTwistWithCovariance( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_with_cov_msg_ptr); + void callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr); + bool getTransform( + const std::string & target_frame, const std::string & source_frame, + const geometry_msgs::msg::TransformStamped::SharedPtr transform_stamped_ptr); + + rclcpp::Subscription::SharedPtr + vehicle_twist_with_cov_sub_; + rclcpp::Subscription::SharedPtr imu_sub_; + + rclcpp::Publisher::SharedPtr twist_pub_; + rclcpp::Publisher::SharedPtr + twist_with_covariance_pub_; + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + std::string output_frame_; + geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_with_cov_msg_ptr_; +}; + +#endif // GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ diff --git a/localization/gyro_odometer/launch/gyro_odometer.launch.xml b/localization/gyro_odometer/launch/gyro_odometer.launch.xml new file mode 100644 index 0000000000000..b3148aec12e55 --- /dev/null +++ b/localization/gyro_odometer/launch/gyro_odometer.launch.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/localization/gyro_odometer/package.xml b/localization/gyro_odometer/package.xml new file mode 100644 index 0000000000000..6c3669a988501 --- /dev/null +++ b/localization/gyro_odometer/package.xml @@ -0,0 +1,27 @@ + + + gyro_odometer + 0.1.0 + The gyro_odometer package as a ROS2 node + Yamato Ando + Apache License 2.0 + + ament_cmake_auto + + geometry_msgs + sensor_msgs + std_msgs + tf2 + tf2_geometry_msgs + tf2_ros + + rclcpp + rclcpp_components + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/gyro_odometer/src/gyro_odometer_core.cpp new file mode 100644 index 0000000000000..e4ab35dbace19 --- /dev/null +++ b/localization/gyro_odometer/src/gyro_odometer_core.cpp @@ -0,0 +1,145 @@ +// Copyright 2015-2019 Autoware Foundation +// +// 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 "gyro_odometer/gyro_odometer_core.hpp" + +#include + +#include +#include +#include + +GyroOdometer::GyroOdometer() +: Node("gyro_odometer"), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_), + output_frame_(declare_parameter("base_link", "base_link")) +{ + vehicle_twist_with_cov_sub_ = create_subscription( + "vehicle/twist_with_covariance", rclcpp::QoS{100}, + std::bind(&GyroOdometer::callbackTwistWithCovariance, this, std::placeholders::_1)); + + imu_sub_ = create_subscription( + "imu", rclcpp::QoS{100}, std::bind(&GyroOdometer::callbackImu, this, std::placeholders::_1)); + + twist_pub_ = create_publisher("twist", rclcpp::QoS{10}); + twist_with_covariance_pub_ = create_publisher( + "twist_with_covariance", rclcpp::QoS{10}); + + // TODO(YamatoAndo) createTimer +} + +GyroOdometer::~GyroOdometer() {} + +void GyroOdometer::callbackTwistWithCovariance( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_with_cov_msg_ptr) +{ + // TODO(YamatoAndo) trans from twist_with_cov_msg_ptr->header to base_frame + twist_with_cov_msg_ptr_ = twist_with_cov_msg_ptr; +} + +void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) +{ + if (!twist_with_cov_msg_ptr_) { + return; + } + + geometry_msgs::msg::TransformStamped::SharedPtr tf_base2imu_ptr = + std::make_shared(); + getTransform(output_frame_, imu_msg_ptr->header.frame_id, tf_base2imu_ptr); + + geometry_msgs::msg::Vector3Stamped angular_velocity; + angular_velocity.header = imu_msg_ptr->header; + angular_velocity.vector = imu_msg_ptr->angular_velocity; + + geometry_msgs::msg::Vector3Stamped transformed_angular_velocity; + transformed_angular_velocity.header = tf_base2imu_ptr->header; + + tf2::doTransform(angular_velocity, transformed_angular_velocity, *tf_base2imu_ptr); + + // clear imu yaw bias if vehicle is stopped + if ( + std::fabs(transformed_angular_velocity.vector.z) < 0.01 && + std::fabs(twist_with_cov_msg_ptr_->twist.twist.linear.x) < 0.01) { + transformed_angular_velocity.vector.z = 0.0; + } + + // TODO(YamatoAndo) move code + geometry_msgs::msg::TwistStamped twist; + twist.header.stamp = imu_msg_ptr->header.stamp; + twist.header.frame_id = output_frame_; + twist.twist.linear = twist_with_cov_msg_ptr_->twist.twist.linear; + twist.twist.angular.z = transformed_angular_velocity.vector.z; // TODO(YamatoAndo) yaw_rate only + twist_pub_->publish(twist); + + geometry_msgs::msg::TwistWithCovarianceStamped twist_with_covariance; + twist_with_covariance.header.stamp = imu_msg_ptr->header.stamp; + twist_with_covariance.header.frame_id = output_frame_; + twist_with_covariance.twist.twist.linear = twist_with_cov_msg_ptr_->twist.twist.linear; + twist_with_covariance.twist.twist.angular.z = + transformed_angular_velocity.vector.z; // TODO(YamatoAndo) yaw_rate only + + // NOTE + // linear.y, linear.z, angular.x, and angular.y are not measured values. + // Therefore, they should be assigned large variance values. + twist_with_covariance.twist.covariance[0] = twist_with_cov_msg_ptr_->twist.covariance[0]; + twist_with_covariance.twist.covariance[7] = 10000.0; + twist_with_covariance.twist.covariance[14] = 10000.0; + twist_with_covariance.twist.covariance[21] = 10000.0; + twist_with_covariance.twist.covariance[28] = 10000.0; + twist_with_covariance.twist.covariance[35] = imu_msg_ptr->angular_velocity_covariance[8]; + + twist_with_covariance_pub_->publish(twist_with_covariance); +} + +bool GyroOdometer::getTransform( + 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->get_clock()->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 = + tf_buffer_.lookupTransform(target_frame, source_frame, tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN(this->get_logger(), "%s", ex.what()); + RCLCPP_ERROR( + this->get_logger(), "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str()); + + transform_stamped_ptr->header.stamp = this->get_clock()->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; +} diff --git a/localization/gyro_odometer/src/gyro_odometer_node.cpp b/localization/gyro_odometer/src/gyro_odometer_node.cpp new file mode 100644 index 0000000000000..06fb094d208e9 --- /dev/null +++ b/localization/gyro_odometer/src/gyro_odometer_node.cpp @@ -0,0 +1,29 @@ +// Copyright 2015-2019 Autoware Foundation +// +// 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 "gyro_odometer/gyro_odometer_core.hpp" + +#include + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + + return 0; +} diff --git a/localization/pose2twist/CMakeLists.txt b/localization/pose2twist/CMakeLists.txt new file mode 100644 index 0000000000000..04c376174a83d --- /dev/null +++ b/localization/pose2twist/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.5) +project(pose2twist) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_add_executable(pose2twist + src/pose2twist_node.cpp + src/pose2twist_core.cpp +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch +) diff --git a/localization/pose2twist/README.md b/localization/pose2twist/README.md new file mode 100644 index 0000000000000..9cd5d04e3ca39 --- /dev/null +++ b/localization/pose2twist/README.md @@ -0,0 +1,32 @@ +# pose2twist + +## Purpose + +This `pose2twist` calculates the velocity from the input pose history. In addition to the computed twist, this node outputs the linear-x and angular-z components as a float message to simplify debugging. + +The `twist.linear.x` is calculated as `sqrt(dx * dx + dy * dy + dz * dz) / dt`, and the values in the `y` and `z` fields are zero. +The `twist.angular` is calculated as `d_roll / dt`, `d_pitch / dt` and `d_yaw / dt` for each field. + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ---- | ------------------------------- | ------------------------------------------------- | +| pose | geometry_msgs::msg::PoseStamped | pose source to used for the velocity calculation. | + +### Output + +| Name | Type | Description | +| --------- | ---------------------------------------- | --------------------------------------------- | +| twist | geometry_msgs::msg::TwistStamped | twist calculated from the input pose history. | +| linear_x | autoware_debug_msgs::msg::Float32Stamped | linear-x field of the output twist. | +| angular_z | autoware_debug_msgs::msg::Float32Stamped | angular-z field of the output twist. | + +## Parameters + +none. + +## Assumptions / Known limits + +none. diff --git a/localization/pose2twist/include/pose2twist/pose2twist_core.hpp b/localization/pose2twist/include/pose2twist/pose2twist_core.hpp new file mode 100644 index 0000000000000..23599fdced692 --- /dev/null +++ b/localization/pose2twist/include/pose2twist/pose2twist_core.hpp @@ -0,0 +1,40 @@ +// Copyright 2015-2019 Autoware Foundation +// +// 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 POSE2TWIST__POSE2TWIST_CORE_HPP_ +#define POSE2TWIST__POSE2TWIST_CORE_HPP_ + +#include + +#include +#include +#include + +class Pose2Twist : public rclcpp::Node +{ +public: + Pose2Twist(); + ~Pose2Twist() = default; + +private: + void callbackPose(geometry_msgs::msg::PoseStamped::SharedPtr pose_msg_ptr); + + rclcpp::Subscription::SharedPtr pose_sub_; + + rclcpp::Publisher::SharedPtr twist_pub_; + rclcpp::Publisher::SharedPtr linear_x_pub_; + rclcpp::Publisher::SharedPtr angular_z_pub_; +}; + +#endif // POSE2TWIST__POSE2TWIST_CORE_HPP_ diff --git a/localization/pose2twist/launch/pose2twist.launch.xml b/localization/pose2twist/launch/pose2twist.launch.xml new file mode 100644 index 0000000000000..15206447cf7a3 --- /dev/null +++ b/localization/pose2twist/launch/pose2twist.launch.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/localization/pose2twist/package.xml b/localization/pose2twist/package.xml new file mode 100644 index 0000000000000..26499488115f9 --- /dev/null +++ b/localization/pose2twist/package.xml @@ -0,0 +1,22 @@ + + + pose2twist + 0.1.0 + The pose2twist package + Yamato Ando + Apache License 2.0 + + ament_cmake + + autoware_debug_msgs + geometry_msgs + rclcpp + tf2_geometry_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/localization/pose2twist/src/pose2twist_core.cpp b/localization/pose2twist/src/pose2twist_core.cpp new file mode 100644 index 0000000000000..d3845334161a8 --- /dev/null +++ b/localization/pose2twist/src/pose2twist_core.cpp @@ -0,0 +1,128 @@ +// Copyright 2015-2019 Autoware Foundation +// +// 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 "pose2twist/pose2twist_core.hpp" + +#include + +#include +#include +#include + +Pose2Twist::Pose2Twist() : Node("pose2twist_core") +{ + using std::placeholders::_1; + + static constexpr std::size_t queue_size = 1; + rclcpp::QoS durable_qos(queue_size); + durable_qos.transient_local(); + + pose_sub_ = create_subscription( + "pose", queue_size, std::bind(&Pose2Twist::callbackPose, this, _1)); + + twist_pub_ = create_publisher("twist", durable_qos); + linear_x_pub_ = + create_publisher("linear_x", durable_qos); + angular_z_pub_ = + create_publisher("angular_z", durable_qos); +} + +double calcDiffForRadian(const double lhs_rad, const double rhs_rad) +{ + double diff_rad = lhs_rad - rhs_rad; + if (diff_rad > M_PI) { + diff_rad = diff_rad - 2 * M_PI; + } else if (diff_rad < -M_PI) { + diff_rad = diff_rad + 2 * M_PI; + } + return diff_rad; +} + +// x: roll, y: pitch, z: yaw +geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::Pose & pose) +{ + geometry_msgs::msg::Vector3 rpy; + tf2::Quaternion q(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w); + tf2::Matrix3x3(q).getRPY(rpy.x, rpy.y, rpy.z); + return rpy; +} + +geometry_msgs::msg::Vector3 getRPY(geometry_msgs::msg::PoseStamped::SharedPtr pose) +{ + return getRPY(pose->pose); +} + +geometry_msgs::msg::TwistStamped calcTwist( + geometry_msgs::msg::PoseStamped::SharedPtr pose_a, + geometry_msgs::msg::PoseStamped::SharedPtr pose_b) +{ + const double dt = + (rclcpp::Time(pose_b->header.stamp) - rclcpp::Time(pose_a->header.stamp)).seconds(); + + if (dt == 0) { + geometry_msgs::msg::TwistStamped twist; + twist.header = pose_b->header; + return twist; + } + + const auto pose_a_rpy = getRPY(pose_a); + const auto pose_b_rpy = getRPY(pose_b); + + geometry_msgs::msg::Vector3 diff_xyz; + geometry_msgs::msg::Vector3 diff_rpy; + + diff_xyz.x = pose_b->pose.position.x - pose_a->pose.position.x; + diff_xyz.y = pose_b->pose.position.y - pose_a->pose.position.y; + diff_xyz.z = pose_b->pose.position.z - pose_a->pose.position.z; + diff_rpy.x = calcDiffForRadian(pose_b_rpy.x, pose_a_rpy.x); + diff_rpy.y = calcDiffForRadian(pose_b_rpy.y, pose_a_rpy.y); + diff_rpy.z = calcDiffForRadian(pose_b_rpy.z, pose_a_rpy.z); + + geometry_msgs::msg::TwistStamped twist; + twist.header = pose_b->header; + twist.twist.linear.x = + std::sqrt(std::pow(diff_xyz.x, 2.0) + std::pow(diff_xyz.y, 2.0) + std::pow(diff_xyz.z, 2.0)) / + dt; + twist.twist.linear.y = 0; + twist.twist.linear.z = 0; + twist.twist.angular.x = diff_rpy.x / dt; + twist.twist.angular.y = diff_rpy.y / dt; + twist.twist.angular.z = diff_rpy.z / dt; + + return twist; +} + +void Pose2Twist::callbackPose(geometry_msgs::msg::PoseStamped::SharedPtr pose_msg_ptr) +{ + // TODO(YamatoAndo) check time stamp diff + // TODO(YamatoAndo) check suddenly move + // TODO(YamatoAndo) apply low pass filter + + geometry_msgs::msg::PoseStamped::SharedPtr current_pose_msg = pose_msg_ptr; + static geometry_msgs::msg::PoseStamped::SharedPtr prev_pose_msg = current_pose_msg; + geometry_msgs::msg::TwistStamped twist_msg = calcTwist(prev_pose_msg, current_pose_msg); + prev_pose_msg = current_pose_msg; + twist_msg.header.frame_id = "base_link"; + twist_pub_->publish(twist_msg); + + autoware_debug_msgs::msg::Float32Stamped linear_x_msg; + linear_x_msg.stamp = this->now(); + linear_x_msg.data = twist_msg.twist.linear.x; + linear_x_pub_->publish(linear_x_msg); + + autoware_debug_msgs::msg::Float32Stamped angular_z_msg; + angular_z_msg.stamp = this->now(); + angular_z_msg.data = twist_msg.twist.angular.z; + angular_z_pub_->publish(angular_z_msg); +} diff --git a/localization/pose2twist/src/pose2twist_node.cpp b/localization/pose2twist/src/pose2twist_node.cpp new file mode 100644 index 0000000000000..81f98461ecffd --- /dev/null +++ b/localization/pose2twist/src/pose2twist_node.cpp @@ -0,0 +1,29 @@ +// Copyright 2015-2019 Autoware Foundation +// +// 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 "pose2twist/pose2twist_core.hpp" + +#include + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + + return 0; +} diff --git a/localization/vehicle_velocity_converter/CMakeLists.txt b/localization/vehicle_velocity_converter/CMakeLists.txt new file mode 100644 index 0000000000000..e2c108c244538 --- /dev/null +++ b/localization/vehicle_velocity_converter/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 3.5) +project(vehicle_velocity_converter) + +### Compile options +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + # add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wno-unused-parameter) +endif() + +# find dependencies +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_add_executable(vehicle_velocity_converter + src/vehicle_velocity_converter_node.cpp + src/vehicle_velocity_converter.cpp +) +ament_target_dependencies(vehicle_velocity_converter) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/localization/vehicle_velocity_converter/README.md b/localization/vehicle_velocity_converter/README.md new file mode 100644 index 0000000000000..561e052439771 --- /dev/null +++ b/localization/vehicle_velocity_converter/README.md @@ -0,0 +1,26 @@ +# vehicle_velocity_converter + +## Purpose + +This package is converted autoware_auto_vehicle_msgs::msg::VehicleReport message to geometry_msgs::msg::TwistWithCovarianceStamped for gyro odometer node. + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ----------------- | ------------------------------------------------ | ---------------- | +| `velocity_status` | `autoware_auto_vehicle_msgs::msg::VehicleReport` | vehicle velocity | + +### Output + +| Name | Type | Description | +| ----------------------- | ------------------------------------------------ | -------------------------------------------------- | +| `twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | twist with covariance converted from VehicleReport | + +## Parameters + +| Name | Type | Description | +| ------------ | ------ | ----------------------------------------- | +| `frame_id` | string | frame id for output message | +| `covariance` | double | set covariance value to the twist message | diff --git a/localization/vehicle_velocity_converter/config/vehicle_velocity_converter.yaml b/localization/vehicle_velocity_converter/config/vehicle_velocity_converter.yaml new file mode 100644 index 0000000000000..8d495b2d267b6 --- /dev/null +++ b/localization/vehicle_velocity_converter/config/vehicle_velocity_converter.yaml @@ -0,0 +1,42 @@ +/**: + ros__parameters: + frame_id: base_link + twist_covariance: + [ + 0.04, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 10000.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 10000.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 10000.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 10000.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.01, + ] diff --git a/localization/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp b/localization/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp new file mode 100644 index 0000000000000..2dfac343b0a86 --- /dev/null +++ b/localization/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp @@ -0,0 +1,47 @@ +// Copyright 2021 TierIV +// +// 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 _VEHICLE_VELOCITY_CONVERTER_HPP_ +#define _VEHICLE_VELOCITY_CONVERTER_HPP_ + +#include + +#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" +#include +#include + +#include +#include +#include + +class VehicleVelocityConverter : public rclcpp::Node +{ +public: + VehicleVelocityConverter(); + ~VehicleVelocityConverter() = default; + +private: + void callbackVelocityReport(const autoware_auto_vehicle_msgs::msg::VelocityReport::SharedPtr msg); + + rclcpp::Subscription::SharedPtr + vehicle_report_sub_; + + rclcpp::Publisher::SharedPtr + twist_with_covariance_pub_; + + std::string frame_id_; + std::array twist_covariance_; +}; + +#endif diff --git a/localization/vehicle_velocity_converter/launch/vehicle_velocity_converter.launch.xml b/localization/vehicle_velocity_converter/launch/vehicle_velocity_converter.launch.xml new file mode 100644 index 0000000000000..7bae938135a18 --- /dev/null +++ b/localization/vehicle_velocity_converter/launch/vehicle_velocity_converter.launch.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/localization/vehicle_velocity_converter/package.xml b/localization/vehicle_velocity_converter/package.xml new file mode 100644 index 0000000000000..afc01f892dd6c --- /dev/null +++ b/localization/vehicle_velocity_converter/package.xml @@ -0,0 +1,21 @@ + + + + vehicle_velocity_converter + 0.0.0 + The vehicle_velocity_converter package + Ryu Yamamoto + Apache License 2.0 + + ament_cmake_auto + + autoware_auto_vehicle_msgs + geometry_msgs + rclcpp + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/localization/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp b/localization/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp new file mode 100644 index 0000000000000..997b98a6bbcf4 --- /dev/null +++ b/localization/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp @@ -0,0 +1,51 @@ +// Copyright 2021 TierIV +// +// 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 "vehicle_velocity_converter/vehicle_velocity_converter.hpp" + +VehicleVelocityConverter::VehicleVelocityConverter() : Node("vehicle_velocity_converter") +{ + // set covariance value for twist with covariance msg + declare_parameter("twist_covariance"); + std::vector covariance = get_parameter("twist_covariance").as_double_array(); + for (std::size_t i = 0; i < covariance.size(); ++i) { + twist_covariance_[i] = covariance[i]; + } + frame_id_ = declare_parameter("frame_id", "base_link"); + + vehicle_report_sub_ = create_subscription( + "velocity_status", rclcpp::QoS{100}, + std::bind(&VehicleVelocityConverter::callbackVelocityReport, this, std::placeholders::_1)); + + twist_with_covariance_pub_ = create_publisher( + "twist_with_covariance", rclcpp::QoS{10}); +} + +void VehicleVelocityConverter::callbackVelocityReport( + const autoware_auto_vehicle_msgs::msg::VelocityReport::SharedPtr msg) +{ + if (msg->header.frame_id != frame_id_) { + RCLCPP_WARN(get_logger(), "frame_id is not base_link."); + } + + // set twist with covariance msg from vehicle report msg + geometry_msgs::msg::TwistWithCovarianceStamped twist_with_covariance_msg; + twist_with_covariance_msg.header = msg->header; + twist_with_covariance_msg.twist.twist.linear.x = msg->longitudinal_velocity; + twist_with_covariance_msg.twist.twist.linear.y = msg->lateral_velocity; + twist_with_covariance_msg.twist.twist.angular.z = msg->heading_rate; + twist_with_covariance_msg.twist.covariance = twist_covariance_; + + twist_with_covariance_pub_->publish(twist_with_covariance_msg); +} diff --git a/localization/vehicle_velocity_converter/src/vehicle_velocity_converter_node.cpp b/localization/vehicle_velocity_converter/src/vehicle_velocity_converter_node.cpp new file mode 100644 index 0000000000000..cee25f6e6c62a --- /dev/null +++ b/localization/vehicle_velocity_converter/src/vehicle_velocity_converter_node.cpp @@ -0,0 +1,25 @@ +// Copyright 2021 TierIV +// +// 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 + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + + return 0; +}