Skip to content

Commit 7050ea0

Browse files
yukkysaitoyukkysaito
authored
feat: visualize candidate trajectories (#16)
1 parent f3cd639 commit 7050ea0

12 files changed

Lines changed: 1620 additions & 0 deletions

File tree

‎autoware_planning_rviz_plugin/CMakeLists.txt‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,16 @@ ament_auto_add_library(autoware_planning_rviz_plugin SHARED
2121
include/autoware_planning_rviz_plugin/path/display_base.hpp
2222
include/autoware_planning_rviz_plugin/path/display.hpp
2323
src/path/display.cpp
24+
# candidate trajectories
25+
include/autoware_planning_rviz_plugin/candidate_trajectories/display_base.hpp
26+
src/candidate_trajectories/display_base.cpp
27+
include/autoware_planning_rviz_plugin/candidate_trajectories/candidate_trajectories_display.hpp
28+
src/candidate_trajectories/candidate_trajectories_display.cpp
29+
include/autoware_planning_rviz_plugin/candidate_trajectories/scored_candidate_trajectories_display.hpp
30+
src/candidate_trajectories/scored_candidate_trajectories_display.cpp
31+
# common utilities
32+
include/autoware_planning_rviz_plugin/common/color_utils.hpp
33+
src/common/color_utils.cpp
2434
# footprint
2535
include/autoware_planning_rviz_plugin/pose_with_uuid_stamped/display.hpp
2636
src/pose_with_uuid_stamped/display.cpp

‎autoware_planning_rviz_plugin/icons/classes/CandidateTrajectories.png‎

18.4 KB
Loading

‎autoware_planning_rviz_plugin/icons/classes/ScoredCandidateTrajectories.png‎

18.4 KB
Loading

‎autoware_planning_rviz_plugin/include/autoware_planning_rviz_plugin/candidate_trajectories/candidate_trajectories_display.hpp‎

Lines changed: 81 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,81 @@
1+
// Copyright 2025 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef AUTOWARE_PLANNING_RVIZ_PLUGIN__CANDIDATE_TRAJECTORIES__CANDIDATE_TRAJECTORIES_DISPLAY_HPP_ // NOLINT
16+
#define AUTOWARE_PLANNING_RVIZ_PLUGIN__CANDIDATE_TRAJECTORIES__CANDIDATE_TRAJECTORIES_DISPLAY_HPP_ // NOLINT
17+
18+
#include "autoware_planning_rviz_plugin/candidate_trajectories/display_base.hpp"
19+
20+
#include <rviz_common/properties/color_property.hpp>
21+
#include <rviz_common/properties/enum_property.hpp>
22+
23+
#include <autoware_internal_planning_msgs/msg/candidate_trajectories.hpp>
24+
25+
#include <memory>
26+
#include <vector>
27+
28+
namespace rviz_plugins
29+
{
30+
class AutowareCandidateTrajectoriesDisplay
31+
: public CandidateTrajectoriesDisplayBase<
32+
autoware_internal_planning_msgs::msg::CandidateTrajectories>
33+
{
34+
Q_OBJECT
35+
36+
public:
37+
AutowareCandidateTrajectoriesDisplay();
38+
~AutowareCandidateTrajectoriesDisplay() override;
39+
40+
protected Q_SLOTS:
41+
void updateVisualization();
42+
void updateColoringModeVisibility();
43+
void onTopicChanged();
44+
45+
protected:
46+
// Override base class virtual methods
47+
void setupColoringModes() override;
48+
void processMessage(
49+
const autoware_internal_planning_msgs::msg::CandidateTrajectories::ConstSharedPtr msg_ptr)
50+
override;
51+
bool validateFloats(
52+
const autoware_internal_planning_msgs::msg::CandidateTrajectories::ConstSharedPtr & msg_ptr)
53+
override;
54+
55+
/// Coloring modes available for candidate trajectories
56+
enum CandidateColoringMode {
57+
CANDIDATE_VELOCITY_BASED = 0, ///< Color trajectories based on velocity
58+
INDEX_BASED = 1 ///< Color trajectories based on their index (cyclical colors)
59+
};
60+
61+
// Index-based coloring properties
62+
rviz_common::properties::ColorProperty property_index_color_1_{
63+
"Index Color 1", QColor("#FF8080"), "", &property_coloring_mode_};
64+
rviz_common::properties::ColorProperty property_index_color_2_{
65+
"Index Color 2", QColor("#80FF80"), "", &property_coloring_mode_};
66+
rviz_common::properties::ColorProperty property_index_color_3_{
67+
"Index Color 3", QColor("#8080FF"), "", &property_coloring_mode_};
68+
rviz_common::properties::ColorProperty property_index_color_4_{
69+
"Index Color 4", QColor("#FFFF80"), "", &property_coloring_mode_};
70+
71+
// Helper method for non-scored trajectory coloring
72+
std::unique_ptr<Ogre::ColourValue> setColorDependsOnIndex(const size_t index);
73+
74+
private:
75+
std::vector<QColor> index_colors_;
76+
void updateIndexColors();
77+
};
78+
} // namespace rviz_plugins
79+
80+
// NOLINTNEXTLINE
81+
#endif // AUTOWARE_PLANNING_RVIZ_PLUGIN__CANDIDATE_TRAJECTORIES__CANDIDATE_TRAJECTORIES_DISPLAY_HPP_

‎autoware_planning_rviz_plugin/include/autoware_planning_rviz_plugin/candidate_trajectories/display_base.hpp‎

Lines changed: 122 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,122 @@
1+
// Copyright 2025 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef AUTOWARE_PLANNING_RVIZ_PLUGIN__CANDIDATE_TRAJECTORIES__DISPLAY_BASE_HPP_
16+
#define AUTOWARE_PLANNING_RVIZ_PLUGIN__CANDIDATE_TRAJECTORIES__DISPLAY_BASE_HPP_
17+
18+
#include "autoware_planning_rviz_plugin/common/color_utils.hpp"
19+
20+
#include <rclcpp/rclcpp.hpp>
21+
#include <rviz_common/display.hpp>
22+
#include <rviz_common/properties/bool_property.hpp>
23+
#include <rviz_common/properties/color_property.hpp>
24+
#include <rviz_common/properties/enum_property.hpp>
25+
#include <rviz_common/properties/float_property.hpp>
26+
#include <rviz_common/properties/ros_topic_property.hpp>
27+
#include <rviz_rendering/objects/movable_text.hpp>
28+
29+
#include <autoware_internal_planning_msgs/msg/candidate_trajectories.hpp>
30+
#include <autoware_internal_planning_msgs/msg/scored_candidate_trajectories.hpp>
31+
32+
#include <OgreManualObject.h>
33+
#include <OgreSceneNode.h>
34+
35+
#include <memory>
36+
#include <vector>
37+
38+
namespace rviz_plugins
39+
{
40+
41+
template <typename MessageType>
42+
class CandidateTrajectoriesDisplayBase : public rviz_common::Display
43+
{
44+
public:
45+
CandidateTrajectoriesDisplayBase();
46+
virtual ~CandidateTrajectoriesDisplayBase();
47+
48+
void onInitialize() override;
49+
void onEnable() override;
50+
void onDisable() override;
51+
void reset() override;
52+
53+
protected:
54+
// Pure virtual methods for message type specialization
55+
/// Process received ROS message and update visualization
56+
virtual void processMessage(const typename MessageType::ConstSharedPtr msg_ptr) = 0;
57+
/// Setup coloring mode options specific to the message type
58+
virtual void setupColoringModes() = 0;
59+
60+
// Common functionality
61+
virtual bool validateFloats(const typename MessageType::ConstSharedPtr & msg_ptr);
62+
virtual std::unique_ptr<Ogre::ColourValue> setColorDependsOnVelocity(const double velocity);
63+
virtual std::unique_ptr<Ogre::ColourValue> gradation(
64+
const QColor & color_min, const QColor & color_max, const double ratio);
65+
66+
// Common initialization methods
67+
void initializePropertyConstraints();
68+
69+
// Common visualization methods
70+
void resizeManualObjects(size_t num_trajectories);
71+
void clearManualObjects();
72+
73+
// Common properties - topic first for UI display order
74+
rviz_common::properties::RosTopicProperty property_topic_;
75+
76+
// Path visualization properties
77+
rviz_common::properties::BoolProperty property_path_view_;
78+
rviz_common::properties::BoolProperty property_path_width_view_;
79+
rviz_common::properties::FloatProperty property_path_width_;
80+
rviz_common::properties::FloatProperty property_path_alpha_;
81+
rviz_common::properties::EnumProperty property_coloring_mode_;
82+
rviz_common::properties::FloatProperty property_fade_out_distance_;
83+
84+
// Velocity-based coloring properties (common to all)
85+
rviz_common::properties::ColorProperty property_velocity_color_min_;
86+
rviz_common::properties::ColorProperty property_velocity_color_mid_;
87+
rviz_common::properties::ColorProperty property_velocity_color_max_;
88+
rviz_common::properties::FloatProperty property_vel_max_;
89+
90+
// Velocity visualization properties
91+
rviz_common::properties::BoolProperty property_velocity_view_;
92+
rviz_common::properties::FloatProperty property_velocity_alpha_;
93+
rviz_common::properties::FloatProperty property_velocity_scale_;
94+
rviz_common::properties::BoolProperty property_velocity_color_view_;
95+
rviz_common::properties::ColorProperty property_velocity_color_;
96+
97+
// Generator visualization properties
98+
rviz_common::properties::BoolProperty property_generator_text_view_;
99+
rviz_common::properties::FloatProperty property_generator_text_scale_;
100+
101+
// Common data members
102+
std::vector<Ogre::ManualObject *> path_manual_objects_;
103+
std::vector<Ogre::ManualObject *> velocity_manual_objects_;
104+
std::vector<rviz_rendering::MovableText *> generator_texts_;
105+
std::vector<Ogre::SceneNode *> generator_text_nodes_;
106+
107+
// Common coloring modes
108+
enum CommonColoringMode { VELOCITY_BASED = 0 };
109+
110+
typename MessageType::ConstSharedPtr last_msg_ptr_;
111+
112+
// Common methods available to derived classes
113+
void subscribe();
114+
void unsubscribe();
115+
116+
private:
117+
typename rclcpp::Subscription<MessageType>::SharedPtr subscription_;
118+
};
119+
120+
} // namespace rviz_plugins
121+
122+
#endif // AUTOWARE_PLANNING_RVIZ_PLUGIN__CANDIDATE_TRAJECTORIES__DISPLAY_BASE_HPP_

‎autoware_planning_rviz_plugin/include/autoware_planning_rviz_plugin/candidate_trajectories/scored_candidate_trajectories_display.hpp‎

Lines changed: 92 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,92 @@
1+
// Copyright 2025 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef AUTOWARE_PLANNING_RVIZ_PLUGIN__CANDIDATE_TRAJECTORIES__SCORED_CANDIDATE_TRAJECTORIES_DISPLAY_HPP_ // NOLINT
16+
#define AUTOWARE_PLANNING_RVIZ_PLUGIN__CANDIDATE_TRAJECTORIES__SCORED_CANDIDATE_TRAJECTORIES_DISPLAY_HPP_ // NOLINT
17+
18+
#include "autoware_planning_rviz_plugin/candidate_trajectories/display_base.hpp"
19+
20+
#include <rviz_common/properties/color_property.hpp>
21+
22+
#include <autoware_internal_planning_msgs/msg/scored_candidate_trajectories.hpp>
23+
24+
#include <memory>
25+
#include <vector>
26+
27+
namespace rviz_plugins
28+
{
29+
class AutowareScoredCandidateTrajectoriesDisplay
30+
: public CandidateTrajectoriesDisplayBase<
31+
autoware_internal_planning_msgs::msg::ScoredCandidateTrajectories>
32+
{
33+
Q_OBJECT
34+
35+
public:
36+
AutowareScoredCandidateTrajectoriesDisplay();
37+
~AutowareScoredCandidateTrajectoriesDisplay() override;
38+
39+
protected Q_SLOTS:
40+
void updateVisualization();
41+
void updateColoringModeVisibility();
42+
void onTopicChanged();
43+
44+
protected:
45+
// Override base class virtual methods
46+
void setupColoringModes() override;
47+
void processMessage(
48+
const autoware_internal_planning_msgs::msg::ScoredCandidateTrajectories::ConstSharedPtr msg_ptr)
49+
override;
50+
bool validateFloats(
51+
const autoware_internal_planning_msgs::msg::ScoredCandidateTrajectories::ConstSharedPtr &
52+
msg_ptr) override;
53+
54+
// Score-specific coloring methods
55+
std::unique_ptr<Ogre::ColourValue> setColorDependsOnScore(const double score);
56+
std::unique_ptr<Ogre::ColourValue> setColorDependsOnHighestScore(const bool is_highest_score);
57+
58+
/// Coloring modes available for scored trajectories
59+
enum ScoredColoringMode {
60+
SCORED_VELOCITY_BASED = 0, ///< Color trajectories based on velocity
61+
SCORE_BASED = 1, ///< Color trajectories based on continuous score values
62+
HIGHEST_SCORE_BASED = 2 ///< Color trajectories binary: highest score vs others
63+
};
64+
65+
// Score-specific data members
66+
std::vector<std::vector<rviz_rendering::MovableText *>> score_texts_;
67+
std::vector<std::vector<Ogre::SceneNode *>> score_text_nodes_;
68+
69+
// Score-based coloring properties - parented to coloring_mode
70+
rviz_common::properties::ColorProperty property_score_color_min_{
71+
"Min Score Color", QColor("#FF0000"), "", &property_coloring_mode_};
72+
rviz_common::properties::ColorProperty property_score_color_mid_{
73+
"Mid Score Color", QColor("#FFFF00"), "", &property_coloring_mode_};
74+
rviz_common::properties::ColorProperty property_score_color_max_{
75+
"Max Score Color", QColor("#00FF00"), "", &property_coloring_mode_};
76+
77+
// Highest-score based coloring properties
78+
rviz_common::properties::ColorProperty property_highest_score_color_{
79+
"Highest Score Color", QColor("#00FF00"), "", &property_coloring_mode_};
80+
rviz_common::properties::ColorProperty property_other_score_color_{
81+
"Other Score Color", QColor("#808080"), "", &property_coloring_mode_};
82+
83+
// Score text properties
84+
rviz_common::properties::BoolProperty property_score_text_view_{
85+
"View Score Text", false, "", this};
86+
rviz_common::properties::FloatProperty property_score_text_scale_{
87+
"Scale", 0.3, "", &property_score_text_view_};
88+
};
89+
} // namespace rviz_plugins
90+
91+
// NOLINTNEXTLINE
92+
#endif // AUTOWARE_PLANNING_RVIZ_PLUGIN__CANDIDATE_TRAJECTORIES__SCORED_CANDIDATE_TRAJECTORIES_DISPLAY_HPP_

‎autoware_planning_rviz_plugin/include/autoware_planning_rviz_plugin/common/color_utils.hpp‎

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
// Copyright 2025 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef AUTOWARE_PLANNING_RVIZ_PLUGIN__COMMON__COLOR_UTILS_HPP_
16+
#define AUTOWARE_PLANNING_RVIZ_PLUGIN__COMMON__COLOR_UTILS_HPP_
17+
18+
#include <QColor>
19+
20+
#include <OgreColourValue.h>
21+
22+
#include <memory>
23+
24+
namespace rviz_plugins
25+
{
26+
namespace color_utils
27+
{
28+
std::unique_ptr<Ogre::ColourValue> setColorDependsOnVelocity(const double velocity);
29+
std::unique_ptr<Ogre::ColourValue> gradation(
30+
const QColor & color_min, const QColor & color_max, const double ratio);
31+
} // namespace color_utils
32+
} // namespace rviz_plugins
33+
34+
#endif // AUTOWARE_PLANNING_RVIZ_PLUGIN__COMMON__COLOR_UTILS_HPP_

‎autoware_planning_rviz_plugin/plugins/plugin_description.xml‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,16 @@
1919
base_class_type="rviz_common::Display">
2020
<description>Display trajectory points of autoware_planning_msg::Trajectory</description>
2121
</class>
22+
<class name="rviz_plugins/ScoredCandidateTrajectories"
23+
type="rviz_plugins::AutowareScoredCandidateTrajectoriesDisplay"
24+
base_class_type="rviz_common::Display">
25+
<description>Display scored candidate trajectories with color-coded scores</description>
26+
</class>
27+
<class name="rviz_plugins/CandidateTrajectories"
28+
type="rviz_plugins::AutowareCandidateTrajectoriesDisplay"
29+
base_class_type="rviz_common::Display">
30+
<description>Display candidate trajectories with generator-based or index-based coloring</description>
31+
</class>
2232
<class name="rviz_plugins/MaxVelocity"
2333
type="rviz_plugins::MaxVelocityDisplay"
2434
base_class_type="rviz_common::Display">

0 commit comments

Comments
 (0)