Skip to content

Commit 3a5577f

Browse files
author
KhalilSelyan
committed
Merge branch 'main' into perception-objects-pointcloud-better-visualization
2 parents a00a737 + 328374e commit 3a5577f

File tree

732 files changed

+141052
-13588
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

732 files changed

+141052
-13588
lines changed

.github/CODEOWNERS

+29-20
Large diffs are not rendered by default.

.github/PULL_REQUEST_TEMPLATE/small-change.md

+4
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,10 @@ Not applicable.
1515

1616
Not applicable.
1717

18+
## Interface changes
19+
20+
<!-- Describe any changed interfaces, such as topics, services, or parameters, including debugging interfaces -->
21+
1822
## Pre-review checklist for the PR author
1923

2024
The PR author **must** check the checkboxes below when creating the PR.

.github/PULL_REQUEST_TEMPLATE/standard-change.md

+13
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,19 @@
1818

1919
<!-- Describe any changed interfaces, such as topics, services, or parameters. -->
2020

21+
### ROS Topic Changes
22+
23+
<!-- | Topic Name | Type | Direction | Update Description | -->
24+
<!-- | ---------------- | ------------------- | --------- | ------------------------------------------------------------- | -->
25+
<!-- | `/example_topic` | `std_msgs/String` | Subscribe | Description of what the topic is used for in the system | -->
26+
<!-- | `/another_topic` | `sensor_msgs/Image` | Publish | Also explain if it is added / modified / deleted with the PR | -->
27+
28+
### ROS Parameter Changes
29+
30+
<!-- | Parameter Name | Default Value | Update Description | -->
31+
<!-- | -------------------- | ------------- | --------------------------------------------------- | -->
32+
<!-- | `example_parameters` | `1.0` | Describe the parameter and also explain the updates | -->
33+
2134
## Effects on system behavior
2235

2336
<!-- Describe how this PR affects the system behavior. -->

build_depends.repos

+4
Original file line numberDiff line numberDiff line change
@@ -41,3 +41,7 @@ repositories:
4141
type: git
4242
url: https://github.com/MORAI-Autonomous/MORAI-ROS2_morai_msgs.git
4343
version: main
44+
universe/external/glog: # TODO: to use isGoogleInitialized() API in v0.6.0. Remove when the rosdep glog version is updated to v0.6.0 (already updated in Ubuntu 24.04)
45+
type: git
46+
url: https://github.com/tier4/glog.git
47+
version: v0.6.0_t4-ros

common/autoware_auto_common/include/autoware_auto_common/common/types.hpp

+2
Original file line numberDiff line numberDiff line change
@@ -37,7 +37,9 @@ namespace types
3737
// We don't currently require code to comply to MISRA, but we should try to where it is
3838
// easily possible.
3939
using bool8_t = bool;
40+
#if __cplusplus < 201811L || !__cpp_char8_t
4041
using char8_t = char;
42+
#endif
4143
using uchar8_t = unsigned char;
4244
// If we ever compile on a platform where this is not true, float32_t and float64_t definitions
4345
// need to be adjusted.

common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp

+5-1
Original file line numberDiff line numberDiff line change
@@ -57,6 +57,9 @@ struct ObjectPropertyValues
5757
float alpha{0.999F};
5858
};
5959

60+
// Control object marker visualization
61+
enum class ObjectFillType { Skeleton, Fill };
62+
6063
// Map defining colors according to value of label field in ObjectClassification msg
6164
const std::map<
6265
autoware_auto_perception_msgs::msg::ObjectClassification::_label_type, ObjectPropertyValues>
@@ -87,7 +90,8 @@ get_shape_marker_ptr(
8790
const autoware_auto_perception_msgs::msg::Shape & shape_msg,
8891
const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
8992
const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width,
90-
const bool & is_orientation_available = true);
93+
const bool & is_orientation_available = true,
94+
const ObjectFillType fill_type = ObjectFillType::Skeleton);
9195

9296
AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr
9397
get_2d_shape_marker_ptr(

common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp

+14-4
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,7 @@
3737

3838
#include <autoware_auto_perception_msgs/msg/object_classification.hpp>
3939
#include <sensor_msgs/msg/point_cloud2.hpp>
40+
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
4041
#include <tf2_sensor_msgs/tf2_sensor_msgs.hpp>
4142
#include <unique_identifier_msgs/msg/uuid.hpp>
4243

@@ -60,9 +61,6 @@
6061
#include <pcl/search/kdtree.h>
6162
#include <pcl/search/pcl_search.h>
6263
#include <pcl_conversions/pcl_conversions.h>
63-
64-
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
65-
6664
#include <tf2_ros/buffer.h>
6765
#include <tf2_ros/transform_listener.h>
6866

@@ -195,6 +193,12 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
195193
SLOT(updateTopic()));
196194
qos_profile_points_property = new rviz_common::properties::QosProfileProperty(
197195
m_default_pointcloud_topic, qos_profile_points);
196+
m_object_fill_type_property = new rviz_common::properties::EnumProperty(
197+
"Object Fill Type", "skeleton", "Change object fill type in visualization", this);
198+
m_object_fill_type_property->addOption(
199+
"skeleton", static_cast<int>(detail::ObjectFillType::Skeleton));
200+
m_object_fill_type_property->addOption("Fill", static_cast<int>(detail::ObjectFillType::Fill));
201+
198202
// iterate over default values to create and initialize the properties.
199203
for (const auto & map_property_it : detail::kDefaultObjectPropertyValues) {
200204
const auto & class_property_values = map_property_it.second;
@@ -416,9 +420,13 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
416420
const bool & is_orientation_available) const
417421
{
418422
const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(labels);
423+
const auto fill_type =
424+
static_cast<detail::ObjectFillType>(m_object_fill_type_property->getOptionInt());
425+
419426
if (m_display_type_property->getOptionInt() == 0) {
420427
return detail::get_shape_marker_ptr(
421-
shape_msg, centroid, orientation, color_rgba, line_width, is_orientation_available);
428+
shape_msg, centroid, orientation, color_rgba, line_width, is_orientation_available,
429+
fill_type);
422430
} else if (m_display_type_property->getOptionInt() == 1) {
423431
return detail::get_2d_shape_marker_ptr(
424432
shape_msg, centroid, orientation, color_rgba, line_width, is_orientation_available);
@@ -860,6 +868,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
860868
rviz_common::properties::EnumProperty * m_simple_visualize_mode_property;
861869
// Property to set confidence interval of state estimations
862870
rviz_common::properties::EnumProperty * m_confidence_interval_property;
871+
// Property to set visualization type
872+
rviz_common::properties::EnumProperty * m_object_fill_type_property;
863873
// Property to enable/disable label visualization
864874
rviz_common::properties::BoolProperty m_display_label_property;
865875
// Property to enable/disable uuid visualization

common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp

+19-7
Original file line numberDiff line numberDiff line change
@@ -495,23 +495,37 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr(
495495
const autoware_auto_perception_msgs::msg::Shape & shape_msg,
496496
const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
497497
const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width,
498-
const bool & is_orientation_available)
498+
const bool & is_orientation_available, const ObjectFillType fill_type)
499499
{
500500
auto marker_ptr = std::make_shared<Marker>();
501501
marker_ptr->ns = std::string("shape");
502+
marker_ptr->color = color_rgba;
503+
marker_ptr->scale.x = line_width;
502504

503505
using autoware_auto_perception_msgs::msg::Shape;
504506
if (shape_msg.type == Shape::BOUNDING_BOX) {
505-
marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
506-
calc_bounding_box_line_list(shape_msg, marker_ptr->points);
507+
if (fill_type == ObjectFillType::Skeleton) {
508+
marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
509+
calc_bounding_box_line_list(shape_msg, marker_ptr->points);
510+
} else if (fill_type == ObjectFillType::Fill) {
511+
marker_ptr->type = visualization_msgs::msg::Marker::CUBE;
512+
marker_ptr->scale = shape_msg.dimensions;
513+
marker_ptr->color.a = 0.75f;
514+
}
507515
if (is_orientation_available) {
508516
calc_bounding_box_direction_line_list(shape_msg, marker_ptr->points);
509517
} else {
510518
calc_bounding_box_orientation_line_list(shape_msg, marker_ptr->points);
511519
}
512520
} else if (shape_msg.type == Shape::CYLINDER) {
513-
marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
514-
calc_cylinder_line_list(shape_msg, marker_ptr->points);
521+
if (fill_type == ObjectFillType::Skeleton) {
522+
marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
523+
calc_cylinder_line_list(shape_msg, marker_ptr->points);
524+
} else if (fill_type == ObjectFillType::Fill) {
525+
marker_ptr->type = visualization_msgs::msg::Marker::CYLINDER;
526+
marker_ptr->scale = shape_msg.dimensions;
527+
marker_ptr->color.a = 0.75f;
528+
}
515529
} else if (shape_msg.type == Shape::POLYGON) {
516530
marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
517531
calc_polygon_line_list(shape_msg, marker_ptr->points);
@@ -523,8 +537,6 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr(
523537
marker_ptr->action = visualization_msgs::msg::Marker::MODIFY;
524538
marker_ptr->pose = to_pose(centroid, orientation);
525539
marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15);
526-
marker_ptr->scale.x = line_width;
527-
marker_ptr->color = color_rgba;
528540

529541
return marker_ptr;
530542
}
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,108 @@
1+
cmake_minimum_required(VERSION 3.8)
2+
project(autoware_mission_details_overlay_rviz_plugin)
3+
4+
find_package(ament_cmake_auto REQUIRED)
5+
ament_auto_find_build_dependencies()
6+
7+
set(
8+
headers_to_moc
9+
include/mission_details_display.hpp
10+
)
11+
12+
set(
13+
headers_to_not_moc
14+
include/remaining_distance_time_display.hpp
15+
)
16+
17+
foreach(header "${headers_to_moc}")
18+
qt5_wrap_cpp(display_moc_files "${header}")
19+
endforeach()
20+
21+
set(
22+
display_source_files
23+
src/overlay_utils.cpp
24+
src/mission_details_display.cpp
25+
src/remaining_distance_time_display.cpp
26+
)
27+
28+
add_library(
29+
${PROJECT_NAME} SHARED
30+
${display_moc_files}
31+
${headers_to_not_moc}
32+
${display_source_files}
33+
)
34+
35+
set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 17)
36+
target_compile_options(${PROJECT_NAME} PRIVATE -Wall -Wextra -Wpedantic)
37+
38+
target_include_directories(
39+
${PROJECT_NAME} PUBLIC
40+
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
41+
$<INSTALL_INTERFACE:include>
42+
${Qt5Widgets_INCLUDE_DIRS}
43+
${OpenCV_INCLUDE_DIRS}
44+
)
45+
46+
target_link_libraries(
47+
${PROJECT_NAME} PUBLIC
48+
rviz_ogre_vendor::OgreMain
49+
rviz_ogre_vendor::OgreOverlay
50+
)
51+
52+
target_compile_definitions(${PROJECT_NAME} PRIVATE "${PROJECT_NAME}_BUILDING_LIBRARY")
53+
54+
# prevent pluginlib from using boost
55+
target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
56+
57+
pluginlib_export_plugin_description_file(rviz_common plugins_description.xml)
58+
59+
ament_target_dependencies(
60+
${PROJECT_NAME}
61+
PUBLIC
62+
rviz_common
63+
rviz_rendering
64+
autoware_internal_msgs
65+
)
66+
67+
ament_export_include_directories(include)
68+
ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET)
69+
ament_export_dependencies(
70+
rviz_common
71+
rviz_ogre_vendor
72+
)
73+
74+
if(BUILD_TESTING)
75+
find_package(ament_lint_auto REQUIRED)
76+
ament_lint_auto_find_test_dependencies()
77+
endif()
78+
79+
install(
80+
TARGETS ${PROJECT_NAME}
81+
EXPORT ${PROJECT_NAME}
82+
ARCHIVE DESTINATION lib
83+
LIBRARY DESTINATION lib
84+
RUNTIME DESTINATION bin
85+
INCLUDES DESTINATION include
86+
)
87+
88+
install(
89+
DIRECTORY include/
90+
DESTINATION include
91+
)
92+
93+
install(
94+
TARGETS
95+
DESTINATION lib/${PROJECT_NAME}
96+
)
97+
98+
# Copy the assets directory to the installation directory
99+
install(
100+
DIRECTORY assets/
101+
DESTINATION share/${PROJECT_NAME}/assets
102+
)
103+
104+
add_definitions(-DQT_NO_KEYWORDS)
105+
106+
ament_package(
107+
CONFIG_EXTRAS "autoware_mission_details_overlay_rviz_plugin-extras.cmake"
108+
)
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,12 @@
1+
Copyright (c) 2022, Team Spatzenhirn
2+
Copyright (c) 2014, JSK Lab
3+
4+
Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
5+
6+
1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
7+
8+
2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
9+
10+
3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
11+
12+
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
# autoware_mission_details_overlay_rviz_plugin
2+
3+
This RViz plugin displays the remaining distance and time for the current mission.
4+
5+
## Inputs / Outputs
6+
7+
### Input
8+
9+
| Name | Type | Description |
10+
| ------------------------------------------- | ----------------------------------------------------------- | ---------------------------------------------------- |
11+
| `/planning/mission_remaining_distance_time` | `autoware_planning_msgs::msg::MissionRemainingDistanceTime` | The topic is for mission remaining distance and time |
12+
13+
## Overlay Parameters
14+
15+
| Name | Type | Default Value | Description |
16+
| -------- | ---- | ------------- | --------------------------------- |
17+
| `Width` | int | 170 | Width of the overlay [px] |
18+
| `Height` | int | 100 | Height of the overlay [px] |
19+
| `Right` | int | 10 | Margin from the right border [px] |
20+
| `Top` | int | 10 | Margin from the top border [px] |
21+
22+
The mission details display is aligned with top right corner of the screen.
23+
24+
## Usage
25+
26+
Similar to [autoware_overlay_rviz_plugin](../autoware_overlay_rviz_plugin/README.md)
27+
28+
## Credits
29+
30+
Based on the [jsk_visualization](https://github.com/jsk-ros-pkg/jsk_visualization) package.
31+
32+
### Icons
33+
34+
- <https://fonts.google.com/icons?selected=Material+Symbols+Outlined:conversion_path:FILL@1;wght@400;GRAD@200;opsz@20&icon.size=20&icon.color=%23e8eaed&icon.query=path>
35+
- <https://fonts.google.com/icons?selected=Material+Symbols+Outlined:av_timer:FILL@1;wght@400;GRAD@200;opsz@20&icon.size=20&icon.color=%23e8eaed&icon.query=av+timer>

0 commit comments

Comments
 (0)