Skip to content

Commit

Permalink
Merge branch 'main' into feat/safety_checker_class
Browse files Browse the repository at this point in the history
Signed-off-by: kyoichi-sugahara <[email protected]>
  • Loading branch information
kyoichi-sugahara committed Sep 4, 2023
2 parents 3c062b7 + 532c050 commit 3fc8152
Show file tree
Hide file tree
Showing 490 changed files with 15,005 additions and 5,798 deletions.
7 changes: 1 addition & 6 deletions .cspell-partial.json
Original file line number Diff line number Diff line change
@@ -1,10 +1,5 @@
{
"ignorePaths": [
"**/perception/**",
"**/planning/**",
"**/sensing/**",
"perception/bytetrack/lib/**"
],
"ignorePaths": ["**/perception/**", "**/sensing/**", "perception/bytetrack/lib/**"],
"ignoreRegExpList": [],
"words": []
}
15 changes: 9 additions & 6 deletions .github/CODEOWNERS
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,9 @@ common/component_interface_tools/** [email protected] [email protected] m
common/component_interface_utils/** [email protected] [email protected] [email protected] [email protected]
common/cuda_utils/** [email protected] [email protected]
common/fake_test_node/** [email protected] [email protected] [email protected] [email protected]
common/geography_utils/** [email protected]
common/global_parameter_loader/** [email protected]
common/glog_component/** [email protected]
common/goal_distance_calculator/** [email protected]
common/grid_map_utils/** [email protected]
common/interpolation/** [email protected] [email protected] [email protected]
Expand Down Expand Up @@ -53,9 +55,10 @@ common/trtexec_vendor/** [email protected] [email protected]
common/tvm_utility/** [email protected] [email protected]
control/autonomous_emergency_braking/** [email protected] [email protected]
control/control_performance_analysis/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
control/control_validator/** [email protected] [email protected] [email protected]
control/external_cmd_selector/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
control/joy_controller/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
control/lane_departure_checker/** [email protected]
control/lane_departure_checker/** [email protected] [email protected]
control/mpc_lateral_controller/** [email protected] [email protected]
control/obstacle_collision_checker/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
control/operation_mode_transition_manager/** [email protected] [email protected]
Expand All @@ -79,9 +82,9 @@ launch/tier4_sensing_launch/** [email protected]
launch/tier4_simulator_launch/** [email protected] [email protected] [email protected] [email protected] [email protected]
launch/tier4_system_launch/** [email protected] [email protected]
launch/tier4_vehicle_launch/** [email protected]
localization/ar_tag_based_localizer/** [email protected] [email protected]
localization/ekf_localizer/** [email protected] [email protected] [email protected] [email protected] [email protected]
localization/gyro_odometer/** [email protected] [email protected]
localization/initial_pose_button_panel/** [email protected] [email protected]
localization/localization_error_monitor/** [email protected] [email protected]
localization/ndt_scan_matcher/** [email protected] [email protected] [email protected] [email protected]
localization/pose2twist/** [email protected] [email protected]
Expand All @@ -95,6 +98,7 @@ localization/yabloc/yabloc_particle_filter/** [email protected] masahiro
localization/yabloc/yabloc_pose_initializer/** [email protected] [email protected]
map/map_height_fitter/** [email protected] [email protected] [email protected]
map/map_loader/** [email protected] [email protected] [email protected] [email protected]
map/map_projection_loader/** [email protected] [email protected] [email protected]
map/map_tf_generator/** [email protected]
map/util/lanelet2_map_preprocessor/** [email protected]
perception/bytetrack/** [email protected]
Expand All @@ -114,7 +118,7 @@ perception/lidar_apollo_segmentation_tvm/** [email protected] xinyu.wang@
perception/lidar_apollo_segmentation_tvm_nodes/** [email protected] [email protected]
perception/lidar_centerpoint/** [email protected] [email protected]
perception/lidar_centerpoint_tvm/** [email protected] [email protected]
perception/map_based_prediction/** [email protected] [email protected] [email protected] [email protected]
perception/map_based_prediction/** [email protected] shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp [email protected] [email protected] [email protected]
perception/multi_object_tracker/** [email protected] [email protected]
perception/object_merger/** [email protected] [email protected]
perception/object_range_splitter/** [email protected]
Expand Down Expand Up @@ -163,7 +167,7 @@ planning/freespace_planning_algorithms/** [email protected] takamasa.hori
planning/mission_planner/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
planning/motion_velocity_smoother/** [email protected] [email protected] [email protected] [email protected]
planning/obstacle_avoidance_planner/** [email protected] [email protected] [email protected]
planning/obstacle_cruise_planner/** [email protected] [email protected]
planning/obstacle_cruise_planner/** [email protected] [email protected] [email protected] [email protected]
planning/obstacle_stop_planner/** [email protected] [email protected] [email protected] [email protected] [email protected]
planning/obstacle_velocity_limiter/** [email protected]
planning/path_smoother/** [email protected] [email protected]
Expand All @@ -181,11 +185,10 @@ planning/sampling_based_planner/sampler_common/** [email protected]
planning/scenario_selector/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
planning/static_centerline_optimizer/** [email protected] [email protected]
planning/surround_obstacle_checker/** [email protected]
sensing/geo_pos_conv/** [email protected]
sensing/gnss_poser/** [email protected]
sensing/image_diagnostics/** [email protected]
sensing/image_transport_decompressor/** [email protected]
sensing/imu_corrector/** [email protected]
sensing/imu_corrector/** [email protected] [email protected] [email protected]
sensing/livox/livox_tag_filter/** [email protected]
sensing/pointcloud_preprocessor/** [email protected] [email protected] [email protected]
sensing/radar_scan_to_pointcloud2/** [email protected] [email protected]
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -146,6 +146,11 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
m_marker_common.addMessage(markers_ptr);
}

void deleteMarker(rviz_default_plugins::displays::MarkerID marker_id)
{
m_marker_common.deleteMarker(marker_id);
}

protected:
/// \brief Convert given shape msg into a Marker
/// \tparam ClassificationContainerT List type with ObjectClassificationMsg
Expand Down Expand Up @@ -346,9 +351,13 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
{
std_msgs::msg::ColorRGBA sample_color;
sample_color.r = 1.0;
sample_color.g = 0.0;
sample_color.b = 1.0;
colors.push_back(sample_color); // magenta
sample_color.g = 0.65;
sample_color.b = 0.0;
colors.push_back(sample_color); // orange
sample_color.r = 1.0;
sample_color.g = 1.0;
sample_color.b = 0.0;
colors.push_back(sample_color); // yellow
sample_color.r = 0.69;
sample_color.g = 1.0;
sample_color.b = 0.18;
Expand All @@ -361,22 +370,18 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
sample_color.g = 1.0;
sample_color.b = 0.0;
colors.push_back(sample_color); // chartreuse green
sample_color.r = 0.12;
sample_color.g = 0.56;
sample_color.b = 1.0;
colors.push_back(sample_color); // dodger blue
sample_color.r = 0.0;
sample_color.g = 1.0;
sample_color.b = 1.0;
colors.push_back(sample_color); // cyan
sample_color.r = 0.54;
sample_color.g = 0.168;
sample_color.b = 0.886;
colors.push_back(sample_color); // blueviolet
sample_color.r = 0.0;
sample_color.g = 1.0;
sample_color.b = 0.5;
colors.push_back(sample_color); // spring green
sample_color.r = 0.53;
sample_color.g = 0.81;
sample_color.b = 0.98;
colors.push_back(sample_color); // light skyblue
sample_color.r = 1.0;
sample_color.g = 0.41;
sample_color.b = 0.71;
colors.push_back(sample_color); // hot pink
}

double get_line_width() { return m_line_width_property.getFloat(); }
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@

#include <condition_variable>
#include <list>
#include <set>
#include <string>
#include <unordered_map>
#include <vector>
Expand Down Expand Up @@ -112,6 +113,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay
std::mutex mutex;
std::condition_variable condition;
std::vector<visualization_msgs::msg::Marker::SharedPtr> markers;
std::set<rviz_default_plugins::displays::MarkerID> existing_marker_ids;
};

} // namespace object_detection
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -64,8 +64,7 @@ visualization_msgs::msg::Marker::SharedPtr get_path_confidence_marker_ptr(
marker_ptr->color = path_confidence_color;
marker_ptr->pose.position = predicted_path.path.back().position;
marker_ptr->text = std::to_string(predicted_path.confidence);
marker_ptr->color.a = std::max(
static_cast<double>(std::min(static_cast<double>(predicted_path.confidence), 0.999)), 0.5);
marker_ptr->color.a = 0.5;
return marker_ptr;
}

Expand All @@ -81,9 +80,8 @@ visualization_msgs::msg::Marker::SharedPtr get_predicted_path_marker_ptr(
marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2);
marker_ptr->pose = initPose();
marker_ptr->color = predicted_path_color;
marker_ptr->color.a = std::max(
static_cast<double>(std::min(static_cast<double>(predicted_path.confidence), 0.999)), 0.5);
marker_ptr->scale.x = 0.03 * marker_ptr->color.a;
marker_ptr->color.a = 0.6;
marker_ptr->scale.x = 0.015;
calc_path_line_list(predicted_path, marker_ptr->points, is_simple);
for (size_t k = 0; k < marker_ptr->points.size(); ++k) {
marker_ptr->points.at(k).z -= shape.dimensions.z / 2.0;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
#include <object_detection/predicted_objects_display.hpp>

#include <memory>
#include <set>

namespace autoware
{
Expand Down Expand Up @@ -130,7 +131,7 @@ std::vector<visualization_msgs::msg::Marker::SharedPtr> PredictedObjectsDisplay:
auto acceleration_text_marker_ptr = acceleration_text_marker.value();
acceleration_text_marker_ptr->header = msg->header;
acceleration_text_marker_ptr->id = uuid_to_marker_id(object.object_id);
add_marker(acceleration_text_marker_ptr);
markers.push_back(acceleration_text_marker_ptr);
}

// Get marker for twist
Expand Down Expand Up @@ -195,11 +196,19 @@ void PredictedObjectsDisplay::update(float wall_dt, float ros_dt)
std::unique_lock<std::mutex> lock(mutex);

if (!markers.empty()) {
clear_markers();

std::set new_marker_ids = std::set<rviz_default_plugins::displays::MarkerID>();
for (const auto & marker : markers) {
rviz_default_plugins::displays::MarkerID marker_id =
rviz_default_plugins::displays::MarkerID(marker->ns, marker->id);
add_marker(marker);
new_marker_ids.insert(marker_id);
}
for (auto itr = existing_marker_ids.begin(); itr != existing_marker_ids.end(); itr++) {
if (new_marker_ids.find(*itr) == new_marker_ids.end()) {
deleteMarker(*itr);
}
}
existing_marker_ids = new_marker_ids;

markers.clear();
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@ namespace map_interface
struct MapProjectorInfo
{
using Message = tier4_map_msgs::msg::MapProjectorInfo;
static constexpr char name[] = "/map/map_projector_type";
static constexpr char name[] = "/map/map_projector_info";
static constexpr size_t depth = 1;
static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
Expand Down
36 changes: 36 additions & 0 deletions common/geography_utils/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,36 @@
cmake_minimum_required(VERSION 3.14)
project(geography_utils)

find_package(autoware_cmake REQUIRED)
autoware_package()

# GeographicLib
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)

ament_auto_add_library(geography_utils SHARED
src/height.cpp
src/lanelet2_projector.cpp
)

target_link_libraries(geography_utils
${GeographicLib_LIBRARIES}
)

if(BUILD_TESTING)
find_package(ament_cmake_ros REQUIRED)

file(GLOB_RECURSE test_files test/*.cpp)

ament_add_ros_isolated_gtest(test_geography_utils ${test_files})

target_link_libraries(test_geography_utils
geography_utils
)
endif()

ament_auto_package()
5 changes: 5 additions & 0 deletions common/geography_utils/README.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
# geography_utils

## Purpose

This package contains geography-related functions used by other packages, so please refer to them as needed.
36 changes: 36 additions & 0 deletions common/geography_utils/include/geography_utils/height.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,36 @@
// 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 GEOGRAPHY_UTILS__HEIGHT_HPP_
#define GEOGRAPHY_UTILS__HEIGHT_HPP_

#include <map>
#include <stdexcept>
#include <string>
#include <utility>

namespace geography_utils
{

typedef double (*HeightConversionFunction)(
const double height, const double latitude, const double longitude);
double convert_wgs84_to_egm2008(const double height, const double latitude, const double longitude);
double convert_egm2008_to_wgs84(const double height, const double latitude, const double longitude);
double convert_height(
const double height, const double latitude, const double longitude,
const std::string & source_vertical_datum, const std::string & target_vertical_datum);

} // namespace geography_utils

#endif // GEOGRAPHY_UTILS__HEIGHT_HPP_
Original file line number Diff line number Diff line change
@@ -0,0 +1,32 @@
// 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 GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_
#define GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_

#include <tier4_map_msgs/msg/map_projector_info.hpp>

#include <lanelet2_io/Projection.h>

#include <memory>

namespace geography_utils
{
using MapProjectorInfo = tier4_map_msgs::msg::MapProjectorInfo;

std::unique_ptr<lanelet::Projector> get_lanelet2_projector(const MapProjectorInfo & projector_info);

} // namespace geography_utils

#endif // GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_
Original file line number Diff line number Diff line change
@@ -1,18 +1,21 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>geo_pos_conv</name>
<version>2.0.0</version>
<description>The ROS 2 geo_pos_conv package</description>
<maintainer email="[email protected]">Yamato Ando</maintainer>

<name>geography_utils</name>
<version>0.1.0</version>
<description>The geography_utils package</description>
<maintainer email="[email protected]">Koji Minoda</maintainer>
<license>Apache License 2.0</license>

<buildtool_depend>ament_cmake_auto</buildtool_depend>
<buildtool_depend>autoware_cmake</buildtool_depend>

<depend>rclcpp</depend>
<depend>geographiclib</depend>
<depend>lanelet2_extension</depend>
<depend>lanelet2_io</depend>
<depend>tier4_map_msgs</depend>

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

Expand Down
Loading

0 comments on commit 3fc8152

Please sign in to comment.