mobilitypath_visualizer.hpp

Go to the documentation of this file.

#pragma once
 
/*
 * Copyright (C) 2019-2022 LEIDOS.
 *
 * 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 <rclcpp/rclcpp.hpp>
#include <functional>
#include <std_msgs/msg/string.hpp>
#include <carma_ros2_utils/carma_lifecycle_node.hpp>
#include <string>
#include <vector>
#include <visualization_msgs/msg/marker_array.hpp>
#include <carma_v2x_msgs/msg/mobility_path.hpp>
#include <unordered_map>
#include <lanelet2_extension/projection/local_frame_projector.h>
 
#include "mobilitypath_visualizer/mobilitypath_visualizer_config.hpp"
 
 
namespace mobilitypath_visualizer {
 
    struct MarkerColor
    {
        double red = 0.0;
        double green = 0.0;
        double blue = 0.0;
    };
 
    class MobilityPathVisualizer :public carma_ros2_utils::CarmaLifecycleNode
    {
 
    public:
 
 
        MobilityPathVisualizer(const rclcpp::NodeOptions &);
 
        visualization_msgs::msg::MarkerArray composeVisualizationMarker(const carma_v2x_msgs::msg::MobilityPath& msg, const MarkerColor& color);
 
        geometry_msgs::msg::Point ECEFToMapPoint(const carma_v2x_msgs::msg::LocationECEF& ecef_point) const;
 
        visualization_msgs::msg::MarkerArray composeLabelMarker(const visualization_msgs::msg::MarkerArray& host_marker, const std::vector<visualization_msgs::msg::MarkerArray>& cav_markers) const;
 
        std::vector<visualization_msgs::msg::MarkerArray> matchTrajectoryTimestamps(const visualization_msgs::msg::MarkerArray& host_marker,
                                                                    const std::vector<visualization_msgs::msg::MarkerArray>& cav_markers) const;
 
 
        void georeferenceCallback(std_msgs::msg::String::UniquePtr msg);
 
        // Overrides
        carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &);
 
        carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &);
 
    private:
 
        // publisher
        carma_ros2_utils::PubPtr<visualization_msgs::msg::MarkerArray> host_marker_pub_;
        carma_ros2_utils::PubPtr<visualization_msgs::msg::MarkerArray> cav_marker_pub_;
        carma_ros2_utils::PubPtr<visualization_msgs::msg::MarkerArray> label_marker_pub_;
 
        // subscriber
        carma_ros2_utils::SubPtr<carma_v2x_msgs::msg::MobilityPath> host_mob_path_sub_;
        carma_ros2_utils::SubPtr<carma_v2x_msgs::msg::MobilityPath> cav_mob_path_sub_;
        carma_ros2_utils::SubPtr<std_msgs::msg::String> georeference_sub_;
 
        // initialize this node before running
        void initialize();
 
        std::string georeference_{""};
        std::shared_ptr<lanelet::projection::LocalFrameProjector> map_projector_;
 
        Config config_;
 
        // Timers
        rclcpp::TimerBase::SharedPtr timer_;
 
        // callbacks
        void callbackMobilityPath(carma_v2x_msgs::msg::MobilityPath::UniquePtr msg);
        void timer_callback();
 
        // latest msgs
        std::unordered_map<std::string, carma_v2x_msgs::msg::MobilityPath> latest_cav_mob_path_msg_;
 
        // marker msgs
        visualization_msgs::msg::MarkerArray host_marker_;
        std::vector<visualization_msgs::msg::MarkerArray> cav_markers_;
        visualization_msgs::msg::MarkerArray label_marker_;
 
        // helper variable
        std::unordered_map<std::string, size_t> prev_marker_list_size_;
        bool host_marker_received_ = false;
 
    };
 
}