diff --git a/perception/front_vehicle_velocity_estimator/CMakeLists.txt b/perception/front_vehicle_velocity_estimator/CMakeLists.txt deleted file mode 100644 index 98094d029a316..0000000000000 --- a/perception/front_vehicle_velocity_estimator/CMakeLists.txt +++ /dev/null @@ -1,30 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(front_vehicle_velocity_estimator) - -# Dependencies -find_package(autoware_cmake REQUIRED) -autoware_package() - -# Targets -ament_auto_add_library(front_vehicle_velocity_estimator_node_component SHARED - src/front_vehicle_velocity_estimator_node/front_vehicle_velocity_estimator_node.cpp - src/front_vehicle_velocity_estimator_node/front_vehicle_velocity_estimator.cpp -) - -rclcpp_components_register_node(front_vehicle_velocity_estimator_node_component - PLUGIN "front_vehicle_velocity_estimator::FrontVehicleVelocityEstimatorNode" - EXECUTABLE front_vehicle_velocity_estimator_node -) - -# Tests -if(BUILD_TESTING) - list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -# Package -ament_auto_package( - INSTALL_TO_SHARE - launch -) diff --git a/perception/front_vehicle_velocity_estimator/README.md b/perception/front_vehicle_velocity_estimator/README.md deleted file mode 100644 index 2225880db101c..0000000000000 --- a/perception/front_vehicle_velocity_estimator/README.md +++ /dev/null @@ -1,45 +0,0 @@ -# front_vehicle_velocity_estimator - -This package contains a front vehicle velocity estimation for offline perception module analysis. -This package can: - -- Attach velocity to 3D detections from velocity estimation with LiDAR pointcloud. - -## Algorithm - -- Processing flow - 1. Choose front vehicle from front area objects. - 2. Choose nearest neighbor point within front vehicle. - 3. Estimate velocity of front vehicle by using the differentiated value from time series of nearest neighbor point positions. - 4. Compensate ego vehicle twist - -## Input - -| Name | Type | Description | -| -------------------- | ---------------------------------------------------- | -------------------- | -| `~/input/objects` | autoware_auto_perception_msgs/msg/DetectedObject.msg | 3D detected objects. | -| `~/input/pointcloud` | sensor_msgs/msg/PointCloud2.msg | LiDAR pointcloud. | -| `~/input/odometry` | nav_msgs::msg::Odometry.msg | Odometry data. | - -## Output - -| Name | Type | Description | -| ------------------------------------- | ----------------------------------------------------- | --------------------------------------------- | -| `~/output/objects` | autoware_auto_perception_msgs/msg/DetectedObjects.msg | 3D detected object with twist. | -| `~/debug/nearest_neighbor_pointcloud` | sensor_msgs/msg/PointCloud2.msg | The pointcloud msg of nearest neighbor point. | - -## Node parameter - -| Name | Type | Description | Default value | -| :--------------- | :----- | :-------------------- | :------------ | -| `update_rate_hz` | double | The update rate [hz]. | 10.0 | - -## Core parameter - -| Name | Type | Description | Default value | -| :---------------------------- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| `moving_average_num` | int | The moving average number for velocity estimation. | 1 | -| `threshold_pointcloud_z_high` | float | The threshold for z position value of point when choosing nearest neighbor point within front vehicle [m]. If z > `threshold_pointcloud_z_high`, the point is considered to noise. | 1.0f | -| `threshold_pointcloud_z_low` | float | The threshold for z position value of point when choosing nearest neighbor point within front vehicle [m]. If z < `threshold_pointcloud_z_low`, the point is considered to noise like ground. | 0.6f | -| `threshold_relative_velocity` | double | The threshold for min and max of estimated relative velocity ($v_{re}$) [m/s]. If $v_{re}$ < - `threshold_relative_velocity` , then $v_{re}$ = - `threshold_relative_velocity`. If $v_{re}$ > `threshold_relative_velocity`, then $v_{re}$ = `threshold_relative_velocity`. | 10.0 | -| `threshold_absolute_velocity` | double | The threshold for max of estimated absolute velocity ($v_{ae}$) [m/s]. If $v_{ae}$ > `threshold_absolute_velocity`, then $v_{ae}$ = `threshold_absolute_velocity`. | 20.0 | diff --git a/perception/front_vehicle_velocity_estimator/include/front_vehicle_velocity_estimator/front_vehicle_velocity_estimator.hpp b/perception/front_vehicle_velocity_estimator/include/front_vehicle_velocity_estimator/front_vehicle_velocity_estimator.hpp deleted file mode 100644 index 01f738fc6beaa..0000000000000 --- a/perception/front_vehicle_velocity_estimator/include/front_vehicle_velocity_estimator/front_vehicle_velocity_estimator.hpp +++ /dev/null @@ -1,107 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef FRONT_VEHICLE_VELOCITY_ESTIMATOR__FRONT_VEHICLE_VELOCITY_ESTIMATOR_HPP_ -#define FRONT_VEHICLE_VELOCITY_ESTIMATOR__FRONT_VEHICLE_VELOCITY_ESTIMATOR_HPP_ - -#include "pcl/point_cloud.h" -#include "pcl/point_types.h" -#include "pcl_conversions/pcl_conversions.h" -#include "rclcpp/logger.hpp" -#include "tier4_autoware_utils/geometry/boost_geometry.hpp" - -#include "autoware_auto_perception_msgs/msg/detected_objects.hpp" -#include "nav_msgs/msg/odometry.hpp" -#include "sensor_msgs/msg/point_cloud2.hpp" - -#include -#include -#include - -namespace front_vehicle_velocity_estimator -{ -using autoware_auto_perception_msgs::msg::DetectedObject; -using autoware_auto_perception_msgs::msg::DetectedObjectKinematics; -using autoware_auto_perception_msgs::msg::DetectedObjects; -using autoware_auto_perception_msgs::msg::ObjectClassification; -using nav_msgs::msg::Odometry; -using sensor_msgs::msg::PointCloud2; -using tier4_autoware_utils::LinearRing2d; -using tier4_autoware_utils::Point2d; - -class FrontVehicleVelocityEstimator -{ -public: - explicit FrontVehicleVelocityEstimator(const rclcpp::Logger & logger) : logger_(logger) {} - - struct Input - { - PointCloud2::ConstSharedPtr pointcloud{}; - DetectedObjects::ConstSharedPtr objects{}; - Odometry::ConstSharedPtr odometry{}; - }; - - struct Output - { - DetectedObjects objects{}; - PointCloud2 nearest_neighbor_pointcloud{}; - }; - - struct Param - { - int moving_average_num{}; - float threshold_pointcloud_z_high{}; - float threshold_pointcloud_z_low{}; - double threshold_relative_velocity{}; - double threshold_absolute_velocity{}; - }; - - void setParam(const Param & param) { param_ = param; } - Output update(const Input & input); - -private: - struct ObjectsWithFrontVehicle - { - DetectedObjects::SharedPtr objects_without_front_vehicle{}; - DetectedObject front_vehicle{}; - bool is_front_vehicle = false; - }; - - rclcpp::Logger logger_; - - // Buffer data - Param param_{}; - std::deque velocity_queue_{}; - rclcpp::Time prev_time_{}; - pcl::PointXYZ prev_point_{}; - - // Function - LinearRing2d createBoxArea(const Point2d size); - LinearRing2d createObjectArea(const DetectedObject & object); - ObjectsWithFrontVehicle filterFrontVehicle( - DetectedObjects::ConstSharedPtr objects, const LinearRing2d & front_area); - pcl::PointXYZ getNearestNeighborPoint( - const DetectedObject & object, PointCloud2::ConstSharedPtr pointcloud, - const Point2d & front_size); - double estimateRelativeVelocity(const pcl::PointXYZ & point, const rclcpp::Time & header_time); - double estimateAbsoluteVelocity( - const double relative_velocity, Odometry::ConstSharedPtr odometry); - bool isFrontVehicle(const DetectedObject & object, const LinearRing2d & front_area); - bool isWithinVehicle( - const DetectedObject & object, const pcl::PointXYZ & point, const Point2d & front_size); -}; - -} // namespace front_vehicle_velocity_estimator - -#endif // FRONT_VEHICLE_VELOCITY_ESTIMATOR__FRONT_VEHICLE_VELOCITY_ESTIMATOR_HPP_ diff --git a/perception/front_vehicle_velocity_estimator/include/front_vehicle_velocity_estimator/front_vehicle_velocity_estimator_node.hpp b/perception/front_vehicle_velocity_estimator/include/front_vehicle_velocity_estimator/front_vehicle_velocity_estimator_node.hpp deleted file mode 100644 index 8b42b61606677..0000000000000 --- a/perception/front_vehicle_velocity_estimator/include/front_vehicle_velocity_estimator/front_vehicle_velocity_estimator_node.hpp +++ /dev/null @@ -1,98 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef FRONT_VEHICLE_VELOCITY_ESTIMATOR__FRONT_VEHICLE_VELOCITY_ESTIMATOR_NODE_HPP_ -#define FRONT_VEHICLE_VELOCITY_ESTIMATOR__FRONT_VEHICLE_VELOCITY_ESTIMATOR_NODE_HPP_ - -#include "front_vehicle_velocity_estimator/front_vehicle_velocity_estimator.hpp" -#include "rclcpp/rclcpp.hpp" - -#include "autoware_auto_perception_msgs/msg/detected_objects.hpp" -#include "nav_msgs/msg/odometry.hpp" -#include "sensor_msgs/msg/point_cloud2.hpp" - -#include -#include -#include - -#include -#include -#include -#include - -namespace front_vehicle_velocity_estimator -{ -using autoware_auto_perception_msgs::msg::DetectedObject; -using autoware_auto_perception_msgs::msg::DetectedObjects; -using nav_msgs::msg::Odometry; -using sensor_msgs::msg::PointCloud2; -using tier4_autoware_utils::LinearRing2d; -using tier4_autoware_utils::Point2d; - -class FrontVehicleVelocityEstimatorNode : public rclcpp::Node -{ -public: - explicit FrontVehicleVelocityEstimatorNode(const rclcpp::NodeOptions & node_options); - - struct NodeParam - { - double update_rate_hz{}; - }; - -private: - // Subscriber - message_filters::Subscriber sub_pointcloud_{}; - message_filters::Subscriber sub_objects_{}; - message_filters::Subscriber sub_odometry_{}; - - using SyncPolicy = - message_filters::sync_policies::ApproximateTime; - using Sync = message_filters::Synchronizer; - typename std::shared_ptr sync_ptr_; - - // Callback - void onData( - const PointCloud2::ConstSharedPtr pointcloud_msg, - const DetectedObjects::ConstSharedPtr object_msg, const Odometry::ConstSharedPtr odometry_msg); - - // Data Buffer - PointCloud2::ConstSharedPtr pointcloud_data_{}; - DetectedObjects::ConstSharedPtr objects_data_{}; - Odometry::ConstSharedPtr odometry_data_{}; - - // Publisher - rclcpp::Publisher::SharedPtr pub_objects_{}; - rclcpp::Publisher::SharedPtr pub_nearest_neighbor_pointcloud_{}; - - // Timer - bool isDataReady(); - - // Parameter Server - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - rcl_interfaces::msg::SetParametersResult onSetParam( - const std::vector & params); - - // Parameter - NodeParam node_param_{}; - - // Core - FrontVehicleVelocityEstimator::Input input_{}; - FrontVehicleVelocityEstimator::Output output_{}; - FrontVehicleVelocityEstimator::Param core_param_{}; - std::unique_ptr front_vehicle_velocity_estimator_{}; -}; - -} // namespace front_vehicle_velocity_estimator - -#endif // FRONT_VEHICLE_VELOCITY_ESTIMATOR__FRONT_VEHICLE_VELOCITY_ESTIMATOR_NODE_HPP_ diff --git a/perception/front_vehicle_velocity_estimator/launch/front_vehicle_velocity_estimator.launch.xml b/perception/front_vehicle_velocity_estimator/launch/front_vehicle_velocity_estimator.launch.xml deleted file mode 100644 index 518791821a0ac..0000000000000 --- a/perception/front_vehicle_velocity_estimator/launch/front_vehicle_velocity_estimator.launch.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/perception/front_vehicle_velocity_estimator/package.xml b/perception/front_vehicle_velocity_estimator/package.xml deleted file mode 100644 index 3b1af8cf544ac..0000000000000 --- a/perception/front_vehicle_velocity_estimator/package.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - - front_vehicle_velocity_estimator - 0.1.0 - front_vehicle_velocity_estimator - Satoshi Tanaka - Shunsuke Miura - Apache License 2.0 - Satoshi Tanaka - - ament_cmake_auto - autoware_cmake - - autoware_auto_perception_msgs - geometry_msgs - message_filters - nav_msgs - pcl_conversions - pcl_ros - rclcpp - rclcpp_components - sensor_msgs - tier4_autoware_utils - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/perception/front_vehicle_velocity_estimator/src/front_vehicle_velocity_estimator_node/front_vehicle_velocity_estimator.cpp b/perception/front_vehicle_velocity_estimator/src/front_vehicle_velocity_estimator_node/front_vehicle_velocity_estimator.cpp deleted file mode 100644 index 6f59823615c04..0000000000000 --- a/perception/front_vehicle_velocity_estimator/src/front_vehicle_velocity_estimator_node/front_vehicle_velocity_estimator.cpp +++ /dev/null @@ -1,248 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "front_vehicle_velocity_estimator/front_vehicle_velocity_estimator.hpp" - -#include "tier4_autoware_utils/geometry/geometry.hpp" - -#include - -#include -#include -#include - -namespace front_vehicle_velocity_estimator -{ -FrontVehicleVelocityEstimator::Output FrontVehicleVelocityEstimator::update( - const FrontVehicleVelocityEstimator::Input & input) -{ - Output output_{}; - ObjectsWithFrontVehicle objects_with_front_vehicle{}; - - // Filter front vehicle - const Point2d front_size{100.0, 3.0}; - LinearRing2d front_area = createBoxArea(front_size); - objects_with_front_vehicle = filterFrontVehicle(input.objects, front_area); - - // Get nearest neighbor pointcloud - pcl::PointXYZ nearest_neighbor_point = - getNearestNeighborPoint(objects_with_front_vehicle.front_vehicle, input.pointcloud, front_size); - - // Set objects output - output_.objects = *(objects_with_front_vehicle.objects_without_front_vehicle); - - // If there is no front vehicle, return output - if (!objects_with_front_vehicle.is_front_vehicle) { - return output_; - } - - // Estimate relative velocity - double now_relative_velocity = - estimateRelativeVelocity(nearest_neighbor_point, input.pointcloud->header.stamp); - - // Estimate absolute velocity - double now_absolute_velocity = estimateAbsoluteVelocity(now_relative_velocity, input.odometry); - - // Validate velocity - if ( - !isfinite(now_relative_velocity) || - now_relative_velocity > param_.threshold_relative_velocity || - now_relative_velocity < -param_.threshold_relative_velocity) { - output_.objects.objects.emplace_back(objects_with_front_vehicle.front_vehicle); - return output_; - } - now_absolute_velocity = std::min(param_.threshold_absolute_velocity, now_absolute_velocity); - - // Set queue of nearest_neighbor_point - if (static_cast(velocity_queue_.size()) >= param_.moving_average_num) { - velocity_queue_.pop_front(); - } - - // Estimate average velocity - velocity_queue_.push_back(now_absolute_velocity); - double velocity = std::accumulate(std::begin(velocity_queue_), std::end(velocity_queue_), 0.0) / - velocity_queue_.size(); - // RCLCPP_INFO( - // rclcpp::get_logger("front_vehicle_velocity_estimator"), "x=%f, v=%f km/h, v_a=%f km/h", - // objects_with_front_vehicle.front_vehicle.kinematics.pose_with_covariance.pose.position.x, - // now_absolute_velocity * 3.6, velocity * 3.6); - - // Set kinematics information for front vehicle - objects_with_front_vehicle.front_vehicle.kinematics.has_twist = true; - objects_with_front_vehicle.front_vehicle.kinematics.twist_with_covariance.twist.linear.x = - velocity; - objects_with_front_vehicle.front_vehicle.kinematics.has_twist_covariance = true; - objects_with_front_vehicle.front_vehicle.kinematics.twist_with_covariance.covariance.at(0) = 1.0; - output_.objects.objects.emplace_back(objects_with_front_vehicle.front_vehicle); - - // Set previous data buffer - prev_time_ = input.pointcloud->header.stamp; - prev_point_ = nearest_neighbor_point; - - // Set nearest_neighbor_pointcloud output for debug - pcl::PointCloud output_pointcloud; - output_pointcloud.points.push_back(nearest_neighbor_point); - pcl::toROSMsg(output_pointcloud, output_.nearest_neighbor_pointcloud); - output_.nearest_neighbor_pointcloud.header = input.pointcloud->header; - - return output_; -} - -// Create front area x = 0 ~ size.x, y = -size.y ~ size.y -LinearRing2d FrontVehicleVelocityEstimator::createBoxArea(const Point2d size) -{ - LinearRing2d box{}; - box.push_back(Point2d{0.0, size.y()}); - box.push_back(Point2d{size.x(), size.y()}); - box.push_back(Point2d{size.x(), -size.y()}); - box.push_back(Point2d{0.0, -size.y()}); - box.push_back(Point2d{0.0, size.y()}); - return box; -} - -// Create object area -LinearRing2d FrontVehicleVelocityEstimator::createObjectArea(const DetectedObject & object) -{ - Point2d object_size{object.shape.dimensions.x, object.shape.dimensions.y}; - const double x_front = object_size.x() / 2.0; - const double x_rear = -object_size.x() / 2.0; - const double y_left = object_size.y() / 2.0; - const double y_right = -object_size.y() / 2.0; - - LinearRing2d box{}; - box.push_back(Point2d{x_front, y_left}); - box.push_back(Point2d{x_front, y_right}); - box.push_back(Point2d{x_rear, y_right}); - box.push_back(Point2d{x_rear, y_left}); - box.push_back(Point2d{x_front, y_left}); - - LinearRing2d transformed_box = tier4_autoware_utils::transformVector( - box, tier4_autoware_utils::pose2transform(object.kinematics.pose_with_covariance.pose)); - - return transformed_box; -} - -// Filter for a front vehicle. -FrontVehicleVelocityEstimator::ObjectsWithFrontVehicle -FrontVehicleVelocityEstimator::filterFrontVehicle( - DetectedObjects::ConstSharedPtr objects, const LinearRing2d & front_area) -{ - // Initialize output - ObjectsWithFrontVehicle output{}; - output.is_front_vehicle = false; - - DetectedObjects output_objects_{}; - output_objects_.header = objects->header; - - for (const auto & object : objects->objects) { - if (!isFrontVehicle(object, front_area)) { - output_objects_.objects.emplace_back(object); - } else if (!output.is_front_vehicle) { - output.front_vehicle = object; - output.is_front_vehicle = true; - } else { - auto front_vehicle_position = - output.front_vehicle.kinematics.pose_with_covariance.pose.position; - if (object.kinematics.pose_with_covariance.pose.position.x < front_vehicle_position.x) { - output_objects_.objects.emplace_back(output.front_vehicle); - output.front_vehicle = object; - } else { - output_objects_.objects.emplace_back(object); - } - } - } - output.objects_without_front_vehicle = std::make_shared(output_objects_); - return output; -} - -pcl::PointXYZ FrontVehicleVelocityEstimator::getNearestNeighborPoint( - const DetectedObject & object, PointCloud2::ConstSharedPtr pointcloud, const Point2d & front_size) -{ - pcl::PointCloud::Ptr pcl_msg(new pcl::PointCloud); - pcl::fromROSMsg(*pointcloud, *pcl_msg); - - // Initialize - pcl::PointXYZ nearest_neighbor_point; - bool is_initialized = false; - - for (const auto & point : *pcl_msg) { - if (isWithinVehicle(object, point, front_size)) { - if (!is_initialized) { - nearest_neighbor_point = point; - } else if (point.x < nearest_neighbor_point.x) { - nearest_neighbor_point = point; - } - } - } - return nearest_neighbor_point; -} - -bool FrontVehicleVelocityEstimator::isWithinVehicle( - const DetectedObject & object, const pcl::PointXYZ & point, const Point2d & front_size) -{ - LinearRing2d object_ring_2d = createObjectArea(object); - Point2d point_{point.x, point.y}; - if (point.z < param_.threshold_pointcloud_z_low) { - return false; - } else if (point.z > param_.threshold_pointcloud_z_high) { - return false; - } else if (point.x < 0) { - return false; - } else if (point.x > front_size.x()) { - return false; - } else if (point.y < -front_size.y()) { - return false; - } else if (point.y > front_size.y()) { - return false; - } else if (!boost::geometry::within(point_, object_ring_2d)) { - return false; - } - return true; -} - -double FrontVehicleVelocityEstimator::estimateRelativeVelocity( - const pcl::PointXYZ & point, const rclcpp::Time & header_time) -{ - if (velocity_queue_.size() == 0) { - return 0.0; - } - const double dt = (header_time - prev_time_).seconds(); - const double relative_velocity = (point.x - prev_point_.x) / dt; - return relative_velocity; -} - -double FrontVehicleVelocityEstimator::estimateAbsoluteVelocity( - const double relative_velocity, Odometry::ConstSharedPtr odometry) -{ - const double velocity = relative_velocity + odometry->twist.twist.linear.x; - return velocity; -} - -bool FrontVehicleVelocityEstimator::isFrontVehicle( - const DetectedObject & object, const LinearRing2d & front_area) -{ - auto position = object.kinematics.pose_with_covariance.pose.position; - auto label = object.classification.at(0).label; - Point2d object_point{position.x, position.y}; - if ( - !(label == ObjectClassification::UNKNOWN) && !(label == ObjectClassification::PEDESTRIAN) && - !(label == ObjectClassification::BICYCLE) && !(label == ObjectClassification::MOTORCYCLE) && - boost::geometry::within(object_point, front_area)) { - return true; - } else { - return false; - } -} - -} // namespace front_vehicle_velocity_estimator diff --git a/perception/front_vehicle_velocity_estimator/src/front_vehicle_velocity_estimator_node/front_vehicle_velocity_estimator_node.cpp b/perception/front_vehicle_velocity_estimator/src/front_vehicle_velocity_estimator_node/front_vehicle_velocity_estimator_node.cpp deleted file mode 100644 index d79164bf54e0d..0000000000000 --- a/perception/front_vehicle_velocity_estimator/src/front_vehicle_velocity_estimator_node/front_vehicle_velocity_estimator_node.cpp +++ /dev/null @@ -1,185 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "front_vehicle_velocity_estimator/front_vehicle_velocity_estimator_node.hpp" - -#include "rclcpp/rclcpp.hpp" - -#include -#include -#include - -using namespace std::literals; -using std::chrono::duration; -using std::chrono::duration_cast; -using std::chrono::nanoseconds; - -namespace -{ -template -bool update_param( - const std::vector & params, const std::string & name, T & value) -{ - const auto itr = std::find_if( - params.cbegin(), params.cend(), - [&name](const rclcpp::Parameter & p) { return p.get_name() == name; }); - - // Not found - if (itr == params.cend()) { - return false; - } - - value = itr->template get_value(); - return true; -} -} // namespace - -namespace front_vehicle_velocity_estimator -{ - -FrontVehicleVelocityEstimatorNode::FrontVehicleVelocityEstimatorNode( - const rclcpp::NodeOptions & node_options) -: Node("front_vehicle_velocity_estimator", node_options) -{ - // Parameter Server - set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&FrontVehicleVelocityEstimatorNode::onSetParam, this, std::placeholders::_1)); - - // Node Parameter - node_param_.update_rate_hz = declare_parameter("node_params.update_rate_hz", 10.0); - - // Core Parameter - core_param_.moving_average_num = declare_parameter("core_params.moving_average_num", 1); - core_param_.threshold_pointcloud_z_high = - declare_parameter("core_params.threshold_pointcloud_z_high", 1.0f); - core_param_.threshold_pointcloud_z_low = - declare_parameter("core_params.threshold_pointcloud_z_low", 0.6f); - core_param_.threshold_relative_velocity = - declare_parameter("core_params.threshold_relative_velocity", 10.0); - core_param_.threshold_absolute_velocity = - declare_parameter("core_params.threshold_absolute_velocity", 20.0); - - // Core - front_vehicle_velocity_estimator_ = std::make_unique(get_logger()); - front_vehicle_velocity_estimator_->setParam(core_param_); - - // Subscriber - sub_pointcloud_.subscribe( - this, "~/input/pointcloud", rclcpp::SensorDataQoS().get_rmw_qos_profile()); - sub_objects_.subscribe(this, "~/input/objects", rclcpp::QoS{1}.get_rmw_qos_profile()); - sub_odometry_.subscribe(this, "~/input/odometry", rclcpp::QoS{1}.get_rmw_qos_profile()); - - using std::placeholders::_1; - using std::placeholders::_2; - using std::placeholders::_3; - sync_ptr_ = std::make_shared(SyncPolicy(20), sub_pointcloud_, sub_objects_, sub_odometry_); - sync_ptr_->registerCallback( - std::bind(&FrontVehicleVelocityEstimatorNode::onData, this, _1, _2, _3)); - - // Publisher - pub_objects_ = create_publisher("~/output/objects", 1); - pub_nearest_neighbor_pointcloud_ = - create_publisher("~/debug/nearest_neighbor_pointcloud", 1); -} - -void FrontVehicleVelocityEstimatorNode::onData( - const PointCloud2::ConstSharedPtr pointcloud_msg, - const DetectedObjects::ConstSharedPtr object_msg, const Odometry::ConstSharedPtr odometry_msg) -{ - pointcloud_data_ = pointcloud_msg; - objects_data_ = object_msg; - odometry_data_ = odometry_msg; - - if (!isDataReady()) { - return; - } - - if (!odometry_data_) { - // If odometry data does not come, publish original objects - pub_objects_->publish(*objects_data_); - } else { - // Set input data - input_.objects = objects_data_; - input_.pointcloud = pointcloud_data_; - input_.odometry = odometry_data_; - - // Update - output_ = front_vehicle_velocity_estimator_->update(input_); - - // Publish - pub_objects_->publish(output_.objects); - pub_nearest_neighbor_pointcloud_->publish(output_.nearest_neighbor_pointcloud); - } -} - -rcl_interfaces::msg::SetParametersResult FrontVehicleVelocityEstimatorNode::onSetParam( - const std::vector & params) -{ - rcl_interfaces::msg::SetParametersResult result; - - try { - // Node Parameter - { - auto & p = node_param_; - - // Update params - update_param(params, "node_params.update_rate_hz", p.update_rate_hz); - } - - // Core Parameter - { - auto & p = core_param_; - - // Update params - update_param(params, "core_params.moving_average_num", p.moving_average_num); - update_param( - params, "core_params.threshold_pointcloud_z_high", p.threshold_pointcloud_z_high); - update_param(params, "core_params.threshold_pointcloud_z_low", p.threshold_pointcloud_z_low); - update_param( - params, "core_params.threshold_relative_velocity", p.threshold_relative_velocity); - update_param( - params, "core_params.threshold_absolute_velocity", p.threshold_absolute_velocity); - - // Set parameter to instance - if (front_vehicle_velocity_estimator_) { - front_vehicle_velocity_estimator_->setParam(core_param_); - } - } - } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { - result.successful = false; - result.reason = e.what(); - return result; - } - - result.successful = true; - result.reason = "success"; - return result; -} - -bool FrontVehicleVelocityEstimatorNode::isDataReady() -{ - if (!objects_data_) { - RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000, "waiting for objects msg..."); - return false; - } - if (!pointcloud_data_) { - RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000, "waiting for pointcloud msg..."); - return false; - } - return true; -} -} // namespace front_vehicle_velocity_estimator - -#include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(front_vehicle_velocity_estimator::FrontVehicleVelocityEstimatorNode)