Skip to content

Commit

Permalink
Merge branch 'main' into feat/detection_by_tracker/add_glog
Browse files Browse the repository at this point in the history
  • Loading branch information
kminoda authored Apr 9, 2024
2 parents 436258a + 8617285 commit 9477d4a
Show file tree
Hide file tree
Showing 103 changed files with 3,039 additions and 1,139 deletions.
5 changes: 5 additions & 0 deletions build_depends.repos
Original file line number Diff line number Diff line change
Expand Up @@ -41,3 +41,8 @@ repositories:
type: git
url: https://github.com/MORAI-Autonomous/MORAI-ROS2_morai_msgs.git
version: main
#vehicle
vehicle/sample_vehicle_launch:
type: git
url: https://github.com/autowarefoundation/sample_vehicle_launch.git
version: main
5 changes: 5 additions & 0 deletions common/global_parameter_loader/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,11 @@ project(global_parameter_loader)
find_package(autoware_cmake REQUIRED)
autoware_package()

if(BUILD_TESTING)
file(GLOB_RECURSE test_files test/*.cpp)
ament_add_ros_isolated_gtest(test_global_params_launch ${test_files})
endif()

ament_auto_package(
INSTALL_TO_SHARE
launch
Expand Down
4 changes: 3 additions & 1 deletion common/global_parameter_loader/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -10,8 +10,10 @@
<buildtool_depend>ament_cmake_auto</buildtool_depend>
<buildtool_depend>autoware_cmake</buildtool_depend>

<exec_depend>vehicle_info_util</exec_depend>
<depend>sample_vehicle_description</depend>
<depend>vehicle_info_util</depend>

<test_depend>ament_cmake_ros</test_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>autoware_lint_common</test_depend>

Expand Down
44 changes: 44 additions & 0 deletions common/global_parameter_loader/test/test_global_params_launch.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,44 @@
// Copyright 2023 The 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 <gtest/gtest.h>

#include <cstdlib>
#include <iostream>
#include <string>

TEST(TestLaunchFile, test_launch_file)
{
// Define the path of Python launch file
std::string global_params_launch_path = "global_params.launch.py";

// Define the parameters you want to pass to the launch file
std::string use_sim_time_param = "false";
std::string vehicle_model_param = "sample_vehicle";
// Construct the command to run the Python launch script with parameters
std::string command = "ros2 launch global_parameter_loader " + global_params_launch_path +
" use_sim_time:=" + use_sim_time_param +
" vehicle_model:=" + vehicle_model_param;

// Use the system() function to execute the command
int result = std::system(command.c_str());
// Check the result of running the launch file
EXPECT_EQ(result, 0);
}

int main(int argc, char * argv[])
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,8 @@
namespace object_recognition_utils
{
using tier4_autoware_utils::Polygon2d;
// minimum area to avoid division by zero
static const double MIN_AREA = 1e-6;

inline double getConvexShapeArea(const Polygon2d & source_polygon, const Polygon2d & target_polygon)
{
Expand Down Expand Up @@ -66,10 +68,12 @@ template <class T1, class T2>
double get2dIoU(const T1 source_object, const T2 target_object, const double min_union_area = 0.01)
{
const auto source_polygon = tier4_autoware_utils::toPolygon2d(source_object);
if (boost::geometry::area(source_polygon) < MIN_AREA) return 0.0;
const auto target_polygon = tier4_autoware_utils::toPolygon2d(target_object);
if (boost::geometry::area(target_polygon) < MIN_AREA) return 0.0;

const double intersection_area = getIntersectionArea(source_polygon, target_polygon);
if (intersection_area == 0.0) return 0.0;
if (intersection_area < MIN_AREA) return 0.0;
const double union_area = getUnionArea(source_polygon, target_polygon);

const double iou =
Expand All @@ -81,7 +85,9 @@ template <class T1, class T2>
double get2dGeneralizedIoU(const T1 & source_object, const T2 & target_object)
{
const auto source_polygon = tier4_autoware_utils::toPolygon2d(source_object);
if (boost::geometry::area(source_polygon) < MIN_AREA) return 0.0;
const auto target_polygon = tier4_autoware_utils::toPolygon2d(target_object);
if (boost::geometry::area(target_polygon) < MIN_AREA) return 0.0;

const double intersection_area = getIntersectionArea(source_polygon, target_polygon);
const double union_area = getUnionArea(source_polygon, target_polygon);
Expand All @@ -95,11 +101,13 @@ template <class T1, class T2>
double get2dPrecision(const T1 source_object, const T2 target_object)
{
const auto source_polygon = tier4_autoware_utils::toPolygon2d(source_object);
const double source_area = boost::geometry::area(source_polygon);
if (source_area < MIN_AREA) return 0.0;
const auto target_polygon = tier4_autoware_utils::toPolygon2d(target_object);
if (boost::geometry::area(target_polygon) < MIN_AREA) return 0.0;

const double intersection_area = getIntersectionArea(source_polygon, target_polygon);
if (intersection_area == 0.0) return 0.0;
const double source_area = boost::geometry::area(source_polygon);
if (intersection_area < MIN_AREA) return 0.0;

return std::min(1.0, intersection_area / source_area);
}
Expand All @@ -108,11 +116,13 @@ template <class T1, class T2>
double get2dRecall(const T1 source_object, const T2 target_object)
{
const auto source_polygon = tier4_autoware_utils::toPolygon2d(source_object);
if (boost::geometry::area(source_polygon) < MIN_AREA) return 0.0;
const auto target_polygon = tier4_autoware_utils::toPolygon2d(target_object);
const double target_area = boost::geometry::area(target_polygon);
if (target_area < MIN_AREA) return 0.0;

const double intersection_area = getIntersectionArea(source_polygon, target_polygon);
if (intersection_area == 0.0) return 0.0;
const double target_area = boost::geometry::area(target_polygon);
if (intersection_area < MIN_AREA) return 0.0;

return std::min(1.0, intersection_area / target_area);
}
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,65 @@
// 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 TIER4_AUTOWARE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_
#define TIER4_AUTOWARE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_

#include <rclcpp/rclcpp.hpp>

#include <string>

namespace tier4_autoware_utils
{

template <typename T>
class InterProcessPollingSubscriber
{
private:
typename rclcpp::Subscription<T>::SharedPtr subscriber_;
std::optional<T> data_;

public:
explicit InterProcessPollingSubscriber(rclcpp::Node * node, const std::string & topic_name)
{
auto noexec_callback_group =
node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
auto noexec_subscription_options = rclcpp::SubscriptionOptions();
noexec_subscription_options.callback_group = noexec_callback_group;

subscriber_ = node->create_subscription<T>(
topic_name, rclcpp::QoS{1}, [node](const typename T::ConstSharedPtr msg) { assert(false); },
noexec_subscription_options);
};
bool updateLatestData()
{
rclcpp::MessageInfo message_info;
T tmp;
// The queue size (QoS) must be 1 to get the last message data.
if (subscriber_->take(tmp, message_info)) {
data_ = tmp;
}
return data_.has_value();
};
const T & getData() const
{
if (!data_.has_value()) {
throw std::runtime_error("Bad_optional_access in class InterProcessPollingSubscriber");
}
return data_.value();
};
};

} // namespace tier4_autoware_utils

#endif // TIER4_AUTOWARE_UTILS__ROS__POLLING_SUBSCRIBER_HPP_
Loading

0 comments on commit 9477d4a

Please sign in to comment.