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

fix/sonor-cloud-issue #1431

Merged
merged 10 commits into from
Nov 11, 2024
8 changes: 4 additions & 4 deletions common/math/geometry/include/geometry/bounding_box.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -63,11 +63,11 @@ auto toPolygon2D(const traffic_simulator_msgs::msg::BoundingBox & bounding_box)
-> std::vector<geometry_msgs::msg::Point>;
auto toPolygon2D(
const geometry_msgs::msg::Pose & pose,
const traffic_simulator_msgs::msg::BoundingBox & bounding_box) -> const boost_polygon;
const traffic_simulator_msgs::msg::BoundingBox & bounding_box) -> boost_polygon;
std::vector<geometry_msgs::msg::Point> getPointsFromBbox(
traffic_simulator_msgs::msg::BoundingBox bounding_box, double width_extension_right = 0.0,
double width_extension_left = 0.0, double length_extension_front = 0.0,
double length_extension_rear = 0.0);
const traffic_simulator_msgs::msg::BoundingBox & bounding_box,
const double width_extension_right = 0.0, const double width_extension_left = 0.0,
const double length_extension_front = 0.0, const double length_extension_rear = 0.0);
DistancesFromCenterToEdge getDistancesFromCenterToEdge(
const traffic_simulator_msgs::msg::BoundingBox & bounding_box);

Expand Down
16 changes: 12 additions & 4 deletions common/math/geometry/src/bounding_box.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -121,14 +121,15 @@ boost_point pointToSegmentProjection(

auto toPolygon2D(
const geometry_msgs::msg::Pose & pose,
const traffic_simulator_msgs::msg::BoundingBox & bounding_box) -> const boost_polygon
const traffic_simulator_msgs::msg::BoundingBox & bounding_box) -> boost_polygon
{
return toBoostPolygon(transformPoints(pose, getPointsFromBbox(bounding_box)));
}

std::vector<geometry_msgs::msg::Point> getPointsFromBbox(
traffic_simulator_msgs::msg::BoundingBox bounding_box, double width_extension_right,
double width_extension_left, double length_extension_front, double length_extension_rear)
const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const double width_extension_right,
const double width_extension_left, const double length_extension_front,
const double length_extension_rear)
{
std::vector<geometry_msgs::msg::Point> points;
auto distances_from_center_to_edge = getDistancesFromCenterToEdge(bounding_box);
Expand Down Expand Up @@ -159,7 +160,14 @@ auto toPolygon2D(const traffic_simulator_msgs::msg::BoundingBox & bounding_box)
-> std::vector<geometry_msgs::msg::Point>
{
std::vector<geometry_msgs::msg::Point> points_bounding_box;
geometry_msgs::msg::Point p0, p1, p2, p3, p4, p5, p6, p7;
geometry_msgs::msg::Point p0;
geometry_msgs::msg::Point p1;
geometry_msgs::msg::Point p2;
geometry_msgs::msg::Point p3;
geometry_msgs::msg::Point p4;
geometry_msgs::msg::Point p5;
geometry_msgs::msg::Point p6;
geometry_msgs::msg::Point p7;

p0.x = bounding_box.center.x + bounding_box.dimensions.x * 0.5;
p0.y = bounding_box.center.y + bounding_box.dimensions.y * 0.5;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -88,13 +88,13 @@ private Q_SLOTS:
protected:
void processMessage(const Context::ConstSharedPtr msg_ptr);
jsk_rviz_plugins::OverlayObject::Ptr overlay_;
rviz_common::properties::ColorProperty * property_text_color_;
rviz_common::properties::IntProperty * property_left_;
rviz_common::properties::IntProperty * property_top_;
rviz_common::properties::IntProperty * property_length_;
rviz_common::properties::IntProperty * property_width_;
rviz_common::properties::StringProperty * property_topic_name_;
rviz_common::properties::FloatProperty * property_value_scale_;
std::unique_ptr<rviz_common::properties::ColorProperty> property_text_color_;
std::unique_ptr<rviz_common::properties::IntProperty> property_left_;
std::unique_ptr<rviz_common::properties::IntProperty> property_top_;
std::unique_ptr<rviz_common::properties::IntProperty> property_length_;
std::unique_ptr<rviz_common::properties::IntProperty> property_width_;
std::unique_ptr<rviz_common::properties::StringProperty> property_topic_name_;
std::unique_ptr<rviz_common::properties::FloatProperty> property_value_scale_;

private:
void loadConditionGroups(const Context::ConstSharedPtr msg_ptr);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -110,7 +110,7 @@ OverlayObject::OverlayObject(

OverlayObject::~OverlayObject()
{
hide();
this->hide();
panel_material_->unload();
Ogre::MaterialManager::getSingleton().remove(panel_material_->getName());
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
#include <QPainter>
#include <algorithm>
#include <iomanip>
#include <memory>
#include <rviz_common/display_context.hpp>
#include <rviz_common/uniform_string_stream.hpp>
#include <string>
Expand All @@ -31,7 +32,7 @@ VisualizationConditionGroupsDisplay::VisualizationConditionGroupsDisplay()
: condition_groups_collection_ptr_(std::make_shared<std::vector<ConditionGroups>>())
{
/// @note Get screen info of default display
const Screen * screen_info = DefaultScreenOfDisplay(XOpenDisplay(NULL));
const Screen * screen_info = DefaultScreenOfDisplay(XOpenDisplay(nullptr));

/// @note Fixed height for a 4k resolution screen
/// @sa https://github.com/tier4/scenario_simulator_v2/pull/1033#discussion_r1412781103
Expand Down Expand Up @@ -75,25 +76,25 @@ VisualizationConditionGroupsDisplay::VisualizationConditionGroupsDisplay()
*/
const int width = static_cast<int>(std::round(2000 * scale));

property_topic_name_ = new rviz_common::properties::StringProperty(
property_topic_name_ = std::make_unique<rviz_common::properties::StringProperty>(
"Topic", "/simulation/context", "The topic on which to publish simulation context.", this,
SLOT(updateTopic()), this);
property_text_color_ = new rviz_common::properties::ColorProperty(
property_text_color_ = std::make_unique<rviz_common::properties::ColorProperty>(
"Text Color", QColor(255, 255, 255), "text color", this, SLOT(updateVisualization()), this);
property_left_ = new rviz_common::properties::IntProperty(
property_left_ = std::make_unique<rviz_common::properties::IntProperty>(
"Left", left, "Left of the plotter window", this, SLOT(updateVisualization()), this);
property_left_->setMin(0);
property_top_ = new rviz_common::properties::IntProperty(
property_top_ = std::make_unique<rviz_common::properties::IntProperty>(
"Top", top, "Top of the plotter window", this, SLOT(updateVisualization()));
property_top_->setMin(0);

property_length_ = new rviz_common::properties::IntProperty(
property_length_ = std::make_unique<rviz_common::properties::IntProperty>(
"Length", length, "Length of the plotter window", this, SLOT(updateVisualization()), this);
property_length_->setMin(10);
property_width_ = new rviz_common::properties::IntProperty(
property_width_ = std::make_unique<rviz_common::properties::IntProperty>(
"Width", width, "Width of the plotter window", this, SLOT(updateVisualization()), this);
property_width_->setMin(10);
property_value_scale_ = new rviz_common::properties::FloatProperty(
property_value_scale_ = std::make_unique<rviz_common::properties::FloatProperty>(
"Value Scale", text_size,
"This property controls the scaling factor for the text size on the panel. Setting a higher "
"value results in larger text, making the displayed information easier to read.",
Expand Down
Loading