Skip to content

Commit

Permalink
feat(tier4_planning_rviz_plugin): add PathWithLaneId rviz plugin (#591)
Browse files Browse the repository at this point in the history
* sync rc rc/v1.7.1 (#2345)

* add behavior_path_rviz_plugin (#2343)

* add behavior_path_rviz_plugin

* edit README

* fix for uncrustify

* fix include guard

* use autoware_lint_common

Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>

* Revert "use autoware_lint_common"

This reverts commit 98c264d5f32d88fb19cd7953fc64a2052648af29.

* fix for cpplint

Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>

* Fix format

Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp>
Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>
Co-authored-by: kosuke55 <kosuke.tnp@gmail.com>
Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* feat(tier4_planning_rviz_plugin): add PathWithLaneId rviz plugin

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

Co-authored-by: autoware-iv-sync-ci[bot] <87871706+autoware-iv-sync-ci[bot]@users.noreply.github.com>
Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp>
Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>
  • Loading branch information
4 people authored Mar 28, 2022
1 parent 21a3037 commit 0b5734f
Show file tree
Hide file tree
Showing 4 changed files with 347 additions and 0 deletions.
2 changes: 2 additions & 0 deletions common/tier4_planning_rviz_plugin/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,8 @@ ament_auto_add_library(tier4_planning_rviz_plugin SHARED
src/path/display.cpp
include/path_footprint/display.hpp
src/path_footprint/display.cpp
include/path_with_lane_id/display.hpp
src/path_with_lane_id/display.cpp
include/trajectory/display.hpp
src/trajectory/display.cpp
include/trajectory_footprint/display.hpp
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,85 @@
// Copyright 2020 Tier IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef PATH_WITH_LANE_ID__DISPLAY_HPP_
#define PATH_WITH_LANE_ID__DISPLAY_HPP_

#include <rclcpp/rclcpp.hpp>
#include <rviz_common/display_context.hpp>
#include <rviz_common/frame_manager_iface.hpp>
#include <rviz_common/message_filter_display.hpp>
#include <rviz_common/properties/bool_property.hpp>
#include <rviz_common/properties/color_property.hpp>
#include <rviz_common/properties/float_property.hpp>
#include <rviz_common/properties/parse_color.hpp>
#include <rviz_common/validate_floats.hpp>

#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>

#include <OgreBillboardSet.h>
#include <OgreManualObject.h>
#include <OgreMaterialManager.h>
#include <OgreSceneManager.h>
#include <OgreSceneNode.h>

#include <deque>
#include <memory>

namespace rviz_plugins
{
class AutowarePathWithLaneIdDisplay
: public rviz_common::MessageFilterDisplay<autoware_auto_planning_msgs::msg::PathWithLaneId>
{
Q_OBJECT

public:
AutowarePathWithLaneIdDisplay();
virtual ~AutowarePathWithLaneIdDisplay();

void onInitialize() override;
void reset() override;

private Q_SLOTS:
void updateVisualization();

protected:
void processMessage(
const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) override;
std::unique_ptr<Ogre::ColourValue> setColorDependsOnVelocity(
const double vel_max, const double cmd_vel);
std::unique_ptr<Ogre::ColourValue> gradation(
const QColor & color_min, const QColor & color_max, const double ratio);
Ogre::ManualObject * path_manual_object_;
Ogre::ManualObject * velocity_manual_object_;
rviz_common::properties::BoolProperty * property_path_view_;
rviz_common::properties::BoolProperty * property_velocity_view_;
rviz_common::properties::FloatProperty * property_path_width_;
rviz_common::properties::ColorProperty * property_path_color_;
rviz_common::properties::ColorProperty * property_velocity_color_;
rviz_common::properties::FloatProperty * property_path_alpha_;
rviz_common::properties::FloatProperty * property_velocity_alpha_;
rviz_common::properties::FloatProperty * property_velocity_scale_;
rviz_common::properties::BoolProperty * property_path_color_view_;
rviz_common::properties::BoolProperty * property_velocity_color_view_;
rviz_common::properties::FloatProperty * property_vel_max_;

private:
autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr last_msg_ptr_;
bool validateFloats(
const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr & msg_ptr);
};

} // namespace rviz_plugins

#endif // PATH_WITH_LANE_ID__DISPLAY_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,11 @@
base_class_type="rviz_common::Display">
<description>Display footprint of autoware_auto_planning_msg::Path</description>
</class>
<class name="rviz_plugins/PathWithLaneId"
type="rviz_plugins::AutowarePathWithLaneIdDisplay"
base_class_type="rviz_common::Display">
<description>Display path_with_lane_id of autoware_auto_planning_msg::PathWithLaneId</description>
</class>
<class name="rviz_plugins/Trajectory"
type="rviz_plugins::AutowareTrajectoryDisplay"
base_class_type="rviz_common::Display">
Expand Down
255 changes: 255 additions & 0 deletions common/tier4_planning_rviz_plugin/src/path_with_lane_id/display.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,255 @@
// Copyright 2020 Tier IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <path_with_lane_id/display.hpp>

#include <memory>
#define EIGEN_MPL2_ONLY
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>

namespace rviz_plugins
{
std::unique_ptr<Ogre::ColourValue> AutowarePathWithLaneIdDisplay::gradation(
const QColor & color_min, const QColor & color_max, const double ratio)
{
std::unique_ptr<Ogre::ColourValue> color_ptr(new Ogre::ColourValue);
color_ptr->g = color_max.greenF() * ratio + color_min.greenF() * (1.0 - ratio);
color_ptr->r = color_max.redF() * ratio + color_min.redF() * (1.0 - ratio);
color_ptr->b = color_max.blueF() * ratio + color_min.blueF() * (1.0 - ratio);

return color_ptr;
}

std::unique_ptr<Ogre::ColourValue> AutowarePathWithLaneIdDisplay::setColorDependsOnVelocity(
const double vel_max, const double cmd_vel)
{
const double cmd_vel_abs = std::fabs(cmd_vel);
const double vel_min = 0.0;

std::unique_ptr<Ogre::ColourValue> color_ptr(new Ogre::ColourValue());
if (vel_min < cmd_vel_abs && cmd_vel_abs <= (vel_max / 2.0)) {
double ratio = (cmd_vel_abs - vel_min) / (vel_max / 2.0 - vel_min);
color_ptr = gradation(Qt::red, Qt::yellow, ratio);
} else if ((vel_max / 2.0) < cmd_vel_abs && cmd_vel_abs <= vel_max) {
double ratio = (cmd_vel_abs - vel_max / 2.0) / (vel_max - vel_max / 2.0);
color_ptr = gradation(Qt::yellow, Qt::green, ratio);
} else if (vel_max < cmd_vel_abs) {
*color_ptr = Ogre::ColourValue::Green;
} else {
*color_ptr = Ogre::ColourValue::Red;
}

return color_ptr;
}

AutowarePathWithLaneIdDisplay::AutowarePathWithLaneIdDisplay()
{
property_path_view_ = new rviz_common::properties::BoolProperty(
"View Path", true, "", this, SLOT(updateVisualization()));
property_path_width_ = new rviz_common::properties::FloatProperty(
"Width", 2.0, "", property_path_view_, SLOT(updateVisualization()), this);
property_path_width_->setMin(0.0);
property_path_alpha_ = new rviz_common::properties::FloatProperty(
"Alpha", 1.0, "", property_path_view_, SLOT(updateVisualization()), this);
property_path_alpha_->setMin(0.0);
property_path_alpha_->setMax(1.0);
property_path_color_view_ = new rviz_common::properties::BoolProperty(
"Constant Color", false, "", property_path_view_, SLOT(updateVisualization()), this);
property_path_color_ = new rviz_common::properties::ColorProperty(
"Color", Qt::black, "", property_path_view_, SLOT(updateVisualization()), this);

property_velocity_view_ = new rviz_common::properties::BoolProperty(
"View Velocity", true, "", this, SLOT(updateVisualization()), this);
property_velocity_alpha_ = new rviz_common::properties::FloatProperty(
"Alpha", 1.0, "", property_velocity_view_, SLOT(updateVisualization()), this);
property_velocity_alpha_->setMin(0.0);
property_velocity_alpha_->setMax(1.0);
property_velocity_scale_ = new rviz_common::properties::FloatProperty(
"Scale", 0.3, "", property_velocity_view_, SLOT(updateVisualization()), this);
property_velocity_scale_->setMin(0.1);
property_velocity_scale_->setMax(10.0);
property_velocity_color_view_ = new rviz_common::properties::BoolProperty(
"Constant Color", false, "", property_velocity_view_, SLOT(updateVisualization()), this);
property_velocity_color_ = new rviz_common::properties::ColorProperty(
"Color", Qt::black, "", property_velocity_view_, SLOT(updateVisualization()), this);

property_vel_max_ = new rviz_common::properties::FloatProperty(
"Color Border Vel Max", 3.0, "[m/s]", this, SLOT(updateVisualization()), this);
property_vel_max_->setMin(0.0);
}

AutowarePathWithLaneIdDisplay::~AutowarePathWithLaneIdDisplay()
{
if (initialized()) {
scene_manager_->destroyManualObject(path_manual_object_);
scene_manager_->destroyManualObject(velocity_manual_object_);
}
}

void AutowarePathWithLaneIdDisplay::onInitialize()
{
MFDClass::onInitialize();

path_manual_object_ = scene_manager_->createManualObject();
velocity_manual_object_ = scene_manager_->createManualObject();
path_manual_object_->setDynamic(true);
velocity_manual_object_->setDynamic(true);
scene_node_->attachObject(path_manual_object_);
scene_node_->attachObject(velocity_manual_object_);
}

void AutowarePathWithLaneIdDisplay::reset()
{
MFDClass::reset();
path_manual_object_->clear();
velocity_manual_object_->clear();
}

bool AutowarePathWithLaneIdDisplay::validateFloats(
const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr & msg_ptr)
{
for (auto && e : msg_ptr->points) {
if (
!rviz_common::validateFloats(e.point.pose) &&
!rviz_common::validateFloats(e.point.longitudinal_velocity_mps)) {
return false;
}
}
return true;
}

void AutowarePathWithLaneIdDisplay::processMessage(
const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr)
{
if (!validateFloats(msg_ptr)) {
setStatus(
rviz_common::properties::StatusProperty::Error, "Topic",
"Message contained invalid floating point values (nans or infs)");
return;
}

Ogre::Vector3 position;
Ogre::Quaternion orientation;
if (!context_->getFrameManager()->getTransform(msg_ptr->header, position, orientation)) {
RCLCPP_DEBUG(
rclcpp::get_logger("AutowarePathWithLaneIdDisplay"),
"Error transforming from frame '%s' to frame '%s'", msg_ptr->header.frame_id.c_str(),
qPrintable(fixed_frame_));
}

scene_node_->setPosition(position);
scene_node_->setOrientation(orientation);

path_manual_object_->clear();
velocity_manual_object_->clear();

Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().getByName(
"BaseWhiteNoLighting", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA);
material->setDepthWriteEnabled(false);

if (!msg_ptr->points.empty()) {
path_manual_object_->estimateVertexCount(msg_ptr->points.size() * 2);
velocity_manual_object_->estimateVertexCount(msg_ptr->points.size());
path_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_STRIP);
// path_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_STRIP);
velocity_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP);

for (auto && e : msg_ptr->points) {
/*
* Path
*/
if (property_path_view_->getBool()) {
Ogre::ColourValue color;
if (property_path_color_view_->getBool()) {
color = rviz_common::properties::qtToOgre(property_path_color_->getColor());
} else {
/* color change depending on velocity */
std::unique_ptr<Ogre::ColourValue> dynamic_color_ptr = setColorDependsOnVelocity(
property_vel_max_->getFloat(), e.point.longitudinal_velocity_mps);
color = *dynamic_color_ptr;
}
color.a = property_path_alpha_->getFloat();
Eigen::Vector3f vec_in, vec_out;
Eigen::Quaternionf quat_yaw_reverse(0, 0, 0, 1);
{
vec_in << 0, (property_path_width_->getFloat() / 2.0), 0;
Eigen::Quaternionf quat(
e.point.pose.orientation.w, e.point.pose.orientation.x, e.point.pose.orientation.y,
e.point.pose.orientation.z);
if (e.point.longitudinal_velocity_mps < 0) {
quat *= quat_yaw_reverse;
}
vec_out = quat * vec_in;
path_manual_object_->position(
e.point.pose.position.x + vec_out.x(), e.point.pose.position.y + vec_out.y(),
e.point.pose.position.z + vec_out.z());
path_manual_object_->colour(color);
}
{
vec_in << 0, -(property_path_width_->getFloat() / 2.0), 0;
Eigen::Quaternionf quat(
e.point.pose.orientation.w, e.point.pose.orientation.x, e.point.pose.orientation.y,
e.point.pose.orientation.z);
if (e.point.longitudinal_velocity_mps < 0) {
quat *= quat_yaw_reverse;
}
vec_out = quat * vec_in;
path_manual_object_->position(
e.point.pose.position.x + vec_out.x(), e.point.pose.position.y + vec_out.y(),
e.point.pose.position.z + vec_out.z());
path_manual_object_->colour(color);
}
}
/*
* Velocity
*/
if (property_velocity_view_->getBool()) {
Ogre::ColourValue color;
if (property_velocity_color_view_->getBool()) {
color = rviz_common::properties::qtToOgre(property_velocity_color_->getColor());
} else {
/* color change depending on velocity */
std::unique_ptr<Ogre::ColourValue> dynamic_color_ptr = setColorDependsOnVelocity(
property_vel_max_->getFloat(), e.point.longitudinal_velocity_mps);
color = *dynamic_color_ptr;
}
color.a = property_velocity_alpha_->getFloat();

velocity_manual_object_->position(
e.point.pose.position.x, e.point.pose.position.y,
e.point.pose.position.z +
e.point.longitudinal_velocity_mps * property_velocity_scale_->getFloat());
velocity_manual_object_->colour(color);
}
}

path_manual_object_->end();
velocity_manual_object_->end();
}
last_msg_ptr_ = msg_ptr;
}

void AutowarePathWithLaneIdDisplay::updateVisualization()
{
if (last_msg_ptr_ != nullptr) {
processMessage(last_msg_ptr_);
}
}

} // namespace rviz_plugins

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowarePathWithLaneIdDisplay, rviz_common::Display)

0 comments on commit 0b5734f

Please sign in to comment.