marker_helper.cpp

// Copyright 2023 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 "autoware_utils_visualization/marker_helper.hpp"

#include <string>

namespace autoware_utils_visualization
{
visualization_msgs::msg::Marker create_default_marker(
  const std::string & frame_id, const rclcpp::Time & now, const std::string & ns, const int32_t id,
  const int32_t type, const geometry_msgs::msg::Vector3 & scale,
  const std_msgs::msg::ColorRGBA & color)
{
  visualization_msgs::msg::Marker marker;

  marker.header.frame_id = frame_id;
  marker.header.stamp = now;
  marker.ns = ns;
  marker.id = id;
  marker.type = type;
  marker.action = visualization_msgs::msg::Marker::ADD;
  marker.lifetime = rclcpp::Duration::from_seconds(0.5);

  marker.pose.position = create_marker_position(0.0, 0.0, 0.0);
  marker.pose.orientation = create_marker_orientation(0.0, 0.0, 0.0, 1.0);
  marker.scale = scale;
  marker.color = color;
  marker.frame_locked = true;

  return marker;
}

visualization_msgs::msg::Marker create_deleted_default_marker(
  const rclcpp::Time & now, const std::string & ns, const int32_t id)
{
  visualization_msgs::msg::Marker marker;

  marker.header.stamp = now;
  marker.ns = ns;
  marker.id = id;
  marker.action = visualization_msgs::msg::Marker::DELETE;

  return marker;
}

void append_marker_array(
  const visualization_msgs::msg::MarkerArray & additional_marker_array,
  visualization_msgs::msg::MarkerArray * marker_array,
  const std::optional<rclcpp::Time> & current_time)
{
  for (const auto & marker : additional_marker_array.markers) {
    marker_array->markers.push_back(marker);

    if (current_time) {
      marker_array->markers.back().header.stamp = current_time.value();
    }
  }
}

}  // namespace autoware_utils_visualization