Skip to content

Commit

Permalink
feat: perception rviz plugins (#74)
Browse files Browse the repository at this point in the history 
* feat(tier4_perception_rviz_plugin): interactive pedestrian for rviz plugin (#534)

* feat(tier4_perception_rviz_plugin): add mouse event util function

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* feat(tier4_perception_rviz_plugin): delete option for QT

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(tier4_perception_rviz_plugin): remove interactive_pedestrian.cpp

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* feat(tier4_perception_rviz_plugin): expand conventional 2D dummy pedestrian to be able to interact

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(tier4_perception_rviz_plugin): change for void

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(tier4_perception_rviz_plugin): not use std::numeric_limits::epsilon()

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* feat(tier4_perception_rviz_plugin): plugin supports interactive manipulation for dummy CAR & UNKNOWN object (#554)

* fix(tier4_perception_rviz_plugin): split pedestrian.hpp(cpp) into two parts

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(tier4_perception_rviz_plugin): modify logic of velocity & pose calculation

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* feat(tier4_perception_rviz_plugin): plugin supports 2D Interactive dummy car

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* feat(tier4_perception_rviz_plugin): plugin supports 2D Interactive unknown object

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* refactor(tier4_perception_rviz_plugin): use common class

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(tier4_perception_rviz_plugin): fix dummy pedestrian shape (#571)

* fix(tier4_perception_rviz_plugin): set interactive object velocity to zero on releasing the object (#686)

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(tier4_perception_rviz_plugin): modify build error in rolling (#791)

* fix(tier4_perception_rviz_plugin): modify build error in rolling

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* fix(tier4_perception_rviz_plugin): add target compile definitions

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* feat(perception_rviz_plugin): add 2d dummy bus with height (#903)

* feat(perception_rviz_plugin): add 2d dummy bus with height

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* ci(pre-commit): autofix

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com>
Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
@taikitanaka3 @satoshi-ota
@1222-takeshi @wep21 @pre-commit-ci
5 people authored Jun 27, 2022
1 parent 4e5f392 commit 16edd36
Show file tree
Hide file tree
Showing 13 changed files with 788 additions and 255 deletions.
10 changes: 9 additions & 1 deletion common/tier4_perception_rviz_plugin/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,10 +18,11 @@ set(QT_LIBRARIES Qt5::Widgets)

set(CMAKE_AUTOMOC ON)
set(CMAKE_INCLUDE_CURRENT_DIR ON)
add_definitions(-DQT_NO_KEYWORDS)

## Declare a C++ library
ament_auto_add_library(tier4_perception_rviz_plugin SHARED
src/tools/util.cpp
src/tools/interactive_object.cpp
src/tools/pedestrian_pose.cpp
src/tools/car_pose.cpp
src/tools/unknown_pose.cpp
Expand All @@ -31,6 +32,13 @@ ament_auto_add_library(tier4_perception_rviz_plugin SHARED
target_link_libraries(tier4_perception_rviz_plugin
${QT_LIBRARIES})

# workaround to allow deprecated header to build on both galactic and rolling
if(${tf2_geometry_msgs_VERSION} VERSION_LESS 0.18.0)
target_compile_definitions(tier4_perception_rviz_plugin PRIVATE
USE_TF2_GEOMETRY_MSGS_DEPRECATED_HEADER
)
endif()

# Export the plugin to be imported by rviz2
pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml)

Expand Down
Binary file added common/tier4_perception_rviz_plugin/icons/classes/BusInitialPoseTool.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
4 changes: 4 additions & 0 deletions common/tier4_perception_rviz_plugin/plugins/plugin_description.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,10 @@
type="rviz_plugins::CarInitialPoseTool"
base_class_type="rviz_common::Tool">
</class>
<class name="rviz_plugins/BusInitialPoseTool"
type="rviz_plugins::BusInitialPoseTool"
base_class_type="rviz_common::Tool">
</class>
<class name="rviz_plugins/UnknownInitialPoseTool"
type="rviz_plugins::UnknownInitialPoseTool"
base_class_type="rviz_common::Tool">
Expand Down
163 changes: 115 additions & 48 deletions common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,10 +47,9 @@

#include "car_pose.hpp"

#include <unique_identifier_msgs/msg/uuid.hpp>
#include "util.hpp"

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
#include <rviz_common/display_context.hpp>

#include <algorithm>
#include <random>
Expand All @@ -62,6 +61,12 @@ CarInitialPoseTool::CarInitialPoseTool()
{
shortcut_key_ = 'k';

enable_interactive_property_ = new rviz_common::properties::BoolProperty(
"Interactive", false, "Enable/Disable interactive action by manipulating mouse.",
getPropertyContainer());
property_frame_ = new rviz_common::properties::TfFrameProperty(
"Target Frame", rviz_common::properties::TfFrameProperty::FIXED_FRAME_STRING,
"The TF frame where the point cloud is output.", getPropertyContainer(), nullptr, true);
topic_property_ = new rviz_common::properties::StringProperty(
"Pose Topic", "/simulation/dummy_perception_publisher/object_info",
"The topic on which to publish dummy object info.", getPropertyContainer(), SLOT(updateTopic()),
Expand All @@ -75,6 +80,12 @@ CarInitialPoseTool::CarInitialPoseTool()
std_dev_theta_ = new rviz_common::properties::FloatProperty(
"Theta std deviation", 5.0 * M_PI / 180.0, "Theta standard deviation for initial pose [rad]",
getPropertyContainer());
length_ = new rviz_common::properties::FloatProperty(
"L vehicle length", 4.0, "L length for vehicle [m]", getPropertyContainer());
width_ = new rviz_common::properties::FloatProperty(
"W vehicle width", 1.8, "W width for vehicle [m]", getPropertyContainer());
height_ = new rviz_common::properties::FloatProperty(
"H vehicle height", 2.0, "H height for vehicle [m]", getPropertyContainer());
position_z_ = new rviz_common::properties::FloatProperty(
"Z position", 0.0, "Z position for initial pose [m]", getPropertyContainer());
velocity_ = new rviz_common::properties::FloatProperty(
Expand All @@ -93,74 +104,130 @@ void CarInitialPoseTool::onInitialize()
updateTopic();
}

void CarInitialPoseTool::updateTopic()
Object CarInitialPoseTool::createObjectMsg() const
{
rclcpp::Node::SharedPtr raw_node = context_->getRosNodeAbstraction().lock()->get_raw_node();
dummy_object_info_pub_ = raw_node->create_publisher<dummy_perception_publisher::msg::Object>(
topic_property_->getStdString(), 1);
clock_ = raw_node->get_clock();
Object object{};
std::string fixed_frame = context_->getFixedFrame().toStdString();

// header
object.header.frame_id = fixed_frame;
object.header.stamp = clock_->now();

// semantic
object.classification.label = ObjectClassification::CAR;
object.classification.probability = 1.0;

// shape
object.shape.type = Shape::BOUNDING_BOX;
object.shape.dimensions.x = length_->getFloat();
object.shape.dimensions.y = width_->getFloat();
object.shape.dimensions.z = height_->getFloat();

// initial state
object.initial_state.pose_covariance.pose.position.z = position_z_->getFloat();
object.initial_state.pose_covariance.covariance[0] =
std_dev_x_->getFloat() * std_dev_x_->getFloat();
object.initial_state.pose_covariance.covariance[7] =
std_dev_y_->getFloat() * std_dev_y_->getFloat();
object.initial_state.pose_covariance.covariance[14] =
std_dev_z_->getFloat() * std_dev_z_->getFloat();
object.initial_state.pose_covariance.covariance[35] =
std_dev_theta_->getFloat() * std_dev_theta_->getFloat();

std::mt19937 gen(std::random_device{}());
std::independent_bits_engine<std::mt19937, 8, uint8_t> bit_eng(gen);
std::generate(object.id.uuid.begin(), object.id.uuid.end(), bit_eng);

return object;
}

void CarInitialPoseTool::onPoseSet(double x, double y, double theta)
BusInitialPoseTool::BusInitialPoseTool()
{
dummy_perception_publisher::msg::Object output_msg;
// short cut below k
shortcut_key_ = 'b';

enable_interactive_property_ = new rviz_common::properties::BoolProperty(
"Interactive", false, "Enable/Disable interactive action by manipulating mouse.",
getPropertyContainer());
property_frame_ = new rviz_common::properties::TfFrameProperty(
"Target Frame", rviz_common::properties::TfFrameProperty::FIXED_FRAME_STRING,
"The TF frame where the point cloud is output.", getPropertyContainer(), nullptr, true);
topic_property_ = new rviz_common::properties::StringProperty(
"Pose Topic", "/simulation/dummy_perception_publisher/object_info",
"The topic on which to publish dummy object info.", getPropertyContainer(), SLOT(updateTopic()),
this);
std_dev_x_ = new rviz_common::properties::FloatProperty(
"X std deviation", 0.03, "X standard deviation for initial pose [m]", getPropertyContainer());
std_dev_y_ = new rviz_common::properties::FloatProperty(
"Y std deviation", 0.03, "Y standard deviation for initial pose [m]", getPropertyContainer());
std_dev_z_ = new rviz_common::properties::FloatProperty(
"Z std deviation", 0.03, "Z standard deviation for initial pose [m]", getPropertyContainer());
std_dev_theta_ = new rviz_common::properties::FloatProperty(
"Theta std deviation", 5.0 * M_PI / 180.0, "Theta standard deviation for initial pose [rad]",
getPropertyContainer());
length_ = new rviz_common::properties::FloatProperty(
"L vehicle length", 10.5, "L length for vehicle [m]", getPropertyContainer());
width_ = new rviz_common::properties::FloatProperty(
"W vehicle width", 2.5, "W width for vehicle [m]", getPropertyContainer());
height_ = new rviz_common::properties::FloatProperty(
"H vehicle height", 3.5, "H height for vehicle [m]", getPropertyContainer());
position_z_ = new rviz_common::properties::FloatProperty(
"Z position", 0.0, "Z position for initial pose [m]", getPropertyContainer());
velocity_ = new rviz_common::properties::FloatProperty(
"Velocity", 0.0, "velocity [m/s]", getPropertyContainer());
std_dev_x_->setMin(0);
std_dev_y_->setMin(0);
std_dev_z_->setMin(0);
std_dev_theta_->setMin(0);
position_z_->setMin(0);
}

void BusInitialPoseTool::onInitialize()
{
PoseTool::onInitialize();
setName("2D Dummy Bus");
updateTopic();
}

Object BusInitialPoseTool::createObjectMsg() const
{
Object object{};
std::string fixed_frame = context_->getFixedFrame().toStdString();

// header
output_msg.header.frame_id = fixed_frame;
output_msg.header.stamp = clock_->now();
object.header.frame_id = fixed_frame;
object.header.stamp = clock_->now();

// semantic
output_msg.classification.label = autoware_auto_perception_msgs::msg::ObjectClassification::CAR;
output_msg.classification.probability = 1.0;
object.classification.label = ObjectClassification::BUS;
object.classification.probability = 1.0;

// shape
output_msg.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX;
const double width = 1.8;
const double length = 4.0;
output_msg.shape.dimensions.x = length;
output_msg.shape.dimensions.y = width;
output_msg.shape.dimensions.z = 2.0;
object.shape.type = Shape::BOUNDING_BOX;
object.shape.dimensions.x = length_->getFloat();
object.shape.dimensions.y = width_->getFloat();
object.shape.dimensions.z = height_->getFloat();

// initial state
// pose
output_msg.initial_state.pose_covariance.pose.position.x = x;
output_msg.initial_state.pose_covariance.pose.position.y = y;
output_msg.initial_state.pose_covariance.pose.position.z = position_z_->getFloat();
output_msg.initial_state.pose_covariance.covariance[0] =
object.initial_state.pose_covariance.pose.position.z = position_z_->getFloat();
object.initial_state.pose_covariance.covariance[0] =
std_dev_x_->getFloat() * std_dev_x_->getFloat();
output_msg.initial_state.pose_covariance.covariance[7] =
object.initial_state.pose_covariance.covariance[7] =
std_dev_y_->getFloat() * std_dev_y_->getFloat();
output_msg.initial_state.pose_covariance.covariance[14] =
object.initial_state.pose_covariance.covariance[14] =
std_dev_z_->getFloat() * std_dev_z_->getFloat();
output_msg.initial_state.pose_covariance.covariance[35] =
object.initial_state.pose_covariance.covariance[35] =
std_dev_theta_->getFloat() * std_dev_theta_->getFloat();
tf2::Quaternion quat;
quat.setRPY(0.0, 0.0, theta);
output_msg.initial_state.pose_covariance.pose.orientation = tf2::toMsg(quat);
RCLCPP_INFO(
rclcpp::get_logger("CarInitialPoseTool"), "Setting pose: %.3f %.3f %.3f %.3f [frame=%s]", x, y,
position_z_->getFloat(), theta, fixed_frame.c_str());
// twist
output_msg.initial_state.twist_covariance.twist.linear.x = velocity_->getFloat();
output_msg.initial_state.twist_covariance.twist.linear.y = 0.0;
output_msg.initial_state.twist_covariance.twist.linear.z = 0.0;
RCLCPP_INFO(
rclcpp::get_logger("CarInitialPoseTool"), "Setting twist: %.3f %.3f %.3f [frame=%s]",
velocity_->getFloat(), 0.0, 0.0, fixed_frame.c_str());

// action
output_msg.action = dummy_perception_publisher::msg::Object::ADD;

// id

std::mt19937 gen(std::random_device{}());
std::independent_bits_engine<std::mt19937, 8, uint8_t> bit_eng(gen);
std::generate(output_msg.id.uuid.begin(), output_msg.id.uuid.end(), bit_eng);
std::generate(object.id.uuid.begin(), object.id.uuid.end(), bit_eng);

dummy_object_info_pub_->publish(output_msg);
return object;
}

} // end namespace rviz_plugins

#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(rviz_plugins::CarInitialPoseTool, rviz_common::Tool)
PLUGINLIB_EXPORT_CLASS(rviz_plugins::BusInitialPoseTool, rviz_common::Tool)
48 changes: 16 additions & 32 deletions common/tier4_perception_rviz_plugin/src/tools/car_pose.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,45 +48,29 @@
#ifndef TOOLS__CAR_POSE_HPP_
#define TOOLS__CAR_POSE_HPP_

#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829
#include <QObject>
#include <rclcpp/node.hpp>
#include <rviz_common/display_context.hpp>
#include <rviz_common/properties/float_property.hpp>
#include <rviz_common/properties/string_property.hpp>
#include <rviz_default_plugins/tools/pose/pose_tool.hpp>
#endif

#include <dummy_perception_publisher/msg/object.hpp>
#include "interactive_object.hpp"

namespace rviz_plugins
{
class CarInitialPoseTool : public rviz_default_plugins::tools::PoseTool
{
Q_OBJECT

using autoware_auto_perception_msgs::msg::ObjectClassification;
using autoware_auto_perception_msgs::msg::Shape;
using dummy_perception_publisher::msg::Object;

class CarInitialPoseTool : public InteractiveObjectTool
{
public:
CarInitialPoseTool();
virtual ~CarInitialPoseTool() {}
virtual void onInitialize();

protected:
virtual void onPoseSet(double x, double y, double theta);

private Q_SLOTS:
void updateTopic();

private:
rclcpp::Clock::SharedPtr clock_;
rclcpp::Publisher<dummy_perception_publisher::msg::Object>::SharedPtr dummy_object_info_pub_;
void onInitialize();
Object createObjectMsg() const override;
};

rviz_common::properties::StringProperty * topic_property_;
rviz_common::properties::FloatProperty * std_dev_x_;
rviz_common::properties::FloatProperty * std_dev_y_;
rviz_common::properties::FloatProperty * std_dev_z_;
rviz_common::properties::FloatProperty * std_dev_theta_;
rviz_common::properties::FloatProperty * position_z_;
rviz_common::properties::FloatProperty * velocity_;
class BusInitialPoseTool : public InteractiveObjectTool
{
public:
BusInitialPoseTool();
void onInitialize();
Object createObjectMsg() const override;
};

} // namespace rviz_plugins
Expand Down
Loading

0 comments on commit 16edd36

Please sign in to comment.