Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(pointcloud_preprocessor): support 3d distortion corrector for distortion corrector node #7027

Closed
Closed
Changes from all commits
Commits
Show all changes
94 commits
Select commit Hold shift + click to select a range
87062b6
add support for 3d distortion corrector
vividf May 15, 2024
7f792aa
fix(trajectory_follower_node): fix config file of plot juggler (#6910)
tkimura4 May 1, 2024
7675df2
chore(component_state_monitor): relax pose_estimator_pose timeout (#6…
shmpwk May 1, 2024
85277b4
docs(tier4_simulated_clock_rviz_plugin): update how to use (#6914)
zulfaqar-azmi-t4 May 1, 2024
1660fed
refactor(avoidance): rebuild object info list (#6913)
satoshi-ota May 2, 2024
4e14075
feat(mission_planner): publish initial and goal poses to logs (#6918)
zulfaqar-azmi-t4 May 2, 2024
ff876ad
feat(pull_request_templates): improve the pull request template to en…
Owen-Liuyuxuan May 3, 2024
de94fc4
chore(radar_track_msgs_converter): change radar tracks subscription q…
YoshiRi May 4, 2024
4e96099
feat(probabilistic_occupancy_grid_map): add downsample filter option …
YoshiRi May 6, 2024
7ace304
fix(behavior_path_planner_common): fix warning of containerOutOfBound…
veqcc May 6, 2024
5f0ae9c
fix: do not use c++20 char8_t keyword (#3629)
ralwing May 6, 2024
2b69477
fix(control_performance_analysis): fix bug of ignoredReturnValue (#6921)
veqcc May 6, 2024
7b91f0d
fix(perception_online_evaluator): fix bug of constStatement (#6922)
veqcc May 7, 2024
bc3425f
refactor(bpp): path shifter clang tidy and logging level configuratio…
zulfaqar-azmi-t4 May 7, 2024
e848a63
fix(occupancy_grid_map_outlier_filter): add intensity field (#6797)
badai-nguyen May 7, 2024
10c3636
feat(crosswalk)!: change ego min assumed speed (#6904)
yuki-takagi-66 May 7, 2024
292556d
perf(lane_change): rework object filter (#6847)
zulfaqar-azmi-t4 May 7, 2024
40a7d47
fix(lane_change): return safe is object list is empty (#6931)
zulfaqar-azmi-t4 May 7, 2024
7c31df3
fix(diagnostic_graph_aggregator): fix a bug where unit links were inc…
isamu-takagi May 7, 2024
581b585
feat(autonomous_emergency_braking): add obstacle velocity estimation …
danielsanchezaran May 7, 2024
a048f17
feat(start_planner): add centerline crossing check (#6900)
danielsanchezaran May 7, 2024
596ff98
fix(ground_segmentation): fix warning of identicalConditionAfterEarly…
veqcc May 8, 2024
e817f97
fix(voxel_grid_downsample_filter): add intensity field (#6849)
badai-nguyen May 8, 2024
eca118b
feat(freespace_planner): only plan if ego is stopped (#6062)
VRichardJP May 8, 2024
e131f2a
feat(image projection based fusion): unrecify 3d point for image proj…
yukkysaito May 8, 2024
1e6f37c
perf(route_handler): simplify queries on the road and shoulder lanele…
maxime-clem May 8, 2024
fba7760
refactor(lane_change): move getCurrentTurnSignalInfo to scene file (#…
zulfaqar-azmi-t4 May 8, 2024
f593bba
refactor(avoidance): organize alias (#6947)
satoshi-ota May 8, 2024
becebc8
fix(tvm_utility): fix warning of negativeContainerIndex (#6924)
veqcc May 8, 2024
138dbfb
perf(yabloc): fix performance warning of iterateByValue (#6929)
veqcc May 8, 2024
b35ab8a
feat: add low_intensity_cluster_filter (#6850)
badai-nguyen May 8, 2024
ab8b085
feat(vehicle_cmd_gate): support for real time update params for v cmd…
danielsanchezaran May 9, 2024
030539f
fix(traffic_light_classifier): fix warning of uninitvar (#6925)
veqcc May 9, 2024
7e14449
feat(build_depends): add glog-vendor (#6952)
TakaHoribe May 9, 2024
1e4cefb
feat(goal_planner): align z-height of road_shoulder (#6854)
soblin May 9, 2024
49cd55d
feat(autoware_pose_covariance_modifier): add new node to early fuse g…
meliketanrikulu May 9, 2024
3f84639
chore(glog): add initialization check (#6792)
TakaHoribe May 9, 2024
5e8cabc
feat(image projection based fusion): revert #6902 (unrecify 3d point …
yukkysaito May 9, 2024
04600b2
chore: update sensing/perception maintainer (#6950)
YoshiRi May 9, 2024
8f6026c
fix(system_monitor): fix warning of containerOutOfBounds (#6927)
veqcc May 9, 2024
020b87b
fix(bpp): keep publishing rtc cooperate status (#6953)
satoshi-ota May 9, 2024
1a729a4
fix(ground_segmentation): add intensity field (#6791)
badai-nguyen May 9, 2024
1ff6276
fix(freespace_planner): fix motion_velocity_smoother error while park…
ahmeddesokyebrahim May 9, 2024
a1e4e92
feat(map_based_prediction): use different time horizon (#6877)
soblin May 10, 2024
4823ec6
fix(map_based_prediction): improve pedestrian path generation efficie…
technolojin May 10, 2024
d77923e
feat(goal_planner): fix non-thread-safe access in goal_planner (rever…
soblin May 10, 2024
ecc730a
feat(autonomous_emergency_braking): add param update support for AEB …
danielsanchezaran May 10, 2024
0410a12
fix(autonomous_emergency_braking): add missing erase to velocity hist…
danielsanchezaran May 10, 2024
0cf87bf
fix(componet_state_monitor): remove ndt node alive monitoring (#6957)
YamatoAndo May 10, 2024
28d1de6
feat(path_sampler): make the trajectory smoother, add params, fix cra…
maxime-clem May 10, 2024
fc1c569
fix(map_based_prediction): revert use different time horizon (#6877) …
YoshiRi May 10, 2024
bb10ea6
feat(control_evaluator): implement a control evaluator (#6959)
danielsanchezaran May 10, 2024
d19f9b8
feat: update rviz2 overlay (#6883)
KhalilSelyan May 10, 2024
90db599
perf(side_shift): fix unupdated prev path that caused heavy interpola…
maxime-clem May 10, 2024
092e80e
refactor(mission_planner): remove redundant `is_reroute` check (#6980)
veqcc May 11, 2024
bc7d9b9
fix(autoware_overlay_rviz_plugin): fix subs and cleanup (#6978)
xmfcx May 11, 2024
a4aad3c
fix(accel_brake_calibrator): fix to set service name and exception fa…
h-ohta May 13, 2024
be98f1b
test: behavior path obstacle avoidance (#6972)
go-sakayori May 13, 2024
1349dc2
feat(rtc_interface)!: add new field to rtc cooperate status (#6933)
satoshi-ota May 13, 2024
326a615
fix(behavior_velocity_run_out_module): initialize `accel_reason_` (#6…
veqcc May 13, 2024
8ca9153
feat: add autoware_remaining_distance_time_calculator and overlay (#6…
ahmeddesokyebrahim May 13, 2024
ae42c6f
fix(accel_brake_map_calibrator): fix accel_brake_map_calibrator not t…
tkimura4 May 13, 2024
51394e2
feat(mrm_emergency_stop_operator): add support for real time param re…
danielsanchezaran May 13, 2024
af59ab6
feat(map_based_prediction): incorporate crosswalk user history (#6905)
dkoldaev May 13, 2024
51635d9
feat(out_of_lane): add option to ignore overlaps in lane changes (#6991)
maxime-clem May 13, 2024
5cbbead
feat(tier4_perception_launch): downsample perception input pointcloud…
YoshiRi May 13, 2024
a3af5cc
feat(tier4_perception_launch): fix typo error (#6999)
YoshiRi May 14, 2024
bc93a8e
refactor(behavior_velocity_occlusion_spot_module): reduce cppcheck wa…
veqcc May 14, 2024
f5b1b94
fix(ndt_scan_matcher): improved tpe (#6990)
SakodaShintaro May 14, 2024
355d677
refactor(behavior_velocity_occlusion_spot_module): remove unnecessary…
veqcc May 14, 2024
61bdccb
chore(tools): move system and evaluation tools to autoware_tools repo…
satoshi-ota May 14, 2024
df603c4
chore: added maintainer (#7003)
go-sakayori May 14, 2024
6af8550
chore: update CODEOWNERS (#6866)
awf-autoware-bot[bot] May 14, 2024
5f7c39f
feat(smart_mpc_trajectory_follower): add smart_mpc_trajectory_followe…
masayukiaino May 14, 2024
b21e7bc
perf(behavior_path_dynamic_avoidance_module): use const reference (#6…
veqcc May 14, 2024
6c3cf79
refactor(turn_signal_decider): straddle bound method (#7006)
danielsanchezaran May 14, 2024
87137d3
feat(goal_planner): reject candidate path whose start pose direction …
soblin May 14, 2024
7763aba
refactor(behavior_velocity_planner_common): move VelocityFactorInterf…
maxime-clem May 15, 2024
14be153
fix(start_planner): issue when ego does not straddle lane bounds and …
danielsanchezaran May 15, 2024
6b3fb9b
chore(simple_planning_simulator): publish control mode before the sel…
tkimura4 May 15, 2024
7341da8
Update sensing/pointcloud_preprocessor/src/distortion_corrector/disto…
vividf May 16, 2024
1e93097
set the parameter the requirement
vividf May 16, 2024
042a833
add explanation to readme
vividf May 16, 2024
32e1beb
fix readme
vividf May 16, 2024
293824d
fix more readme
vividf May 16, 2024
1a27488
feat: componentize-system_error_monitor (#7009)
TetsuKawa May 15, 2024
9852abd
refactor(autoware_planning_test_manager): rename package (#6995)
zulfaqar-azmi-t4 May 15, 2024
b5edd91
feat(diagnostic_graph_aggregator): componentize node (#7025)
isamu-takagi May 15, 2024
a2b8711
feat(component_interface_tools): componentize node (#7023)
isamu-takagi May 15, 2024
7d66313
chore(simple_planning_simulator): add maintainer (#7026)
zulfaqar-azmi-t4 May 15, 2024
68c976b
build(static_centerline_generator): prefix package and namespace with…
esteve May 15, 2024
8d57d3f
perf(map_height_fitter): find z value with a more efficient closest p…
maxime-clem May 15, 2024
069bac2
fix(autoware_static_centerline_generator): remove prefix from topics …
esteve May 15, 2024
fabb0e0
refactor(route_handler): remove unused functions in route_handler (#7…
mkquda May 16, 2024
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
The table of contents is too big for display.
Diff view
Diff view
38 changes: 22 additions & 16 deletions .github/CODEOWNERS

Large diffs are not rendered by default.

4 changes: 4 additions & 0 deletions .github/PULL_REQUEST_TEMPLATE/small-change.md
Original file line number Diff line number Diff line change
@@ -15,6 +15,10 @@ Not applicable.

Not applicable.

## Interface changes

<!-- Describe any changed interfaces, such as topics, services, or parameters, including debugging interfaces -->

## Pre-review checklist for the PR author

The PR author **must** check the checkboxes below when creating the PR.
13 changes: 13 additions & 0 deletions .github/PULL_REQUEST_TEMPLATE/standard-change.md
Original file line number Diff line number Diff line change
@@ -18,6 +18,19 @@

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

### ROS Topic Changes

<!-- | Topic Name | Type | Direction | Update Description | -->
<!-- | ---------------- | ------------------- | --------- | ------------------------------------------------------------- | -->
<!-- | `/example_topic` | `std_msgs/String` | Subscribe | Description of what the topic is used for in the system | -->
<!-- | `/another_topic` | `sensor_msgs/Image` | Publish | Also explain if it is added / modified / deleted with the PR | -->

### ROS Parameter Changes

<!-- | Parameter Name | Default Value | Update Description | -->
<!-- | -------------------- | ------------- | --------------------------------------------------- | -->
<!-- | `example_parameters` | `1.0` | Describe the parameter and also explain the updates | -->

## Effects on system behavior

<!-- Describe how this PR affects the system behavior. -->
4 changes: 4 additions & 0 deletions build_depends.repos
Original file line number Diff line number Diff line change
@@ -41,3 +41,7 @@ repositories:
type: git
url: https://github.com/MORAI-Autonomous/MORAI-ROS2_morai_msgs.git
version: main
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)
type: git
url: https://github.com/tier4/glog.git
version: v0.6.0_t4-ros
Original file line number Diff line number Diff line change
@@ -37,7 +37,9 @@ namespace types
// We don't currently require code to comply to MISRA, but we should try to where it is
// easily possible.
using bool8_t = bool;
#if __cplusplus < 201811L || !__cpp_char8_t
using char8_t = char;
#endif
using uchar8_t = unsigned char;
// If we ever compile on a platform where this is not true, float32_t and float64_t definitions
// need to be adjusted.
Original file line number Diff line number Diff line change
@@ -0,0 +1,108 @@
cmake_minimum_required(VERSION 3.8)
project(autoware_mission_details_overlay_rviz_plugin)

find_package(ament_cmake_auto REQUIRED)
ament_auto_find_build_dependencies()

set(
headers_to_moc
include/mission_details_display.hpp
)

set(
headers_to_not_moc
include/remaining_distance_time_display.hpp
)

foreach(header "${headers_to_moc}")
qt5_wrap_cpp(display_moc_files "${header}")
endforeach()

set(
display_source_files
src/overlay_utils.cpp
src/mission_details_display.cpp
src/remaining_distance_time_display.cpp
)

add_library(
${PROJECT_NAME} SHARED
${display_moc_files}
${headers_to_not_moc}
${display_source_files}
)

set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 17)
target_compile_options(${PROJECT_NAME} PRIVATE -Wall -Wextra -Wpedantic)

target_include_directories(
${PROJECT_NAME} PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
${Qt5Widgets_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
)

target_link_libraries(
${PROJECT_NAME} PUBLIC
rviz_ogre_vendor::OgreMain
rviz_ogre_vendor::OgreOverlay
)

target_compile_definitions(${PROJECT_NAME} PRIVATE "${PROJECT_NAME}_BUILDING_LIBRARY")

# prevent pluginlib from using boost
target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")

pluginlib_export_plugin_description_file(rviz_common plugins_description.xml)

ament_target_dependencies(
${PROJECT_NAME}
PUBLIC
rviz_common
rviz_rendering
autoware_internal_msgs
)

ament_export_include_directories(include)
ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET)
ament_export_dependencies(
rviz_common
rviz_ogre_vendor
)

if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
endif()

install(
TARGETS ${PROJECT_NAME}
EXPORT ${PROJECT_NAME}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
INCLUDES DESTINATION include
)

install(
DIRECTORY include/
DESTINATION include
)

install(
TARGETS
DESTINATION lib/${PROJECT_NAME}
)

# Copy the assets directory to the installation directory
install(
DIRECTORY assets/
DESTINATION share/${PROJECT_NAME}/assets
)

add_definitions(-DQT_NO_KEYWORDS)

ament_package(
CONFIG_EXTRAS "autoware_mission_details_overlay_rviz_plugin-extras.cmake"
)
Original file line number Diff line number Diff line change
@@ -0,0 +1,12 @@
Copyright (c) 2022, Team Spatzenhirn
Copyright (c) 2014, JSK Lab

Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.

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.

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.

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 number Diff line number Diff line change
@@ -0,0 +1,35 @@
# autoware_mission_details_overlay_rviz_plugin

This RViz plugin displays the remaining distance and time for the current mission.

## Inputs / Outputs

### Input

| Name | Type | Description |
| ------------------------------------------- | ----------------------------------------------------------- | ---------------------------------------------------- |
| `/planning/mission_remaining_distance_time` | `autoware_planning_msgs::msg::MissionRemainingDistanceTime` | The topic is for mission remaining distance and time |

## Overlay Parameters

| Name | Type | Default Value | Description |
| -------- | ---- | ------------- | --------------------------------- |
| `Width` | int | 170 | Width of the overlay [px] |
| `Height` | int | 100 | Height of the overlay [px] |
| `Right` | int | 10 | Margin from the right border [px] |
| `Top` | int | 10 | Margin from the top border [px] |

The mission details display is aligned with top right corner of the screen.

## Usage

Similar to [autoware_overlay_rviz_plugin](../autoware_overlay_rviz_plugin/README.md)

## Credits

Based on the [jsk_visualization](https://github.com/jsk-ros-pkg/jsk_visualization) package.

### Icons

- <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>
- <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>
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Original file line number Diff line number Diff line change
@@ -0,0 +1,93 @@
Copyright 2011 The Quicksand Project Authors (https://github.com/andrew-paglinawan/QuicksandFamily), with Reserved Font Name “Quicksand”.

This Font Software is licensed under the SIL Open Font License, Version 1.1.
This license is copied below, and is also available with a FAQ at:
http://scripts.sil.org/OFL


-----------------------------------------------------------
SIL OPEN FONT LICENSE Version 1.1 - 26 February 2007
-----------------------------------------------------------

PREAMBLE
The goals of the Open Font License (OFL) are to stimulate worldwide
development of collaborative font projects, to support the font creation
efforts of academic and linguistic communities, and to provide a free and
open framework in which fonts may be shared and improved in partnership
with others.

The OFL allows the licensed fonts to be used, studied, modified and
redistributed freely as long as they are not sold by themselves. The
fonts, including any derivative works, can be bundled, embedded,
redistributed and/or sold with any software provided that any reserved
names are not used by derivative works. The fonts and derivatives,
however, cannot be released under any other type of license. The
requirement for fonts to remain under this license does not apply
to any document created using the fonts or their derivatives.

DEFINITIONS
"Font Software" refers to the set of files released by the Copyright
Holder(s) under this license and clearly marked as such. This may
include source files, build scripts and documentation.

"Reserved Font Name" refers to any names specified as such after the
copyright statement(s).

"Original Version" refers to the collection of Font Software components as
distributed by the Copyright Holder(s).

"Modified Version" refers to any derivative made by adding to, deleting,
or substituting -- in part or in whole -- any of the components of the
Original Version, by changing formats or by porting the Font Software to a
new environment.

"Author" refers to any designer, engineer, programmer, technical
writer or other person who contributed to the Font Software.

PERMISSION & CONDITIONS
Permission is hereby granted, free of charge, to any person obtaining
a copy of the Font Software, to use, study, copy, merge, embed, modify,
redistribute, and sell modified and unmodified copies of the Font
Software, subject to the following conditions:

1) Neither the Font Software nor any of its individual components,
in Original or Modified Versions, may be sold by itself.

2) Original or Modified Versions of the Font Software may be bundled,
redistributed and/or sold with any software, provided that each copy
contains the above copyright notice and this license. These can be
included either as stand-alone text files, human-readable headers or
in the appropriate machine-readable metadata fields within text or
binary files as long as those fields can be easily viewed by the user.

3) No Modified Version of the Font Software may use the Reserved Font
Name(s) unless explicit written permission is granted by the corresponding
Copyright Holder. This restriction only applies to the primary font name as
presented to the users.

4) The name(s) of the Copyright Holder(s) or the Author(s) of the Font
Software shall not be used to promote, endorse or advertise any
Modified Version, except to acknowledge the contribution(s) of the
Copyright Holder(s) and the Author(s) or with their explicit written
permission.

5) The Font Software, modified or unmodified, in part or in whole,
must be distributed entirely under this license, and must not be
distributed under any other license. The requirement for fonts to
remain under this license does not apply to any document created
using the Font Software.

TERMINATION
This license becomes null and void if any of the above conditions are
not met.

DISCLAIMER
THE FONT SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO ANY WARRANTIES OF
MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT
OF COPYRIGHT, PATENT, TRADEMARK, OR OTHER RIGHT. IN NO EVENT SHALL THE
COPYRIGHT HOLDER BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
INCLUDING ANY GENERAL, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL
DAMAGES, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
FROM, OUT OF THE USE OR INABILITY TO USE THE FONT SOFTWARE OR FROM
OTHER DEALINGS IN THE FONT SOFTWARE.
Binary file not shown.
Binary file not shown.
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Original file line number Diff line number Diff line change
@@ -0,0 +1,31 @@
# Copyright (c) 2021, Open Source Robotics Foundation, Inc. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
#
# * 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.
#
# * 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.
#
# 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.

# find package Qt5 because otherwise using the rviz_default_plugins::rviz_default_plugins
# exported target will complain that the Qt5::Widgets target does not exist
find_package(Qt5 REQUIRED QUIET COMPONENTS Widgets)
Original file line number Diff line number Diff line change
@@ -0,0 +1,83 @@
// Copyright 2024 The Autoware Contributors
//
// 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 MISSION_DETAILS_DISPLAY_HPP_
#define MISSION_DETAILS_DISPLAY_HPP_
#ifndef Q_MOC_RUN
#include "overlay_utils.hpp"
#include "remaining_distance_time_display.hpp"

#include <QImage>
#include <QString>
#include <rviz_common/display.hpp>
#include <rviz_common/properties/color_property.hpp>
#include <rviz_common/properties/float_property.hpp>
#include <rviz_common/properties/int_property.hpp>
#include <rviz_common/properties/ros_topic_property.hpp>

#include <OgreColourValue.h>
#include <OgreMaterial.h>
#include <OgreTexture.h>

#include <memory>
#include <mutex>
#endif

namespace autoware::mission_details_overlay_rviz_plugin
{
class MissionDetailsDisplay : public rviz_common::Display
{
Q_OBJECT
public:
MissionDetailsDisplay();
~MissionDetailsDisplay() override;

protected:
void onInitialize() override;
void update(float wall_dt, float ros_dt) override;
void reset() override;
void onEnable() override;
void onDisable() override;

private Q_SLOTS:
void update_size();
void topic_updated_remaining_distance_time();

private:
std::mutex mutex_;
autoware::mission_details_overlay_rviz_plugin::OverlayObject::SharedPtr overlay_;
rviz_common::properties::IntProperty * property_width_;
rviz_common::properties::IntProperty * property_height_;
rviz_common::properties::IntProperty * property_right_;
rviz_common::properties::IntProperty * property_top_;
std::unique_ptr<rviz_common::properties::RosTopicProperty>
remaining_distance_time_topic_property_;

void draw_rounded_rect(QPainter & painter, const QRectF & backgroundRect);
void setupRosSubscriptions();

std::unique_ptr<RemainingDistanceTimeDisplay> remaining_distance_time_display_;

rclcpp::Subscription<autoware_internal_msgs::msg::MissionRemainingDistanceTime>::SharedPtr
remaining_distance_time_sub_;

std::mutex property_mutex_;

void cb_remaining_distance_time(
const autoware_internal_msgs::msg::MissionRemainingDistanceTime::ConstSharedPtr & msg);
void draw_widget(QImage & hud);
};
} // namespace autoware::mission_details_overlay_rviz_plugin

#endif // MISSION_DETAILS_DISPLAY_HPP_
Original file line number Diff line number Diff line change
@@ -0,0 +1,131 @@
// Copyright 2024 The Autoware Contributors
//
// 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.

// -*- mode: c++ -*-
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2022, Team Spatzenhirn
* Copyright (c) 2014, JSK Lab
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/o2r other materials provided
* with the distribution.
* * Neither the name of the JSK Lab nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* 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 OWNER 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.
*********************************************************************/

#ifndef OVERLAY_UTILS_HPP_
#define OVERLAY_UTILS_HPP_

#include <QColor>
#include <QImage>

#include <OgreHardwarePixelBuffer.h>
#include <OgreMaterialManager.h>
#include <OgreTechnique.h>
#include <OgreTexture.h>
#include <OgreTextureManager.h>
#include <Overlay/OgreOverlay.h>
#include <Overlay/OgreOverlayContainer.h>
#include <Overlay/OgreOverlayElement.h>
#include <Overlay/OgreOverlayManager.h>
#include <Overlay/OgrePanelOverlayElement.h>

#include <memory>
#include <string>

namespace autoware::mission_details_overlay_rviz_plugin
{
class OverlayObject;

class ScopedPixelBuffer
{
public:
explicit ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer);
virtual ~ScopedPixelBuffer();
virtual Ogre::HardwarePixelBufferSharedPtr getPixelBuffer();
virtual QImage getQImage(unsigned int width, unsigned int height);
virtual QImage getQImage(OverlayObject & overlay);
virtual QImage getQImage(unsigned int width, unsigned int height, QColor & bg_color);
virtual QImage getQImage(OverlayObject & overlay, QColor & bg_color);

protected:
Ogre::HardwarePixelBufferSharedPtr pixel_buffer_;
};

enum class VerticalAlignment : uint8_t { CENTER, TOP, BOTTOM };

enum class HorizontalAlignment : uint8_t { LEFT, RIGHT, CENTER };

/**
* Helper class for realizing an overlay object on top of the rviz 3D panel.
*
* This class is supposed to be instantiated in the onInitialize method of the
* rviz_common::Display class.
*/
class OverlayObject
{
public:
using SharedPtr = std::shared_ptr<OverlayObject>;

explicit OverlayObject(const std::string & name);
virtual ~OverlayObject();

virtual std::string getName() const;
virtual void hide();
virtual void show();
virtual bool isTextureReady() const;
virtual void updateTextureSize(unsigned int width, unsigned int height);
virtual ScopedPixelBuffer getBuffer();
virtual void setPosition(
double hor_dist, double ver_dist, HorizontalAlignment hor_alignment = HorizontalAlignment::LEFT,
VerticalAlignment ver_alignment = VerticalAlignment::TOP);
virtual void setDimensions(double width, double height);
virtual bool isVisible() const;
virtual unsigned int getTextureWidth() const;
virtual unsigned int getTextureHeight() const;

protected:
const std::string name_;
Ogre::Overlay * overlay_;
Ogre::PanelOverlayElement * panel_;
Ogre::MaterialPtr panel_material_;
Ogre::TexturePtr texture_;
};
} // namespace autoware::mission_details_overlay_rviz_plugin

#endif // OVERLAY_UTILS_HPP_
Original file line number Diff line number Diff line change
@@ -0,0 +1,57 @@
// Copyright 2024 The Autoware Contributors
//
// 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 REMAINING_DISTANCE_TIME_DISPLAY_HPP_
#define REMAINING_DISTANCE_TIME_DISPLAY_HPP_
#include "overlay_utils.hpp"

#include <QImage>
#include <QString>
#include <rviz_common/properties/color_property.hpp>
#include <rviz_common/properties/float_property.hpp>
#include <rviz_common/properties/int_property.hpp>
#include <rviz_common/ros_topic_display.hpp>

#include <autoware_internal_msgs/msg/mission_remaining_distance_time.hpp>

#include <OgreColourValue.h>
#include <OgreMaterial.h>
#include <OgreTexture.h>

namespace autoware::mission_details_overlay_rviz_plugin
{

class RemainingDistanceTimeDisplay
{
public:
RemainingDistanceTimeDisplay();
void drawRemainingDistanceTimeDisplay(QPainter & painter, const QRectF & backgroundRect);
void updateRemainingDistanceTimeData(
const autoware_internal_msgs::msg::MissionRemainingDistanceTime::ConstSharedPtr & msg);

private:
double remaining_distance_; // Internal variable to store remaining distance
double remaining_time_; // Internal variable to store remaining time

QColor gray = QColor(194, 194, 194);

QImage icon_dist_;
QImage icon_dist_scaled_;
QImage icon_time_;
QImage icon_time_scaled_;
};

} // namespace autoware::mission_details_overlay_rviz_plugin

#endif // REMAINING_DISTANCE_TIME_DISPLAY_HPP_
Original file line number Diff line number Diff line change
@@ -0,0 +1,28 @@
<?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>autoware_mission_details_overlay_rviz_plugin</name>
<version>0.0.1</version>
<description>
RViz2 plugin for 2D overlays for mission details in the 3D view. Mainly a port of the JSK overlay plugin
(https://github.com/jsk-ros-pkg/jsk_visualization).
</description>
<maintainer email="ahmed.ebrahim@leodrive.ai">Ahmed Ebrahim</maintainer>

<license>BSD-3-Clause</license>

<depend>autoware_internal_msgs</depend>
<depend>boost</depend>
<depend>rviz_common</depend>
<depend>rviz_ogre_vendor</depend>
<depend>rviz_rendering</depend>

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

<buildtool_depend>ament_cmake_auto</buildtool_depend>

<export>
<build_type>ament_cmake</build_type>
</export>
</package>
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
<library path="autoware_mission_details_overlay_rviz_plugin">
<class name="autoware_mission_details_overlay_rviz_plugin/MissionDetailsDisplay" type="autoware::mission_details_overlay_rviz_plugin::MissionDetailsDisplay" base_class_type="rviz_common::Display">
<description>Mission Details overlay plugin for the 3D view.</description>
</class>
</library>
Original file line number Diff line number Diff line change
@@ -0,0 +1,212 @@
// Copyright 2024 The Autoware Contributors
//
// 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 "mission_details_display.hpp"

#include <QFontDatabase>
#include <QPainter>
#include <rclcpp/rclcpp.hpp>
#include <rviz_common/properties/ros_topic_property.hpp>
#include <rviz_rendering/render_system.hpp>

#include <algorithm>
#include <iomanip>
#include <memory>
#include <mutex>
#include <string>

namespace autoware::mission_details_overlay_rviz_plugin
{

MissionDetailsDisplay::MissionDetailsDisplay()
{
property_width_ = new rviz_common::properties::IntProperty(
"Width", 170, "Width of the overlay", this, SLOT(update_size()));
property_height_ = new rviz_common::properties::IntProperty(
"Height", 100, "Height of the overlay", this, SLOT(update_size()));
property_right_ = new rviz_common::properties::IntProperty(
"Right", 10, "Margin from the right border", this, SLOT(update_size()));
property_top_ = new rviz_common::properties::IntProperty(
"Top", 10, "Margin from the top border", this, SLOT(update_size()));

// Initialize the component displays
remaining_distance_time_display_ = std::make_unique<RemainingDistanceTimeDisplay>();
}

void MissionDetailsDisplay::onInitialize()
{
std::lock_guard<std::mutex> lock(property_mutex_);

rviz_common::Display::onInitialize();
rviz_rendering::RenderSystem::get()->prepareOverlays(scene_manager_);
static int count = 0;
std::stringstream ss;
ss << "MissionDetailsDisplay" << count++;
overlay_ =
std::make_shared<autoware::mission_details_overlay_rviz_plugin::OverlayObject>(ss.str());
overlay_->show();
update_size();

auto rviz_ros_node = context_->getRosNodeAbstraction();

remaining_distance_time_topic_property_ =
std::make_unique<rviz_common::properties::RosTopicProperty>(
"Remaining Distance and Time Topic", "/planning/mission_remaining_distance_time",
"autoware_internal_msgs/msg/MissionRemainingDistanceTime",
"Topic for Mission Remaining Distance and Time Data", this,
SLOT(topic_updated_remaining_distance_time()));
remaining_distance_time_topic_property_->initialize(rviz_ros_node);
}

void MissionDetailsDisplay::setupRosSubscriptions()
{
// Don't create a node, just use the one from the parent class
auto rviz_node_ = context_->getRosNodeAbstraction().lock()->get_raw_node();

remaining_distance_time_sub_ =
rviz_node_->create_subscription<autoware_internal_msgs::msg::MissionRemainingDistanceTime>(
remaining_distance_time_topic_property_->getTopicStd(),
rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
[this](const autoware_internal_msgs::msg::MissionRemainingDistanceTime::SharedPtr msg) {
cb_remaining_distance_time(msg);
});
}

Check warning on line 84 in common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/src/mission_details_display.cpp

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Code Duplication

The module contains 2 functions with similar structure: MissionDetailsDisplay::setupRosSubscriptions,MissionDetailsDisplay::topic_updated_remaining_distance_time. Avoid duplicated, aka copy-pasted, code inside the module. More duplication lowers the code health.

MissionDetailsDisplay::~MissionDetailsDisplay()
{
std::lock_guard<std::mutex> lock(property_mutex_);
overlay_.reset();

remaining_distance_time_sub_.reset();
remaining_distance_time_display_.reset();
remaining_distance_time_topic_property_.reset();
}

// mark maybe unused
void MissionDetailsDisplay::update(float wall_dt, float ros_dt)
{
(void)wall_dt; // Mark as unused
(void)ros_dt; // Mark as unused

if (!overlay_) {
return;
}
autoware::mission_details_overlay_rviz_plugin::ScopedPixelBuffer buffer = overlay_->getBuffer();
QImage hud = buffer.getQImage(*overlay_);
hud.fill(Qt::transparent);
draw_widget(hud);
}

void MissionDetailsDisplay::onEnable()
{
std::lock_guard<std::mutex> lock(property_mutex_);
if (overlay_) {
overlay_->show();
}
setupRosSubscriptions();
}

void MissionDetailsDisplay::onDisable()
{
std::lock_guard<std::mutex> lock(property_mutex_);

remaining_distance_time_sub_.reset();

if (overlay_) {
overlay_->hide();
}
}

void MissionDetailsDisplay::cb_remaining_distance_time(
const autoware_internal_msgs::msg::MissionRemainingDistanceTime::ConstSharedPtr & msg)
{
std::lock_guard<std::mutex> lock(property_mutex_);

if (remaining_distance_time_display_) {
remaining_distance_time_display_->updateRemainingDistanceTimeData(msg);
queueRender();
}
}

void MissionDetailsDisplay::draw_widget(QImage & hud)
{
std::lock_guard<std::mutex> lock(property_mutex_);

if (!overlay_->isVisible()) {
return;
}

QPainter painter(&hud);
painter.setRenderHint(QPainter::Antialiasing, true);

QRectF backgroundRect(0, 0, qreal(property_width_->getInt()), qreal(property_height_->getInt()));
draw_rounded_rect(painter, backgroundRect);

if (remaining_distance_time_display_) {
remaining_distance_time_display_->drawRemainingDistanceTimeDisplay(painter, backgroundRect);
}

painter.end();
}

void MissionDetailsDisplay::draw_rounded_rect(QPainter & painter, const QRectF & backgroundRect)
{
painter.setRenderHint(QPainter::Antialiasing, true);
QColor colorFromHSV;
colorFromHSV.setHsv(0, 0, 29);
colorFromHSV.setAlphaF(0.60);

painter.setBrush(colorFromHSV);

painter.setPen(Qt::NoPen);
painter.drawRoundedRect(backgroundRect, backgroundRect.height() / 2, backgroundRect.height() / 2);
}

void MissionDetailsDisplay::reset()
{
rviz_common::Display::reset();
overlay_->hide();
}

void MissionDetailsDisplay::update_size()
{
std::lock_guard<std::mutex> lock(mutex_);
overlay_->updateTextureSize(property_width_->getInt(), property_height_->getInt());
overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight());
overlay_->setPosition(
property_right_->getInt(), property_top_->getInt(), HorizontalAlignment::RIGHT,
VerticalAlignment::TOP);
queueRender();
}

void MissionDetailsDisplay::topic_updated_remaining_distance_time()
{
// resubscribe to the topic
remaining_distance_time_sub_.reset();
auto rviz_ros_node = context_->getRosNodeAbstraction().lock();
remaining_distance_time_sub_ =
rviz_ros_node->get_raw_node()
->create_subscription<autoware_internal_msgs::msg::MissionRemainingDistanceTime>(
remaining_distance_time_topic_property_->getTopicStd(),
rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
[this](const autoware_internal_msgs::msg::MissionRemainingDistanceTime::SharedPtr msg) {
cb_remaining_distance_time(msg);
});
}

} // namespace autoware::mission_details_overlay_rviz_plugin

#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(
autoware::mission_details_overlay_rviz_plugin::MissionDetailsDisplay, rviz_common::Display)
Original file line number Diff line number Diff line change
@@ -0,0 +1,267 @@
// Copyright 2024 The Autoware Contributors
//
// 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.

// -*- mode: c++ -*-
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2022, Team Spatzenhirn
* Copyright (c) 2014, JSK Lab
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/o2r other materials provided
* with the distribution.
* * Neither the name of the JSK Lab nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* 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 OWNER 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.
*********************************************************************/

#include "overlay_utils.hpp"

#include <rviz_common/logging.hpp>

namespace autoware::mission_details_overlay_rviz_plugin
{
ScopedPixelBuffer::ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer)
: pixel_buffer_(pixel_buffer)
{
pixel_buffer_->lock(Ogre::HardwareBuffer::HBL_NORMAL);
}

ScopedPixelBuffer::~ScopedPixelBuffer()
{
pixel_buffer_->unlock();
}

Ogre::HardwarePixelBufferSharedPtr ScopedPixelBuffer::getPixelBuffer()
{
return pixel_buffer_;
}

QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height)
{
const Ogre::PixelBox & pixelBox = pixel_buffer_->getCurrentLock();
Ogre::uint8 * pDest = static_cast<Ogre::uint8 *>(pixelBox.data);
memset(pDest, 0, width * height);
return QImage(pDest, width, height, QImage::Format_ARGB32);
}

QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height, QColor & bg_color)
{
QImage Hud = getQImage(width, height);
for (unsigned int i = 0; i < width; i++) {
for (unsigned int j = 0; j < height; j++) {
Hud.setPixel(i, j, bg_color.rgba());
}
}
return Hud;
}

QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay)
{
return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight());
}

QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay, QColor & bg_color)
{
return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight(), bg_color);
}

OverlayObject::OverlayObject(const std::string & name) : name_(name)
{
std::string material_name = name_ + "Material";
Ogre::OverlayManager * mOverlayMgr = Ogre::OverlayManager::getSingletonPtr();
overlay_ = mOverlayMgr->create(name_);
panel_ = static_cast<Ogre::PanelOverlayElement *>(
mOverlayMgr->createOverlayElement("Panel", name_ + "Panel"));
panel_->setMetricsMode(Ogre::GMM_PIXELS);

panel_material_ = Ogre::MaterialManager::getSingleton().create(
material_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
panel_->setMaterialName(panel_material_->getName());
overlay_->add2D(panel_);
}

OverlayObject::~OverlayObject()
{
Ogre::OverlayManager * mOverlayMgr = Ogre::OverlayManager::getSingletonPtr();
if (mOverlayMgr) {
mOverlayMgr->destroyOverlayElement(panel_);
mOverlayMgr->destroy(overlay_);
}
if (panel_material_) {
panel_material_->unload();
Ogre::MaterialManager::getSingleton().remove(panel_material_->getName());
}
}

std::string OverlayObject::getName() const
{
return name_;
}

void OverlayObject::hide()
{
if (overlay_->isVisible()) {
overlay_->hide();
}
}

void OverlayObject::show()
{
if (!overlay_->isVisible()) {
overlay_->show();
}
}

bool OverlayObject::isTextureReady() const
{
return texture_ != nullptr;
}

void OverlayObject::updateTextureSize(unsigned int width, unsigned int height)
{
const std::string texture_name = name_ + "Texture";
if (width == 0) {
RVIZ_COMMON_LOG_WARNING_STREAM("[OverlayObject] width=0 is specified as texture size");
width = 1;
}

if (height == 0) {
RVIZ_COMMON_LOG_WARNING_STREAM("[OverlayObject] height=0 is specified as texture size");
height = 1;
}

if (!isTextureReady() || ((width != texture_->getWidth()) || (height != texture_->getHeight()))) {

Check warning on line 167 in common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/src/overlay_utils.cpp

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Complex Conditional

OverlayObject::updateTextureSize has 1 complex conditionals with 2 branches, threshold = 2. A complex conditional is an expression inside a branch (e.g. if, for, while) which consists of multiple, logical operators such as AND/OR. The more logical operators in an expression, the more severe the code smell.
if (isTextureReady()) {
Ogre::TextureManager::getSingleton().remove(texture_name);
panel_material_->getTechnique(0)->getPass(0)->removeAllTextureUnitStates();
}

texture_ = Ogre::TextureManager::getSingleton().createManual(
texture_name, // name
Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
Ogre::TEX_TYPE_2D, // type
width, height, // width & height of the render window
0, // number of mipmaps
Ogre::PF_A8R8G8B8, // pixel format chosen to match a format Qt can use
Ogre::TU_DEFAULT // usage
);
panel_material_->getTechnique(0)->getPass(0)->createTextureUnitState(texture_name);

panel_material_->getTechnique(0)->getPass(0)->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA);
}
}

ScopedPixelBuffer OverlayObject::getBuffer()
{
if (isTextureReady()) {
return ScopedPixelBuffer(texture_->getBuffer());
} else {
return ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr());
}
}

void OverlayObject::setPosition(
double hor_dist, double ver_dist, HorizontalAlignment hor_alignment,
VerticalAlignment ver_alignment)
{
// ogre position is always based on the top left corner of the panel, while our position input
// depends on the chosen alignment, i.e. if the horizontal alignment is right, increasing the
// horizontal dist moves the panel to the left (further away from the right border)
double left = 0;
double top = 0;

switch (hor_alignment) {
case HorizontalAlignment::LEFT:
panel_->setHorizontalAlignment(Ogre::GuiHorizontalAlignment::GHA_LEFT);
left = hor_dist;
break;
case HorizontalAlignment::CENTER:
panel_->setHorizontalAlignment(Ogre::GuiHorizontalAlignment::GHA_CENTER);
left = hor_dist - panel_->getWidth() / 2;
break;
case HorizontalAlignment::RIGHT:
panel_->setHorizontalAlignment(Ogre::GuiHorizontalAlignment::GHA_RIGHT);
left = -hor_dist - panel_->getWidth();
break;
}

switch (ver_alignment) {
case VerticalAlignment::BOTTOM:
panel_->setVerticalAlignment(Ogre::GuiVerticalAlignment::GVA_BOTTOM);
top = -ver_dist - panel_->getHeight();
break;
case VerticalAlignment::CENTER:
panel_->setVerticalAlignment(Ogre::GuiVerticalAlignment::GVA_CENTER);
top = ver_dist - panel_->getHeight() / 2;
break;
case VerticalAlignment::TOP:
panel_->setVerticalAlignment(Ogre::GuiVerticalAlignment::GVA_TOP);
top = ver_dist;
break;
}

panel_->setPosition(left, top);
}

void OverlayObject::setDimensions(double width, double height)
{
panel_->setDimensions(width, height);
}

bool OverlayObject::isVisible() const
{
return overlay_->isVisible();
}

unsigned int OverlayObject::getTextureWidth() const
{
if (isTextureReady()) {
return texture_->getWidth();
} else {
return 0;
}
}

unsigned int OverlayObject::getTextureHeight() const
{
if (isTextureReady()) {
return texture_->getHeight();
} else {
return 0;
}
}
} // namespace autoware::mission_details_overlay_rviz_plugin
Original file line number Diff line number Diff line change
@@ -0,0 +1,157 @@
// Copyright 2024 The Autoware Contributors
//
// 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 "remaining_distance_time_display.hpp"

#include <QColor>
#include <QFontDatabase>
#include <QPainter>
#include <ament_index_cpp/get_package_share_directory.hpp>

#include <qpoint.h>

#include <cmath>
#include <iomanip>
#include <memory>
#include <string>

namespace autoware::mission_details_overlay_rviz_plugin
{

RemainingDistanceTimeDisplay::RemainingDistanceTimeDisplay()
: remaining_distance_(0.0), remaining_time_(0.0)
{
std::string package_path =
ament_index_cpp::get_package_share_directory("autoware_mission_details_overlay_rviz_plugin");
std::string font_path = package_path + "/assets/font/Quicksand/static/Quicksand-Regular.ttf";
std::string font_path2 = package_path + "/assets/font/Quicksand/static/Quicksand-Bold.ttf";
int fontId = QFontDatabase::addApplicationFont(
font_path.c_str()); // returns -1 on failure (see docs for more info)
int fontId2 = QFontDatabase::addApplicationFont(
font_path2.c_str()); // returns -1 on failure (see docs for more info)
if (fontId == -1 || fontId2 == -1) {
std::cout << "Failed to load the Quicksand font.";
}

std::string dist_image = package_path + "/assets/path.png";
std::string time_image = package_path + "/assets/av_timer.png";
icon_dist_.load(dist_image.c_str());
icon_time_.load(time_image.c_str());
icon_dist_scaled_ = icon_dist_.scaled(28, 28, Qt::KeepAspectRatio, Qt::SmoothTransformation);
icon_time_scaled_ = icon_time_.scaled(28, 28, Qt::KeepAspectRatio, Qt::SmoothTransformation);
}

void RemainingDistanceTimeDisplay::updateRemainingDistanceTimeData(
const autoware_internal_msgs::msg::MissionRemainingDistanceTime::ConstSharedPtr & msg)
{
remaining_distance_ = msg->remaining_distance;
remaining_time_ = msg->remaining_time;
}

void RemainingDistanceTimeDisplay::drawRemainingDistanceTimeDisplay(
QPainter & painter, const QRectF & backgroundRect)
{
const QFont font("Quicksand", 15, QFont::Bold);
painter.setFont(font);

// We'll display the distance and time in two rows

auto calculate_inner_rect = [](const QRectF & outer_rect, double ratio_x, double ratio_y) {
QSizeF size_inner(outer_rect.width() * ratio_x, outer_rect.height() * ratio_y);
QPointF top_left_inner = QPointF(
outer_rect.center().x() - size_inner.width() / 2,
outer_rect.center().y() - size_inner.height() / 2);
return QRectF(top_left_inner, size_inner);
};

QRectF rect_inner = calculate_inner_rect(backgroundRect, 0.7, 0.7);
// Proportionally extend the rect to the right to account for visual icon distance to the left
rect_inner.setWidth(rect_inner.width() * 1.08);

// Calculate distance row rectangle
const QSizeF distance_row_size(rect_inner.width(), rect_inner.height() / 2);
const QPointF distance_row_top_left(rect_inner.topLeft());
const QRectF distance_row_rect_outer(distance_row_top_left, distance_row_size);

// Calculate time row rectangle
const QSizeF time_row_size(rect_inner.width(), rect_inner.height() / 2);
const QPointF time_row_top_left(
rect_inner.topLeft().x(), rect_inner.topLeft().y() + rect_inner.height() / 2);
const QRectF time_row_rect_outer(time_row_top_left, time_row_size);

const auto rect_time = calculate_inner_rect(time_row_rect_outer, 1.0, 0.9);
const auto rect_dist = calculate_inner_rect(distance_row_rect_outer, 1.0, 0.9);

auto place_row = [&, this](
const QRectF & rect, const QImage & icon, const QString & str_value,
const QString & str_unit) {
// order: icon, value, unit

// place the icon
QPointF icon_top_left(rect.topLeft().x(), rect.center().y() - icon.height() / 2.0);
painter.drawImage(icon_top_left, icon);

// place the unit text
const float unit_width = 40.0;
QRectF rect_text_unit(
rect.topRight().x() - unit_width, rect.topRight().y(), unit_width, rect.height());

painter.setPen(gray);
painter.drawText(rect_text_unit, Qt::AlignLeft | Qt::AlignVCenter, str_unit);

// place the value text
const double margin_x = 5.0; // margin around the text

const double used_width = icon.width() + rect_text_unit.width() + margin_x * 2.0;

QRectF rect_text(
rect.topLeft().x() + icon.width() + margin_x, rect.topLeft().y(), rect.width() - used_width,
rect.height());

painter.drawText(rect_text, Qt::AlignRight | Qt::AlignVCenter, str_value);
};

// remaining_time_ is in seconds
if (remaining_time_ <= 60) {
place_row(rect_time, icon_time_scaled_, QString::number(remaining_time_, 'f', 0), "sec");
} else if (remaining_time_ <= 600) {
place_row(rect_time, icon_time_scaled_, QString::number(remaining_time_ / 60.0, 'f', 1), "min");
} else if (remaining_time_ <= 3600) {
place_row(rect_time, icon_time_scaled_, QString::number(remaining_time_ / 60.0, 'f', 0), "min");
} else if (remaining_time_ <= 36000) {
place_row(
rect_time, icon_time_scaled_, QString::number(remaining_time_ / 3600.0, 'f', 1), "hrs");
} else {
place_row(
rect_time, icon_time_scaled_, QString::number(remaining_time_ / 3600.0, 'f', 0), "hrs");
}

// remaining_distance_ is in meters
if (remaining_distance_ <= 10) {
place_row(rect_dist, icon_dist_scaled_, QString::number(remaining_distance_, 'f', 1), "m");
} else if (remaining_distance_ <= 1000) {
place_row(rect_dist, icon_dist_scaled_, QString::number(remaining_distance_, 'f', 0), "m");
} else if (remaining_distance_ <= 10000) {
place_row(
rect_dist, icon_dist_scaled_, QString::number(remaining_distance_ / 1000.0, 'f', 2), "km");
} else if (remaining_distance_ <= 100000) {
place_row(
rect_dist, icon_dist_scaled_, QString::number(remaining_distance_ / 1000.0, 'f', 1), "km");
} else {
place_row(
rect_dist, icon_dist_scaled_, QString::number(remaining_distance_ / 1000.0, 'f', 0), "km");
}
}

Check warning on line 155 in common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/src/remaining_distance_time_display.cpp

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Complex Method

RemainingDistanceTimeDisplay::drawRemainingDistanceTimeDisplay has a cyclomatic complexity of 9, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

} // namespace autoware::mission_details_overlay_rviz_plugin

This file was deleted.

This file was deleted.

This file was deleted.

This file was deleted.

Original file line number Diff line number Diff line change
@@ -3,28 +3,13 @@ project(autoware_overlay_rviz_plugin)

# find dependencies
find_package(ament_cmake_auto REQUIRED)
find_package(ament_cmake REQUIRED)
find_package(autoware_auto_vehicle_msgs REQUIRED)
find_package(tier4_planning_msgs REQUIRED)
find_package(autoware_perception_msgs REQUIRED)
ament_auto_find_build_dependencies()

find_package(autoware_overlay_msgs REQUIRED)

find_package(rviz_common REQUIRED)
find_package(rviz_rendering REQUIRED)
find_package(rviz_ogre_vendor REQUIRED)
find_package(std_msgs REQUIRED)

set(
headers_to_moc
include/overlay_text_display.hpp
include/signal_display.hpp

set(headers_to_moc
include/signal_display.hpp
)

set(
headers_to_not_moc
set(headers_to_not_moc
include/steering_wheel_display.hpp
include/gear_display.hpp
include/speed_display.hpp
@@ -34,47 +19,42 @@ set(
)



foreach(header "${headers_to_moc}")
qt5_wrap_cpp(display_moc_files "${header}")
qt5_wrap_cpp(display_moc_files "${header}")
endforeach()

set(
display_source_files
src/overlay_text_display.cpp
src/overlay_utils.cpp
src/signal_display.cpp
src/steering_wheel_display.cpp
src/gear_display.cpp
src/speed_display.cpp
src/turn_signals_display.cpp
src/traffic_display.cpp
src/speed_limit_display.cpp

set(display_source_files
src/overlay_utils.cpp
src/signal_display.cpp
src/steering_wheel_display.cpp
src/gear_display.cpp
src/speed_display.cpp
src/turn_signals_display.cpp
src/traffic_display.cpp
src/speed_limit_display.cpp
)

add_library(
${PROJECT_NAME} SHARED
${display_moc_files}
${headers_to_not_moc}
${display_source_files}
add_library(${PROJECT_NAME} SHARED
${display_moc_files}
${headers_to_not_moc}
${display_source_files}
)

set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 17)
target_compile_options(${PROJECT_NAME} PRIVATE -Wall -Wextra -Wpedantic)

target_include_directories(
${PROJECT_NAME} PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
${Qt5Widgets_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
${PROJECT_NAME} PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
${Qt5Widgets_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
)

target_link_libraries(
${PROJECT_NAME} PUBLIC
rviz_ogre_vendor::OgreMain
rviz_ogre_vendor::OgreOverlay
${PROJECT_NAME} PUBLIC
rviz_ogre_vendor::OgreMain
rviz_ogre_vendor::OgreOverlay
)


@@ -85,22 +65,19 @@ target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNC

pluginlib_export_plugin_description_file(rviz_common plugins_description.xml)

ament_target_dependencies(
${PROJECT_NAME}
PUBLIC
rviz_common
rviz_rendering
autoware_overlay_msgs
autoware_auto_vehicle_msgs
tier4_planning_msgs
autoware_perception_msgs
ament_target_dependencies(${PROJECT_NAME} PUBLIC
rviz_common
rviz_rendering
autoware_auto_vehicle_msgs
tier4_planning_msgs
autoware_perception_msgs
)

ament_export_include_directories(include)
ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET)
ament_export_dependencies(
rviz_common
rviz_ogre_vendor
rviz_common
rviz_ogre_vendor
)

if(BUILD_TESTING)
@@ -109,32 +86,32 @@ if(BUILD_TESTING)
endif()

install(
TARGETS ${PROJECT_NAME}
EXPORT ${PROJECT_NAME}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
INCLUDES DESTINATION include
TARGETS ${PROJECT_NAME}
EXPORT ${PROJECT_NAME}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
INCLUDES DESTINATION include
)

install(
DIRECTORY include/
DESTINATION include
DIRECTORY include/
DESTINATION include
)

install(
TARGETS
DESTINATION lib/${PROJECT_NAME}
TARGETS
DESTINATION lib/${PROJECT_NAME}
)

# Copy the assets directory to the installation directory
install(
DIRECTORY assets/
DESTINATION share/${PROJECT_NAME}/assets
DIRECTORY assets/
DESTINATION share/${PROJECT_NAME}/assets
)

add_definitions(-DQT_NO_KEYWORDS)

ament_package(
CONFIG_EXTRAS "autoware_overlay_rviz_plugin-extras.cmake"
CONFIG_EXTRAS "autoware_overlay_rviz_plugin-extras.cmake"
)

This file was deleted.

Original file line number Diff line number Diff line change
@@ -54,8 +54,6 @@
#include <QColor>
#include <QImage>

#include "autoware_overlay_msgs/msg/overlay_text.hpp"

#include <OgreHardwarePixelBuffer.h>
#include <OgreMaterialManager.h>
#include <OgreTechnique.h>
@@ -89,17 +87,9 @@ class ScopedPixelBuffer
Ogre::HardwarePixelBufferSharedPtr pixel_buffer_;
};

enum class VerticalAlignment : uint8_t {
CENTER = autoware_overlay_msgs::msg::OverlayText::CENTER,
TOP = autoware_overlay_msgs::msg::OverlayText::TOP,
BOTTOM = autoware_overlay_msgs::msg::OverlayText::BOTTOM,
};
enum class VerticalAlignment : uint8_t { CENTER, TOP, BOTTOM };

enum class HorizontalAlignment : uint8_t {
LEFT = autoware_overlay_msgs::msg::OverlayText::LEFT,
RIGHT = autoware_overlay_msgs::msg::OverlayText::RIGHT,
CENTER = autoware_overlay_msgs::msg::OverlayText::CENTER
};
enum class HorizontalAlignment : uint8_t { LEFT, RIGHT, CENTER };

/**
* Helper class for realizing an overlay object on top of the rviz 3D panel.
Original file line number Diff line number Diff line change
@@ -47,7 +47,7 @@ class TurnSignalsDisplay

private:
QImage arrowImage;
QColor gray = QColor(46, 46, 46);
QColor gray = QColor(79, 79, 79);

int current_turn_signal_; // Internal variable to store turn signal state
int current_hazard_lights_; // Internal variable to store hazard lights state
Original file line number Diff line number Diff line change
@@ -12,7 +12,6 @@
<license>BSD-3-Clause</license>

<depend>autoware_auto_vehicle_msgs</depend>
<depend>autoware_overlay_msgs</depend>
<depend>autoware_perception_msgs</depend>
<depend>boost</depend>
<depend>rviz_common</depend>
Original file line number Diff line number Diff line change
@@ -81,18 +81,19 @@ void GearDisplay::drawGearIndicator(QPainter & painter, const QRectF & backgroun
break;
}

QFont gearFont("Quicksand", 16, QFont::Bold);
QFont gearFont("Quicksand", 12, QFont::Bold);
painter.setFont(gearFont);
QPen borderPen(gray);
borderPen.setWidth(4);
borderPen.setWidth(1);
painter.setPen(borderPen);

int gearBoxSize = 30;
int gearX = backgroundRect.left() + 30 + gearBoxSize;
int gearY = backgroundRect.height() - gearBoxSize - 20;
double gearBoxSize = 37.5;
double gearX = backgroundRect.left() + 54;
double gearY = backgroundRect.height() / 2 - gearBoxSize / 2;
QRect gearRect(gearX, gearY, gearBoxSize, gearBoxSize);
painter.setBrush(QColor(0, 0, 0, 0));
painter.setBrush(gray);
painter.drawRoundedRect(gearRect, 10, 10);
painter.setPen(Qt::black);
painter.drawText(gearRect, Qt::AlignCenter, QString::fromStdString(gearString));
}

This file was deleted.

Original file line number Diff line number Diff line change
@@ -39,11 +39,11 @@ namespace autoware_overlay_rviz_plugin
SignalDisplay::SignalDisplay()
{
property_width_ = new rviz_common::properties::IntProperty(
"Width", 517, "Width of the overlay", this, SLOT(updateOverlaySize()));
"Width", 550, "Width of the overlay", this, SLOT(updateOverlaySize()));
property_height_ = new rviz_common::properties::IntProperty(
"Height", 175, "Height of the overlay", this, SLOT(updateOverlaySize()));
"Height", 100, "Height of the overlay", this, SLOT(updateOverlaySize()));
property_left_ = new rviz_common::properties::IntProperty(
"Left", 10, "Left position of the overlay", this, SLOT(updateOverlayPosition()));
"Left", 0, "Left position of the overlay", this, SLOT(updateOverlayPosition()));
property_top_ = new rviz_common::properties::IntProperty(
"Top", 10, "Top position of the overlay", this, SLOT(updateOverlayPosition()));
property_signal_color_ = new rviz_common::properties::ColorProperty(
@@ -120,59 +120,13 @@ void SignalDisplay::onInitialize()

void SignalDisplay::setupRosSubscriptions()
{
// Don't create a node, just use the one from the parent class
auto rviz_node_ = context_->getRosNodeAbstraction().lock()->get_raw_node();

gear_sub_ = rviz_node_->create_subscription<autoware_auto_vehicle_msgs::msg::GearReport>(
gear_topic_property_->getTopicStd(),
rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
[this](const autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr msg) {
updateGearData(msg);
});

steering_sub_ = rviz_node_->create_subscription<autoware_auto_vehicle_msgs::msg::SteeringReport>(
steering_topic_property_->getTopicStd(),
rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
[this](const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg) {
updateSteeringData(msg);
});

speed_sub_ = rviz_node_->create_subscription<autoware_auto_vehicle_msgs::msg::VelocityReport>(
speed_topic_property_->getTopicStd(),
rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
[this](const autoware_auto_vehicle_msgs::msg::VelocityReport::SharedPtr msg) {
updateSpeedData(msg);
});

turn_signals_sub_ =
rviz_node_->create_subscription<autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport>(
turn_signals_topic_property_->getTopicStd(),
rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
[this](const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::SharedPtr msg) {
updateTurnSignalsData(msg);
});

hazard_lights_sub_ =
rviz_node_->create_subscription<autoware_auto_vehicle_msgs::msg::HazardLightsReport>(
hazard_lights_topic_property_->getTopicStd(),
rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
[this](const autoware_auto_vehicle_msgs::msg::HazardLightsReport::SharedPtr msg) {
updateHazardLightsData(msg);
});

traffic_sub_ = rviz_node_->create_subscription<autoware_perception_msgs::msg::TrafficSignal>(
traffic_topic_property_->getTopicStd(),
rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
[this](const autoware_perception_msgs::msg::TrafficSignal::SharedPtr msg) {
updateTrafficLightData(msg);
});

speed_limit_sub_ = rviz_node_->create_subscription<tier4_planning_msgs::msg::VelocityLimit>(
speed_limit_topic_property_->getTopicStd(),
rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
[this](const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg) {
updateSpeedLimitData(msg);
});
topic_updated_gear();
topic_updated_steering();
topic_updated_speed();
topic_updated_speed_limit();
topic_updated_turn_signals();
topic_updated_hazard_lights();
topic_updated_traffic();
}

SignalDisplay::~SignalDisplay()
@@ -325,35 +279,31 @@ void SignalDisplay::drawWidget(QImage & hud)
QPainter painter(&hud);
painter.setRenderHint(QPainter::Antialiasing, true);

QRectF backgroundRect(0, 0, 322, hud.height());
QRectF backgroundRect(0, 0, 550, hud.height());
drawHorizontalRoundedRectangle(painter, backgroundRect);

// Draw components
if (steering_wheel_display_) {
steering_wheel_display_->drawSteeringWheel(painter, backgroundRect);
}
if (gear_display_) {
gear_display_->drawGearIndicator(painter, backgroundRect);
}

if (steering_wheel_display_) {
steering_wheel_display_->drawSteeringWheel(painter, backgroundRect);
}

if (speed_display_) {
speed_display_->drawSpeedDisplay(painter, backgroundRect);
}
if (turn_signals_display_) {
turn_signals_display_->drawArrows(painter, backgroundRect, property_signal_color_->getColor());
}

// a 27px space between the two halves of the HUD

QRectF smallerBackgroundRect(340, 0, 190.0 / 2, hud.height());

drawVerticalRoundedRectangle(painter, smallerBackgroundRect);

if (traffic_display_) {
traffic_display_->drawTrafficLightIndicator(painter, smallerBackgroundRect);
traffic_display_->drawTrafficLightIndicator(painter, backgroundRect);
}

if (speed_limit_display_) {
speed_limit_display_->drawSpeedLimitIndicator(painter, smallerBackgroundRect);
speed_limit_display_->drawSpeedLimitIndicator(painter, backgroundRect);
}

painter.end();
@@ -364,8 +314,8 @@ void SignalDisplay::drawHorizontalRoundedRectangle(
{
painter.setRenderHint(QPainter::Antialiasing, true);
QColor colorFromHSV;
colorFromHSV.setHsv(0, 0, 0); // Hue, Saturation, Value
colorFromHSV.setAlphaF(0.65); // Transparency
colorFromHSV.setHsv(0, 0, 29); // Hue, Saturation, Value
colorFromHSV.setAlphaF(0.60); // Transparency

painter.setBrush(colorFromHSV);

@@ -404,7 +354,9 @@ void SignalDisplay::updateOverlaySize()
void SignalDisplay::updateOverlayPosition()
{
std::lock_guard<std::mutex> lock(mutex_);
overlay_->setPosition(property_left_->getInt(), property_top_->getInt());
overlay_->setPosition(
property_left_->getInt(), property_top_->getInt(), HorizontalAlignment::CENTER,
VerticalAlignment::TOP);
queueRender();
}

@@ -421,8 +373,7 @@ void SignalDisplay::topic_updated_gear()
auto rviz_ros_node = context_->getRosNodeAbstraction().lock();
gear_sub_ =
rviz_ros_node->get_raw_node()->create_subscription<autoware_auto_vehicle_msgs::msg::GearReport>(
gear_topic_property_->getTopicStd(),
rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
gear_topic_property_->getTopicStd(), rclcpp::QoS(rclcpp::KeepLast(10)),
[this](const autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr msg) {
updateGearData(msg);
});
@@ -435,8 +386,7 @@ void SignalDisplay::topic_updated_steering()
auto rviz_ros_node = context_->getRosNodeAbstraction().lock();
steering_sub_ = rviz_ros_node->get_raw_node()
->create_subscription<autoware_auto_vehicle_msgs::msg::SteeringReport>(
steering_topic_property_->getTopicStd(),
rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
steering_topic_property_->getTopicStd(), rclcpp::QoS(rclcpp::KeepLast(10)),
[this](const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg) {
updateSteeringData(msg);
});
@@ -449,8 +399,7 @@ void SignalDisplay::topic_updated_speed()
auto rviz_ros_node = context_->getRosNodeAbstraction().lock();
speed_sub_ = rviz_ros_node->get_raw_node()
->create_subscription<autoware_auto_vehicle_msgs::msg::VelocityReport>(
speed_topic_property_->getTopicStd(),
rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
speed_topic_property_->getTopicStd(), rclcpp::QoS(rclcpp::KeepLast(10)),
[this](const autoware_auto_vehicle_msgs::msg::VelocityReport::SharedPtr msg) {
updateSpeedData(msg);
});
@@ -464,7 +413,7 @@ void SignalDisplay::topic_updated_speed_limit()
speed_limit_sub_ =
rviz_ros_node->get_raw_node()->create_subscription<tier4_planning_msgs::msg::VelocityLimit>(
speed_limit_topic_property_->getTopicStd(),
rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
rclcpp::QoS(rclcpp::KeepLast(10)).transient_local(),
[this](const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg) {
updateSpeedLimitData(msg);
});
@@ -479,8 +428,7 @@ void SignalDisplay::topic_updated_turn_signals()
turn_signals_sub_ =
rviz_ros_node->get_raw_node()
->create_subscription<autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport>(
turn_signals_topic_property_->getTopicStd(),
rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
turn_signals_topic_property_->getTopicStd(), rclcpp::QoS(rclcpp::KeepLast(10)),
[this](const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::SharedPtr msg) {
updateTurnSignalsData(msg);
});
@@ -495,8 +443,7 @@ void SignalDisplay::topic_updated_hazard_lights()
hazard_lights_sub_ =
rviz_ros_node->get_raw_node()
->create_subscription<autoware_auto_vehicle_msgs::msg::HazardLightsReport>(
hazard_lights_topic_property_->getTopicStd(),
rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
hazard_lights_topic_property_->getTopicStd(), rclcpp::QoS(rclcpp::KeepLast(10)),
[this](const autoware_auto_vehicle_msgs::msg::HazardLightsReport::SharedPtr msg) {
updateHazardLightsData(msg);
});
@@ -509,8 +456,7 @@ void SignalDisplay::topic_updated_traffic()
auto rviz_ros_node = context_->getRosNodeAbstraction().lock();
traffic_sub_ = rviz_ros_node->get_raw_node()
->create_subscription<autoware_perception_msgs::msg::TrafficSignal>(
traffic_topic_property_->getTopicStd(),
rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
traffic_topic_property_->getTopicStd(), rclcpp::QoS(rclcpp::KeepLast(10)),
[this](const autoware_perception_msgs::msg::TrafficSignal::SharedPtr msg) {
updateTrafficLightData(msg);
});
Original file line number Diff line number Diff line change
@@ -85,7 +85,7 @@ void SpeedDisplay::drawSpeedDisplay(QPainter & painter, const QRectF & backgroun
backgroundRect.width() / 2 - referenceRect.width() / 2 - 5, backgroundRect.height() / 2);

QString speedNumber = QString::number(current_speed_, 'f', 0);
int fontSize = 60;
int fontSize = 40;
QFont speedFont("Quicksand", fontSize);
painter.setFont(speedFont);

@@ -94,16 +94,17 @@ void SpeedDisplay::drawSpeedDisplay(QPainter & painter, const QRectF & backgroun

// Center the speed number in the backgroundRect
QPointF speedPos(
backgroundRect.center().x() - speedNumberRect.width() / 2, backgroundRect.center().y());
backgroundRect.center().x() - speedNumberRect.width() / 2,
backgroundRect.center().y() + speedNumberRect.bottom());
painter.setPen(gray);
painter.drawText(speedPos, speedNumber);

QFont unitFont("Quicksand", 14);
QFont unitFont("Quicksand", 8, QFont::DemiBold);
painter.setFont(unitFont);
QString speedUnit = "km/h";
QRect unitRect = painter.fontMetrics().boundingRect(speedUnit);
QPointF unitPos(
(backgroundRect.width() / 2 - unitRect.width() / 2), referencePos.y() + unitRect.height());
(backgroundRect.width() / 2 - unitRect.width() / 2), referencePos.y() + unitRect.height() + 15);
painter.drawText(unitPos, speedUnit);
}

Original file line number Diff line number Diff line change
@@ -24,6 +24,7 @@
#include <OgreTechnique.h>
#include <OgreTexture.h>
#include <OgreTextureManager.h>
#include <qobject.h>

#include <algorithm>
#include <cmath>
@@ -99,10 +100,9 @@ void SpeedLimitDisplay::drawSpeedLimitIndicator(QPainter & painter, const QRectF
}

// Define the area for the outer circle
QRectF outerCircleRect = backgroundRect;
outerCircleRect.setWidth(backgroundRect.width() - 30);
outerCircleRect.setHeight(backgroundRect.width() - 30);
outerCircleRect.moveTopLeft(QPointF(backgroundRect.left() + 15, backgroundRect.top() + 10));
QRectF outerCircleRect = QRectF(45, 45, 45, 45);
outerCircleRect.moveTopRight(
QPointF(backgroundRect.right() - 44, backgroundRect.top() + outerCircleRect.height() / 2 + 5));

// Now use borderColor for drawing
painter.setPen(QPen(borderColor, 2, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
@@ -112,24 +112,30 @@ void SpeedLimitDisplay::drawSpeedLimitIndicator(QPainter & painter, const QRectF

// Define the area for the inner circle
QRectF innerCircleRect = outerCircleRect;
innerCircleRect.setWidth(outerCircleRect.width() / 1.25);
innerCircleRect.setHeight(outerCircleRect.height() / 1.25);
innerCircleRect.setWidth(outerCircleRect.width() / 1.09);
innerCircleRect.setHeight(outerCircleRect.height() / 1.09);
innerCircleRect.moveCenter(outerCircleRect.center());

QRectF innerCircleRect2 = innerCircleRect;

painter.setRenderHint(QPainter::Antialiasing, true);
QColor colorFromHSV;
colorFromHSV.setHsv(0, 0, 0); // Hue, Saturation, Value

colorFromHSV.setHsv(0, 0, 29); // Hue, Saturation, Value
colorFromHSV.setAlphaF(0.60); // Transparency
painter.setBrush(colorFromHSV);
painter.drawEllipse(innerCircleRect);

// Add a second inner circle as a mask to make the speed limit indicator look like a ring
// and follow the rest of the background color as close as possible
painter.drawEllipse(innerCircleRect2);

int current_limit_int = std::round(current_limit * 3.6);

// Define the text to be drawn
QString text = QString::number(current_limit_int);

// Set the font and color for the text
QFont font = QFont("Quicksand", 24, QFont::Bold);
QFont font = QFont("Quicksand", 16, QFont::Bold);

painter.setFont(font);
// #C2C2C2
Original file line number Diff line number Diff line change
@@ -82,8 +82,8 @@ void SteeringWheelDisplay::drawSteeringWheel(QPainter & painter, const QRectF &
// Rotate the wheel
float steeringAngle = steering_angle_; // No need to round here
// Calculate the position
int wheelCenterX = backgroundRect.right() - wheel.width() - 17.5;
int wheelCenterY = backgroundRect.height() - wheel.height() + 15;
int wheelCenterX = backgroundRect.left() + wheel.width() + 54 + 54;
int wheelCenterY = backgroundRect.height() / 2;

// Rotate the wheel image
QTransform rotationTransform;
Original file line number Diff line number Diff line change
@@ -62,12 +62,10 @@ void TrafficDisplay::drawTrafficLightIndicator(QPainter & painter, const QRectF
painter.setBrush(QBrush(tl_gray_, Qt::SolidPattern));
painter.setPen(Qt::NoPen);
// Define the area for the circle (background)
QRectF circleRect = backgroundRect;
circleRect.setWidth(backgroundRect.width() - 30);
circleRect.setHeight(backgroundRect.width() - 30);
QRectF circleRect = QRectF(50, 50, 50, 50);
circleRect.moveTopRight(QPointF(
backgroundRect.left() + circleRect.width() + 15,
backgroundRect.top() + circleRect.height() + 30));
backgroundRect.right() - circleRect.width() - 75,
backgroundRect.top() + circleRect.height() / 2));
painter.drawEllipse(circleRect);

if (!current_traffic_.elements.empty()) {
@@ -96,7 +94,7 @@ void TrafficDisplay::drawTrafficLightIndicator(QPainter & painter, const QRectF
}

// Scaling factor (e.g., 1.5 for 150% size)
float scaleFactor = 1.00;
float scaleFactor = 0.75;

// Calculate the scaled size
QSize scaledSize = traffic_light_image_.size() * scaleFactor;
Original file line number Diff line number Diff line change
@@ -72,12 +72,12 @@ void TurnSignalsDisplay::updateHazardLightsData(
void TurnSignalsDisplay::drawArrows(
QPainter & painter, const QRectF & backgroundRect, const QColor & color)
{
QImage scaledLeftArrow = arrowImage.scaled(64, 43, Qt::KeepAspectRatio, Qt::SmoothTransformation);
QImage scaledLeftArrow = arrowImage.scaled(50, 32, Qt::KeepAspectRatio, Qt::SmoothTransformation);
scaledLeftArrow = coloredImage(scaledLeftArrow, gray);
QImage scaledRightArrow = scaledLeftArrow.mirrored(true, false);
int arrowYPos = (backgroundRect.height() / 3 - scaledLeftArrow.height() / 2);
int leftArrowXPos = backgroundRect.width() / 4 - scaledLeftArrow.width(); // Adjust as needed
int rightArrowXPos = backgroundRect.width() * 3 / 4; // Adjust as needed
int arrowYPos = (backgroundRect.height() / 2 - scaledLeftArrow.height() / 2 - 4);
int leftArrowXPos = backgroundRect.left() + scaledLeftArrow.width() * 2 + 180;
int rightArrowXPos = backgroundRect.right() - scaledRightArrow.width() * 3 - 175;

bool leftActive =
(current_turn_signal_ == autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_LEFT ||
11 changes: 10 additions & 1 deletion common/component_interface_tools/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -3,5 +3,14 @@ project(component_interface_tools)

find_package(autoware_cmake REQUIRED)
autoware_package()
ament_auto_add_executable(service_log_checker src/service_log_checker.cpp)

ament_auto_add_library(${PROJECT_NAME} SHARED
src/service_log_checker.cpp
)

rclcpp_components_register_node(${PROJECT_NAME}
PLUGIN "ServiceLogChecker"
EXECUTABLE service_log_checker_node
)

ament_auto_package(INSTALL_TO_SHARE launch)
Original file line number Diff line number Diff line change
@@ -1,3 +1,3 @@
<launch>
<node pkg="component_interface_tools" exec="service_log_checker" name="service_log_checker"/>
<node pkg="component_interface_tools" exec="service_log_checker_node"/>
</launch>
1 change: 1 addition & 0 deletions common/component_interface_tools/package.xml
Original file line number Diff line number Diff line change
@@ -13,6 +13,7 @@
<depend>diagnostic_updater</depend>
<depend>fmt</depend>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
<depend>tier4_system_msgs</depend>
<depend>yaml_cpp_vendor</depend>

15 changes: 4 additions & 11 deletions common/component_interface_tools/src/service_log_checker.cpp
Original file line number Diff line number Diff line change
@@ -22,7 +22,8 @@
#define FMT_HEADER_ONLY
#include <fmt/format.h>

ServiceLogChecker::ServiceLogChecker() : Node("service_log_checker"), diagnostics_(this)
ServiceLogChecker::ServiceLogChecker(const rclcpp::NodeOptions & options)
: Node("service_log_checker", options), diagnostics_(this)
{
sub_ = create_subscription<ServiceLog>(
"/service_log", 50, std::bind(&ServiceLogChecker::on_service_log, this, std::placeholders::_1));
@@ -98,13 +99,5 @@ void ServiceLogChecker::update_diagnostics(diagnostic_updater::DiagnosticStatusW
}
}

int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
rclcpp::executors::SingleThreadedExecutor executor;
auto node = std::make_shared<ServiceLogChecker>();
executor.add_node(node);
executor.spin();
executor.remove_node(node);
rclcpp::shutdown();
}
#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(ServiceLogChecker)
Original file line number Diff line number Diff line change
@@ -26,7 +26,7 @@
class ServiceLogChecker : public rclcpp::Node
{
public:
ServiceLogChecker();
explicit ServiceLogChecker(const rclcpp::NodeOptions & options);

private:
using ServiceLog = tier4_system_msgs::msg::ServiceLog;
2 changes: 1 addition & 1 deletion common/glog_component/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -8,7 +8,7 @@ autoware_package()
ament_auto_add_library(glog_component SHARED
src/glog_component.cpp
)
target_link_libraries(glog_component glog)
target_link_libraries(glog_component glog::glog)

rclcpp_components_register_node(glog_component
PLUGIN "GlogComponent"
2 changes: 1 addition & 1 deletion common/glog_component/package.xml
Original file line number Diff line number Diff line change
@@ -13,7 +13,7 @@
<buildtool_depend>ament_cmake_auto</buildtool_depend>
<buildtool_depend>autoware_cmake</buildtool_depend>

<depend>libgoogle-glog-dev</depend>
<depend>glog</depend>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>

6 changes: 4 additions & 2 deletions common/glog_component/src/glog_component.cpp
Original file line number Diff line number Diff line change
@@ -17,8 +17,10 @@
GlogComponent::GlogComponent(const rclcpp::NodeOptions & node_options)
: Node("glog_component", node_options)
{
google::InitGoogleLogging("glog_component");
google::InstallFailureSignalHandler();
if (!google::IsGoogleLoggingInitialized()) {
google::InitGoogleLogging("glog_component");
google::InstallFailureSignalHandler();
}
}

#include <rclcpp_components/register_node_macro.hpp>
10 changes: 1 addition & 9 deletions common/motion_utils/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -7,15 +7,7 @@ autoware_package()
find_package(Boost REQUIRED)

ament_auto_add_library(motion_utils SHARED
src/distance/distance.cpp
src/marker/marker_helper.cpp
src/marker/virtual_wall_marker_creator.cpp
src/resample/resample.cpp
src/trajectory/trajectory.cpp
src/trajectory/interpolation.cpp
src/trajectory/path_with_lane_id.cpp
src/trajectory/conversion.cpp
src/vehicle/vehicle_state_checker.cpp
DIRECTORY src
)

if(BUILD_TESTING)
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@

// Copyright 2022 TIER IV, Inc.
// Copyright 2022-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.
@@ -13,8 +13,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__VELOCITY_FACTOR_INTERFACE_HPP_
#define BEHAVIOR_VELOCITY_PLANNER_COMMON__VELOCITY_FACTOR_INTERFACE_HPP_
#ifndef MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_
#define MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_

#include <autoware_adapi_v1_msgs/msg/planning_behavior.hpp>
#include <autoware_adapi_v1_msgs/msg/velocity_factor.hpp>
@@ -25,35 +25,31 @@
#include <string>
#include <vector>

namespace behavior_velocity_planner
namespace motion_utils
{

using autoware_adapi_v1_msgs::msg::PlanningBehavior;
using autoware_adapi_v1_msgs::msg::VelocityFactor;
using autoware_adapi_v1_msgs::msg::VelocityFactorArray;
using geometry_msgs::msg::Pose;
using VelocityFactorBehavior = VelocityFactor::_behavior_type;
using VelocityFactorStatus = VelocityFactor::_status_type;
using geometry_msgs::msg::Pose;

class VelocityFactorInterface
{
public:
VelocityFactorInterface() { behavior_ = VelocityFactor::UNKNOWN; }

VelocityFactor get() const { return velocity_factor_; }
void init(const VelocityFactorBehavior behavior) { behavior_ = behavior; }
[[nodiscard]] VelocityFactor get() const { return velocity_factor_; }
void init(const VelocityFactorBehavior & behavior) { behavior_ = behavior; }
void reset() { velocity_factor_.behavior = PlanningBehavior::UNKNOWN; }

void set(
const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & points,
const Pose & curr_pose, const Pose & stop_pose, const VelocityFactorStatus status,
const std::string detail = "");
const std::string & detail = "");

private:
VelocityFactorBehavior behavior_;
VelocityFactor velocity_factor_;
VelocityFactorBehavior behavior_{VelocityFactor::UNKNOWN};
VelocityFactor velocity_factor_{};
};

} // namespace behavior_velocity_planner
} // namespace motion_utils

#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__VELOCITY_FACTOR_INTERFACE_HPP_
#endif // MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_
1 change: 1 addition & 0 deletions common/motion_utils/package.xml
Original file line number Diff line number Diff line change
@@ -21,6 +21,7 @@
<buildtool_depend>ament_cmake_auto</buildtool_depend>
<buildtool_depend>autoware_cmake</buildtool_depend>

<depend>autoware_adapi_v1_msgs</depend>
<depend>autoware_auto_planning_msgs</depend>
<depend>autoware_auto_vehicle_msgs</depend>
<depend>builtin_interfaces</depend>
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2023 TIER IV, Inc.
// Copyright 2023-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.
@@ -12,23 +12,24 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#include <behavior_velocity_planner_common/velocity_factor_interface.hpp>
#include <motion_utils/factor/velocity_factor_interface.hpp>
#include <motion_utils/trajectory/trajectory.hpp>

namespace behavior_velocity_planner
namespace motion_utils
{
void VelocityFactorInterface::set(
const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & points,
const Pose & curr_pose, const Pose & stop_pose, const VelocityFactorStatus status,
const std::string detail)
const std::string & detail)
{
const auto & curr_point = curr_pose.position;
const auto & stop_point = stop_pose.position;
velocity_factor_.behavior = behavior_;
velocity_factor_.pose = stop_pose;
velocity_factor_.distance = motion_utils::calcSignedArcLength(points, curr_point, stop_point);
velocity_factor_.distance =
static_cast<float>(motion_utils::calcSignedArcLength(points, curr_point, stop_point));
velocity_factor_.status = status;
velocity_factor_.detail = detail;
}

} // namespace behavior_velocity_planner
} // namespace motion_utils
Original file line number Diff line number Diff line change
@@ -34,13 +34,19 @@ namespace tier4_calibration_rviz_plugin
AccelBrakeMapCalibratorButtonPanel::AccelBrakeMapCalibratorButtonPanel(QWidget * parent)
: rviz_common::Panel(parent)
{
topic_label_ = new QLabel("Topic name of update suggest ");
topic_label_ = new QLabel("topic: ");
topic_label_->setAlignment(Qt::AlignCenter);

topic_edit_ =
new QLineEdit("/vehicle/calibration/accel_brake_map_calibrator/output/update_suggest");
connect(topic_edit_, SIGNAL(textEdited(QString)), SLOT(editTopic()));

service_label_ = new QLabel("service: ");
service_label_->setAlignment(Qt::AlignCenter);

service_edit_ = new QLineEdit("/vehicle/calibration/accel_brake_map_calibrator/update_map_dir");
connect(service_edit_, SIGNAL(textEdited(QString)), SLOT(editService()));

calibration_button_ = new QPushButton("Wait for subscribe topic");
calibration_button_->setEnabled(false);
connect(calibration_button_, SIGNAL(clicked(bool)), SLOT(pushCalibrationButton()));
@@ -56,8 +62,13 @@ AccelBrakeMapCalibratorButtonPanel::AccelBrakeMapCalibratorButtonPanel(QWidget *
topic_layout->addWidget(topic_label_);
topic_layout->addWidget(topic_edit_);

auto * service_layout = new QHBoxLayout;
service_layout->addWidget(service_label_);
service_layout->addWidget(service_edit_);

auto * v_layout = new QVBoxLayout;
v_layout->addLayout(topic_layout);
v_layout->addLayout(service_layout);
v_layout->addWidget(calibration_button_);
v_layout->addWidget(status_label_);

@@ -75,7 +86,7 @@ void AccelBrakeMapCalibratorButtonPanel::onInitialize()
&AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest, this, std::placeholders::_1));

client_ = raw_node->create_client<tier4_vehicle_msgs::srv::UpdateAccelBrakeMap>(
"/vehicle/calibration/accel_brake_map_calibrator/update_map_dir");
service_edit_->text().toStdString());
}

void AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest(
@@ -85,6 +96,12 @@ void AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest(
return;
}

if (!client_ || !client_->service_is_ready()) {
calibration_button_->setText("wait for service");
calibration_button_->setEnabled(false);
return;
}

if (msg->data) {
status_label_->setText("Ready");
status_label_->setStyleSheet("QLabel { background-color : white;}");
@@ -98,17 +115,34 @@ void AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest(

void AccelBrakeMapCalibratorButtonPanel::editTopic()
{
update_suggest_sub_.reset();
rclcpp::Node::SharedPtr raw_node =
this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node();
update_suggest_sub_ = raw_node->create_subscription<std_msgs::msg::Bool>(
topic_edit_->text().toStdString(), 10,
std::bind(
&AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest, this, std::placeholders::_1));
try {
update_suggest_sub_.reset();
update_suggest_sub_ = raw_node->create_subscription<std_msgs::msg::Bool>(
topic_edit_->text().toStdString(), 10,
std::bind(
&AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest, this, std::placeholders::_1));
} catch (const rclcpp::exceptions::InvalidTopicNameError & e) {
RCLCPP_WARN_STREAM(raw_node->get_logger(), e.what());
}
calibration_button_->setText("Wait for subscribe topic");
calibration_button_->setEnabled(false);
}

void AccelBrakeMapCalibratorButtonPanel::editService()
{
rclcpp::Node::SharedPtr raw_node =
this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node();
try {
client_.reset();
client_ = raw_node->create_client<tier4_vehicle_msgs::srv::UpdateAccelBrakeMap>(
service_edit_->text().toStdString());
} catch (const rclcpp::exceptions::InvalidServiceNameError & e) {
RCLCPP_WARN_STREAM(raw_node->get_logger(), e.what());
}
}

void AccelBrakeMapCalibratorButtonPanel::pushCalibrationButton()
{
// lock button
Original file line number Diff line number Diff line change
@@ -46,6 +46,7 @@ class AccelBrakeMapCalibratorButtonPanel : public rviz_common::Panel

public Q_SLOTS: // NOLINT for Qt
void editTopic();
void editService();
void pushCalibrationButton();

protected:
@@ -56,6 +57,8 @@ public Q_SLOTS: // NOLINT for Qt

QLabel * topic_label_;
QLineEdit * topic_edit_;
QLabel * service_label_;
QLineEdit * service_edit_;
QPushButton * calibration_button_;
QLabel * status_label_;
};
Original file line number Diff line number Diff line change
@@ -33,6 +33,8 @@ Planning:
logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner
- node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
logger_name: tier4_autoware_utils
- node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
logger_name: behavior_path_planner.path_shifter

behavior_path_planner_avoidance:
- node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
39 changes: 29 additions & 10 deletions common/tier4_simulated_clock_rviz_plugin/README.md
Original file line number Diff line number Diff line change
@@ -10,18 +10,37 @@ This plugin allows publishing and controlling the simulated ROS time.
| -------- | --------------------------- | -------------------------- |
| `/clock` | `rosgraph_msgs::msg::Clock` | the current simulated time |

## HowToUse
## How to use the plugin

1. Launch [planning simulator](https://autowarefoundation.github.io/autoware-documentation/main/tutorials/ad-hoc-simulation/planning-simulation/#1-launch-autoware) with `use_sim_time:=true`.

```shell
ros2 launch autoware_launch planning_simulator.launch.xml map_path:=$HOME/autoware_map/sample-map-planning vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit use_sim_time:=true
```

> <span style="color: orange; font-weight: bold;">Warning</span>
> If you launch the planning simulator without adding the `tier4_simulated_clock_rviz_plugin`, your simulation will not be running. You'll not even be able to place the initial and the goal poses.
2. Start rviz and select panels/Add new panel.

1. Start rviz and select panels/Add new panel.
![select_panel](./images/select_panels.png)
2. Select tier4_clock_rviz_plugin/SimulatedClock and press OK.

3. Select tier4_clock_rviz_plugin/SimulatedClock and press OK.

![select_clock_plugin](./images/select_clock_plugin.png)
3. Use the added panel to control how the simulated clock is published.

4. Use the added panel to control how the simulated clock is published.

![use_clock_plugin](./images/use_clock_plugin.png)

- Pause button: pause/resume the clock.
- Speed: speed of the clock relative to the system clock.
- Rate: publishing rate of the clock.
- Step button: advance the clock by the specified time step.
- Time step: value used to advance the clock when pressing the step button d).
- Time unit: time unit associated with the value from e).
<ol type="a">
<li>Pause button: pause/resume the clock.</li>
<li>Speed: speed of the clock relative to the system clock.</li>
<li>Rate: publishing rate of the clock.</li>
<li>Step button: advance the clock by the specified time step.</li>
<li>Time step: value used to advance the clock when pressing the step button d).</li>
<li>Time unit: time unit associated with the value from e).</li>
</ol>

> <span style="color: orange; font-weight: bold;">Warning</span>
> If you set the time step too large, your simulation will go haywire.
2 changes: 2 additions & 0 deletions common/tvm_utility/example/yolo_v2_tiny/main.cpp
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2021-2022 Arm Limited and Contributors.

Check notice on line 1 in common/tvm_utility/example/yolo_v2_tiny/main.cpp

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Overall Code Complexity

The mean cyclomatic complexity increases from 4.14 to 4.43, threshold = 4. This file has many conditional statements (e.g. if, for, while) across its implementation, leading to lower code health. Avoid adding more conditionals.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
@@ -200,6 +200,8 @@
}
}

if (max_ind == -1) continue;

Check warning on line 204 in common/tvm_utility/example/yolo_v2_tiny/main.cpp

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Complex Method

schedule has a cyclomatic complexity of 10, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.
// Decode and copy class probabilities
std::vector<float> class_probabilities{};
float p_total = 0;
27 changes: 27 additions & 0 deletions control/autonomous_emergency_braking/README.md
Original file line number Diff line number Diff line change
@@ -108,6 +108,33 @@ After Noise filtering, it performs a geometric collision check to determine whet

Finally, the vertex that is closest to the ego vehicle is chosen as the candidate for collision checking: Since rss distance is used to judge if a collision will happen or not, if the closest vertex to the ego is deemed to be safe, the rest of the vertices (and the points in the clusters) will also be safe.

#### Obstacle velocity estimation

Once the position of the closest obstacle/point is determined, the AEB modules uses the history of previously detected objects to estimate the closest object speed using the following equations:

$$
d_{t} = o_{time stamp} - prev_{time stamp}
$$

$$
d_{pos} = norm(o_{pos} - prev_{pos})
$$

$$
v_{norm} = d_{pos} / d_{t}
$$

Where $o_{time stamp}$ and $prev_{time stamp}$ are the timestamps of the point clouds used to detect the current closest object and the closest object of the previous point cloud frame, and $o_{pos}$ and $prev_{pos}$ are the positions of those objects, respectively.

Finally, the velocity vector is compared against the ego's predicted path to get the longitudinal velocity $v_{obj}$:

$$
v_{obj} = v_{norm} * Cos(yaw_{diff}) + v_{ego}
$$

where $yaw_{diff}$ is the difference in yaw between the ego path and the displacement vector $$v_{pos} = o_{pos} - prev_{pos} $$ and $v_{ego}$ is the ego's speed, which accounts for the movement of points caused by the ego moving and not the object.
All these equations are performed disregarding the z axis (in 2D).

### 4. Collision check with target obstacles

In the fourth step, it checks the collision with filtered obstacles using RSS distance. RSS is formulated as:
Original file line number Diff line number Diff line change
@@ -1,26 +1,38 @@
/**:
ros__parameters:
publish_debug_pointcloud: false
# Ego path calculation
use_predicted_trajectory: true
use_imu_path: true
path_footprint_extra_margin: 1.0
use_imu_path: false
min_generated_path_length: 0.5
imu_prediction_time_horizon: 1.5
imu_prediction_time_interval: 0.1
mpc_prediction_time_horizon: 1.5
mpc_prediction_time_interval: 0.1

# Debug
publish_debug_pointcloud: false

# Point cloud partitioning
detection_range_min_height: 0.0
detection_range_max_height_margin: 0.0
voxel_grid_x: 0.05
voxel_grid_y: 0.05
voxel_grid_z: 100000.0
min_generated_path_length: 0.5

# Point cloud cropping
expand_width: 0.1
path_footprint_extra_margin: 4.0

# Point cloud clustering
cluster_tolerance: 0.1 #[m]
minimum_cluster_size: 10
maximum_cluster_size: 10000

# RSS distance collision check
longitudinal_offset: 2.0
t_response: 1.0
a_ego_min: -3.0
a_obj_min: -1.0
cluster_tolerance: 0.1 #[m]
minimum_cluster_size: 10
maximum_cluster_size: 10000
imu_prediction_time_horizon: 1.5
imu_prediction_time_interval: 0.1
mpc_prediction_time_horizon: 1.5
mpc_prediction_time_interval: 0.1
collision_keeping_sec: 0.0
collision_keeping_sec: 2.0
previous_obstacle_keep_time: 1.0
aeb_hz: 10.0
Original file line number Diff line number Diff line change
@@ -16,6 +16,7 @@
#define AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_

#include <diagnostic_updater/diagnostic_updater.hpp>
#include <motion_utils/trajectory/trajectory.hpp>
#include <pcl_ros/transforms.hpp>
#include <rclcpp/rclcpp.hpp>
#include <tier4_autoware_utils/geometry/geometry.hpp>
@@ -40,12 +41,13 @@
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

#include <deque>
#include <limits>
#include <memory>
#include <mutex>
#include <optional>
#include <string>
#include <utility>
#include <vector>

namespace autoware::motion::control::autonomous_emergency_braking
{

@@ -79,30 +81,146 @@ class CollisionDataKeeper
public:
explicit CollisionDataKeeper(rclcpp::Clock::SharedPtr clock) { clock_ = clock; }

void setTimeout(const double timeout_sec) { timeout_sec_ = timeout_sec; }
void setTimeout(const double collision_keep_time, const double previous_obstacle_keep_time)
{
collision_keep_time_ = collision_keep_time;
previous_obstacle_keep_time_ = previous_obstacle_keep_time;
}

std::pair<double, double> getTimeout()
{
return {collision_keep_time_, previous_obstacle_keep_time_};
}

bool checkExpired()
bool checkObjectDataExpired(std::optional<ObjectData> & data, const double timeout)
{
if (data_ && (clock_->now() - data_->stamp).seconds() > timeout_sec_) {
data_.reset();
if (!data.has_value()) return true;
const auto now = clock_->now();
const auto & prev_obj = data.value();
const auto & data_time_stamp = prev_obj.stamp;
if ((now - data_time_stamp).nanoseconds() * 1e-9 > timeout) {
data = std::nullopt;
return true;
}
return (data_ == nullptr);
return false;
}

bool checkCollisionExpired()
{
return this->checkObjectDataExpired(closest_object_, collision_keep_time_);
}

bool checkPreviousObjectDataExpired()
{
return this->checkObjectDataExpired(prev_closest_object_, previous_obstacle_keep_time_);
}

ObjectData get() const
{
return (closest_object_.has_value()) ? closest_object_.value() : ObjectData();
}

ObjectData getPreviousObjectData() const
{
return (prev_closest_object_.has_value()) ? prev_closest_object_.value() : ObjectData();
}

void setCollisionData(const ObjectData & data)
{
closest_object_ = std::make_optional<ObjectData>(data);
}

void setPreviousObjectData(const ObjectData & data)
{
prev_closest_object_ = std::make_optional<ObjectData>(data);
}

void resetVelocityHistory() { obstacle_velocity_history_.clear(); }

void updateVelocityHistory(
const double current_object_velocity, const rclcpp::Time & current_object_velocity_time_stamp)
{
// remove old msg from deque
const auto now = clock_->now();
obstacle_velocity_history_.erase(
std::remove_if(
obstacle_velocity_history_.begin(), obstacle_velocity_history_.end(),
[&](const auto & velocity_time_pair) {
const auto & vel_time = velocity_time_pair.second;
return ((now - vel_time).nanoseconds() * 1e-9 > previous_obstacle_keep_time_);
}),
obstacle_velocity_history_.end());
obstacle_velocity_history_.emplace_back(
std::make_pair(current_object_velocity, current_object_velocity_time_stamp));
}

void update(const ObjectData & data) { data_.reset(new ObjectData(data)); }
std::optional<double> getMedianObstacleVelocity() const
{
if (obstacle_velocity_history_.empty()) return std::nullopt;
std::vector<double> raw_velocities;
for (const auto & vel_time_pair : obstacle_velocity_history_) {
raw_velocities.emplace_back(vel_time_pair.first);
}

const size_t med1 = (raw_velocities.size() % 2 == 0) ? (raw_velocities.size()) / 2 - 1
: (raw_velocities.size()) / 2.0;
const size_t med2 = (raw_velocities.size()) / 2.0;
std::nth_element(raw_velocities.begin(), raw_velocities.begin() + med1, raw_velocities.end());
const double vel1 = raw_velocities.at(med1);
std::nth_element(raw_velocities.begin(), raw_velocities.begin() + med2, raw_velocities.end());
const double vel2 = raw_velocities.at(med2);
return (vel1 + vel2) / 2.0;
}

ObjectData get()
std::optional<double> calcObjectSpeedFromHistory(
const ObjectData & closest_object, const Path & path, const double current_ego_speed)
{
if (data_) {
return *data_;
} else {
return ObjectData();
if (this->checkPreviousObjectDataExpired()) {
this->setPreviousObjectData(closest_object);
this->resetVelocityHistory();
return std::nullopt;
}

const auto estimated_velocity_opt = std::invoke([&]() -> std::optional<double> {
const auto & prev_object = this->getPreviousObjectData();
const double p_dt =
(closest_object.stamp.nanoseconds() - prev_object.stamp.nanoseconds()) * 1e-9;
if (p_dt < std::numeric_limits<double>::epsilon()) return std::nullopt;
const auto & nearest_collision_point = closest_object.position;
const auto & prev_collision_point = prev_object.position;

const double p_dx = nearest_collision_point.x - prev_collision_point.x;
const double p_dy = nearest_collision_point.y - prev_collision_point.y;
const double p_dist = std::hypot(p_dx, p_dy);
const double p_yaw = std::atan2(p_dy, p_dx);
const double p_vel = p_dist / p_dt;

const auto nearest_idx = motion_utils::findNearestIndex(path, nearest_collision_point);
const auto & nearest_path_pose = path.at(nearest_idx);
const auto & traj_yaw = tf2::getYaw(nearest_path_pose.orientation);
const auto estimated_velocity = p_vel * std::cos(p_yaw - traj_yaw) + current_ego_speed;

// Current RSS distance calculation does not account for negative velocities
return (estimated_velocity > 0.0) ? estimated_velocity : 0.0;
});

if (!estimated_velocity_opt.has_value()) {
return std::nullopt;
}

const auto & estimated_velocity = estimated_velocity_opt.value();
this->setPreviousObjectData(closest_object);
this->updateVelocityHistory(estimated_velocity, closest_object.stamp);
return this->getMedianObstacleVelocity();
}

private:
std::unique_ptr<ObjectData> data_;
double timeout_sec_{0.0};
std::optional<ObjectData> prev_closest_object_{std::nullopt};
std::optional<ObjectData> closest_object_{std::nullopt};
double collision_keep_time_{0.0};
double previous_obstacle_keep_time_{0.0};

std::deque<std::pair<double, rclcpp::Time>> obstacle_velocity_history_;
rclcpp::Clock::SharedPtr clock_;
};

@@ -132,6 +250,8 @@ class AEB : public rclcpp::Node
void onTimer();
void onPredictedTrajectory(const Trajectory::ConstSharedPtr input_msg);
void onAutowareState(const AutowareState::ConstSharedPtr input_msg);
rcl_interfaces::msg::SetParametersResult onParameter(
const std::vector<rclcpp::Parameter> & parameters);

bool isDataReady();

@@ -152,14 +272,22 @@ class AEB : public rclcpp::Node
void cropPointCloudWithEgoFootprintPath(
const std::vector<Polygon2d> & ego_polys, pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_objects);

void createObjectDataUsingPointCloudClusters(
const Path & ego_path, const std::vector<Polygon2d> & ego_polys, const rclcpp::Time & stamp,
std::vector<ObjectData> & objects);
void cropPointCloudWithEgoFootprintPath(const std::vector<Polygon2d> & ego_polys);

void addMarker(
const rclcpp::Time & current_time, const Path & path, const std::vector<Polygon2d> & polygons,
const std::vector<ObjectData> & objects, const double color_r, const double color_g,
const double color_b, const double color_a, const std::string & ns,
MarkerArray & debug_markers);
const std::vector<ObjectData> & objects, const std::optional<ObjectData> & closest_object,
const double color_r, const double color_g, const double color_b, const double color_a,
const std::string & ns, MarkerArray & debug_markers);

void addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers);

std::optional<double> calcObjectSpeedFromHistory(
const ObjectData & closest_object, const Path & path, const double current_ego_speed);

PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr};
VelocityReport::ConstSharedPtr current_velocity_ptr_{nullptr};
Vector3::SharedPtr angular_velocity_ptr_{nullptr};
@@ -199,6 +327,8 @@ class AEB : public rclcpp::Node
double mpc_prediction_time_horizon_;
double mpc_prediction_time_interval_;
CollisionDataKeeper collision_data_keeper_;
// Parameter callback
OnSetParametersCallbackHandle::SharedPtr set_param_res_;
};
} // namespace autoware::motion::control::autonomous_emergency_braking

Loading