Skip to content

Commit

Permalink
What: Changed the data type of marker color to use the existing stand…
Browse files Browse the repository at this point in the history
…ard ROS2 message format instead of custom defined struct
  • Loading branch information
sujithvemi committed Dec 12, 2022
1 parent 9e0b7d5 commit 989bb7e
Showing 1 changed file with 6 additions and 11 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -19,8 +19,10 @@

#ifdef ROS_DISTRO_GALACTIC
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <std_msgs/color_rgba.h>
#else
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <std_msgs/color_rgba.hpp>
#endif

#include <vector>
Expand All @@ -31,19 +33,12 @@ using tier4_autoware_utils::appendMarkerArray;
using tier4_autoware_utils::createDefaultMarker;
using tier4_autoware_utils::createMarkerColor;
using tier4_autoware_utils::createMarkerScale;
using std_msgs::msg::ColorRGBA;

namespace
{
using DebugData = DetectionAreaModule::DebugData;

struct MarkerColor
{
float r;
float g;
float b;
float a;
};

lanelet::BasicPoint3d getCentroidPoint(const lanelet::BasicPolygon3d & poly)
{
lanelet::BasicPoint3d p_sum{0.0, 0.0, 0.0};
Expand All @@ -64,7 +59,7 @@ geometry_msgs::msg::Point toMsg(const lanelet::BasicPoint3d & point)

visualization_msgs::msg::MarkerArray createCorrespondenceMarkerArray(
const lanelet::autoware::DetectionArea & detection_area_reg_elem, const rclcpp::Time & now,
MarkerColor marker_color)
ColorRGBA marker_color)
{
visualization_msgs::msg::MarkerArray msg;

Expand Down Expand Up @@ -147,7 +142,7 @@ visualization_msgs::msg::MarkerArray DetectionAreaModule::createDebugMarkerArray
const rclcpp::Time now = clock_->now();

if (!debug_data_.stop_poses.empty()) {
MarkerColor marker_color;
ColorRGBA marker_color;
marker_color.r = 1.0;
marker_color.g = 0.0;
marker_color.b = 0.0;
Expand All @@ -161,7 +156,7 @@ visualization_msgs::msg::MarkerArray DetectionAreaModule::createDebugMarkerArray
debug_data_.obstacle_points, "obstacles", module_id_, now, 0.6, 0.6, 0.6, 1.0, 0.0, 0.0),
&wall_marker, now);
} else {
MarkerColor marker_color;
ColorRGBA marker_color;
marker_color.r = 0.0;
marker_color.g = 1.0;
marker_color.b = 0.0;
Expand Down

0 comments on commit 989bb7e

Please sign in to comment.