#include <mobilitypath_visualizer.hpp>

Inheritance diagram for mobilitypath_visualizer::MobilityPathVisualizer:

Collaboration diagram for mobilitypath_visualizer::MobilityPathVisualizer:

Public Member Functions
	MobilityPathVisualizer (const rclcpp::NodeOptions &)
	Constructor. More...

visualization_msgs::msg::MarkerArray	composeVisualizationMarker (const carma_v2x_msgs::msg::MobilityPath &msg, const MarkerColor &color)
	Compose a visualization marker for mobilitypath messages. More...

geometry_msgs::msg::Point	ECEFToMapPoint (const carma_v2x_msgs::msg::LocationECEF &ecef_point) const
	Accepts ECEF point in cm to convert to a point in map in meters. More...

visualization_msgs::msg::MarkerArray	composeLabelMarker (const visualization_msgs::msg::MarkerArray &host_marker, const std::vector< visualization_msgs::msg::MarkerArray > &cav_markers) const
	Compose a label marker that displays whether if any of the cav's path cross with that of host (respective points are within 1 meter) More...

std::vector< visualization_msgs::msg::MarkerArray >	matchTrajectoryTimestamps (const visualization_msgs::msg::MarkerArray &host_marker, const std::vector< visualization_msgs::msg::MarkerArray > &cav_markers) const
	Matches timestamps of CAV's individual points of cav_markers to that of host_marker and interpolates their points using the speed between points. More...

void	georeferenceCallback (std_msgs::msg::String::UniquePtr msg)
	Callback for map projection string to define lat/lon -> map conversion. More...

carma_ros2_utils::CallbackReturn	handle_on_configure (const rclcpp_lifecycle::State &)

carma_ros2_utils::CallbackReturn	handle_on_activate (const rclcpp_lifecycle::State &)

Private Member Functions
void	initialize ()

void	callbackMobilityPath (carma_v2x_msgs::msg::MobilityPath::UniquePtr msg)

void	timer_callback ()

Private Attributes
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_

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_

std::string	georeference_ {""}

std::shared_ptr< lanelet::projection::LocalFrameProjector >	map_projector_

Config	config_

rclcpp::TimerBase::SharedPtr	timer_

std::unordered_map< std::string, carma_v2x_msgs::msg::MobilityPath >	latest_cav_mob_path_msg_

visualization_msgs::msg::MarkerArray	host_marker_

std::vector< visualization_msgs::msg::MarkerArray >	cav_markers_

visualization_msgs::msg::MarkerArray	label_marker_

std::unordered_map< std::string, size_t >	prev_marker_list_size_

bool	host_marker_received_ = false

Detailed Description

Definition at line 47 of file mobilitypath_visualizer.hpp.

Constructor & Destructor Documentation

◆ MobilityPathVisualizer()

mobilitypath_visualizer::MobilityPathVisualizer::MobilityPathVisualizer ( const rclcpp::NodeOptions & options )

Constructor.

Definition at line 36 of file mobilitypath_visualizer.cpp.

            : carma_ros2_utils::CarmaLifecycleNode(options)
    {
        // Create initial config
        config_ = Config();
 
        // Declare parameters
        config_.timer_cb_rate = declare_parameter<int>("timer_cb_rate", config_.timer_cb_rate);
        config_.x = declare_parameter<double>("x", config_.x);
        config_.y = declare_parameter<double>("y", config_.y);
        config_.z = declare_parameter<double>("z", config_.z);
        config_.t = declare_parameter<double>("t", config_.t);
        config_.host_id = declare_parameter<std::string>("vehicle_id", config_.host_id);
    }

References config_, mobilitypath_visualizer::Config::host_id, mobilitypath_visualizer::Config::t, mobilitypath_visualizer::Config::timer_cb_rate, mobilitypath_visualizer::Config::x, mobilitypath_visualizer::Config::y, and mobilitypath_visualizer::Config::z.

Member Function Documentation

◆ callbackMobilityPath()

void mobilitypath_visualizer::MobilityPathVisualizer::callbackMobilityPath ( carma_v2x_msgs::msg::MobilityPath::UniquePtr msg )

private

Definition at line 128 of file mobilitypath_visualizer.cpp.

    {
 
        RCLCPP_DEBUG_STREAM(get_logger(), "Received a msg from sender: " << msg->m_header.sender_id << ", and plan id:" << msg->m_header.plan_id << ", for receiver:"
                        << msg->m_header.recipient_id << ", time:" << msg->m_header.timestamp << ", at now: " << std::to_string(this->now().seconds()));
 
        if (latest_cav_mob_path_msg_.find(msg->m_header.sender_id) != latest_cav_mob_path_msg_.end() &&
            msg->m_header.plan_id.compare(latest_cav_mob_path_msg_[msg->m_header.sender_id].m_header.plan_id) == 0 &&
            msg->m_header.timestamp == latest_cav_mob_path_msg_[msg->m_header.sender_id].m_header.timestamp)
        {
            RCLCPP_DEBUG_STREAM(get_logger(), "Already received this plan id:" << msg->m_header.plan_id << "from sender_id: " << msg->m_header.sender_id);
            return;
        }
        latest_cav_mob_path_msg_[msg->m_header.sender_id] = *msg;
 
        if (!map_projector_) {
            RCLCPP_ERROR(get_logger(), "Cannot visualize mobility path as map projection not yet available");
            return;
        }
 
        if (msg->m_header.timestamp == 0) //if empty
        {
            host_marker_received_ = false;
            return;
        }
 
        MarkerColor cav_color;
        if (msg->m_header.sender_id.compare(config_.host_id) == 0)
        {
            cav_color.green = 1.0;
 
            host_marker_ = composeVisualizationMarker(*msg,cav_color);
            host_marker_received_ = true;
 
            RCLCPP_DEBUG_STREAM(get_logger(), "Composed host marker successfuly!");
        }
        else
        {
 
            cav_color.blue = 1.0;
            cav_markers_.push_back(composeVisualizationMarker(*msg,cav_color));
 
            RCLCPP_DEBUG_STREAM(get_logger(), "Composed cav marker successfuly! with sender_id: " << msg->m_header.sender_id);
        }
 
    }

References mobilitypath_visualizer::MarkerColor::blue, cav_markers_, composeVisualizationMarker(), config_, mobilitypath_visualizer::MarkerColor::green, mobilitypath_visualizer::Config::host_id, host_marker_, host_marker_received_, latest_cav_mob_path_msg_, map_projector_, and carma_cooperative_perception::to_string().

Referenced by handle_on_configure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ composeLabelMarker()

visualization_msgs::msg::MarkerArray mobilitypath_visualizer::MobilityPathVisualizer::composeLabelMarker	(	const visualization_msgs::msg::MarkerArray &	host_marker,
		const std::vector< visualization_msgs::msg::MarkerArray > &	cav_markers
	)		const

Compose a label marker that displays whether if any of the cav's path cross with that of host (respective points are within 1 meter)

Parameters

host_marker	Host marker's visualization marker as arrow type
cav_markers	Other CAV marker's visualization markers as arrow type

Note: This function assumes that every point's timestamp in marker is matched

Returns: Visualization Marker in text type

Definition at line 280 of file mobilitypath_visualizer.cpp.

    {
        visualization_msgs::msg::MarkerArray output;
        visualization_msgs::msg::Marker marker;
        marker.header.frame_id = "map";
        marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
        marker.action = visualization_msgs::msg::Marker::ADD;
        marker.ns = "mobilitypath_visualizer";
 
        marker.scale.x = config_.x;
        marker.scale.y = config_.y;
        marker.scale.z = config_.z;
        marker.color.r = 1.0;
        marker.color.g = 1.0;
        marker.color.b = 1.0;
        marker.color.a = 1.0;
        marker.frame_locked = true;
        marker.lifetime = builtin_interfaces::msg::Duration((rclcpp::Duration(config_.t * 1e9)));
 
        for (auto const& cav_marker: cav_markers)
        {
            size_t idx = 0;
            while(idx < host_marker.markers.size() && idx < cav_marker.markers.size())
            {
                if (compute_2d_distance(cav_marker.markers[idx].points[0], host_marker.markers[idx].points[0]) <= 1.0) // within 1 meter
                {
                    marker.id = (int32_t)idx;
                    marker.header.stamp = host_marker.markers[idx].header.stamp;
                    marker.pose.position.x = cav_marker.markers[idx].points[0].x;
                    marker.pose.position.y = cav_marker.markers[idx].points[0].y;
                    marker.pose.position.z = cav_marker.markers[idx].points[0].z;
                    marker.pose.orientation.w = 1.0f;
 
                    std::string collision_time = std::to_string((rclcpp::Time(cav_marker.markers[idx].header.stamp) - rclcpp::Time(host_marker.markers[0].header.stamp)).seconds());
                    collision_time = collision_time.substr(0,collision_time.size() - 4); //reduce precision to 2 decimals
                    marker.text = "Collision in " + collision_time + "s!";
 
                    output.markers.push_back(marker);
                }
                idx++;
            }
 
            if (compute_2d_distance(cav_marker.markers[idx-1].points[1], host_marker.markers[idx-1].points[1]) <= 1.0) // within 1 meter
            {
                // last point
                marker.id = (int32_t)idx;
                marker.header.stamp = host_marker.markers[idx-1].header.stamp;
                marker.pose.position.x = cav_marker.markers[idx-1].points[1].x;
                marker.pose.position.y = cav_marker.markers[idx-1].points[1].y;
                marker.pose.position.z = cav_marker.markers[idx-1].points[1].z;
 
                rclcpp::Time cav_marker_stamp(cav_marker.markers[idx-1].header.stamp);
                rclcpp::Time host_marker_stamp(host_marker.markers[0].header.stamp);
                rclcpp::Duration duration = cav_marker_stamp - host_marker_stamp;
                std::string collision_time = std::to_string(duration.seconds());
                collision_time = collision_time.substr(0,collision_time.size() - 4); //reduce precision to 2 decimals
                marker.text = "Collision in " + collision_time + "s!";
                output.markers.push_back(marker);
            }
        }
 
        return output;
    }

References mobilitypath_visualizer::anonymous_namespace{mobilitypath_visualizer.cpp}::compute_2d_distance(), config_, mobilitypath_visualizer::Config::t, carma_cooperative_perception::to_string(), mobilitypath_visualizer::Config::x, mobilitypath_visualizer::Config::y, and mobilitypath_visualizer::Config::z.

Referenced by timer_callback().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ composeVisualizationMarker()

visualization_msgs::msg::MarkerArray mobilitypath_visualizer::MobilityPathVisualizer::composeVisualizationMarker	(	const carma_v2x_msgs::msg::MobilityPath &	msg,
		const MarkerColor &	color
	)

Compose a visualization marker for mobilitypath messages.

Parameters

msg	Mobiliy path message
color	color to visualize the marker, for example host car should have different color than other car

Returns: Visualization Marker in arrow type

Definition at line 175 of file mobilitypath_visualizer.cpp.

    {
        visualization_msgs::msg::MarkerArray output;
 
        visualization_msgs::msg::Marker marker;
        marker.header.frame_id = "map";
 
        rclcpp::Time marker_header_stamp = rclcpp::Time((msg.m_header.timestamp/1000.0) * 1e9);
        marker.header.stamp = builtin_interfaces::msg::Time(marker_header_stamp);
        marker.type = visualization_msgs::msg::Marker::ARROW;
        marker.action = visualization_msgs::msg::Marker::ADD;
        marker.ns = "mobilitypath_visualizer";
 
        marker.scale.x = config_.x;
        marker.scale.y = config_.y;
        marker.scale.z = config_.z;
        marker.frame_locked = true;
 
        marker.color.r = (float)color.red;
        marker.color.g = (float)color.green;
        marker.color.b = (float)color.blue;
        marker.color.a = 1.0f;
 
        size_t count = std::max(prev_marker_list_size_[msg.m_header.sender_id], msg.trajectory.offsets.size());
 
        auto curr_location_msg = msg; //variable to update on each iteration as offsets are measured since last traj point
 
        marker.id = 0;
        geometry_msgs::msg::Point arrow_start;
        geometry_msgs::msg::Point arrow_end;
 
        if (msg.trajectory.offsets.empty())
        {
            marker.action = visualization_msgs::msg::Marker::DELETE;
        }
        else
        {
            RCLCPP_DEBUG_STREAM(get_logger(), "ECEF point x: " << curr_location_msg.trajectory.location.ecef_x << ", y:" << curr_location_msg.trajectory.location.ecef_y);
            arrow_start = ECEFToMapPoint(curr_location_msg.trajectory.location); //also convert from cm to m
            RCLCPP_DEBUG_STREAM(get_logger(), "Map point x: " << arrow_start.x << ", y:" << arrow_start.y);
 
            curr_location_msg.trajectory.location.ecef_x += + msg.trajectory.offsets[0].offset_x;
            curr_location_msg.trajectory.location.ecef_y += + msg.trajectory.offsets[0].offset_y;
            curr_location_msg.trajectory.location.ecef_z += + msg.trajectory.offsets[0].offset_z;
            arrow_end = ECEFToMapPoint(curr_location_msg.trajectory.location); //also convert from cm to m
 
            marker.points.push_back(arrow_start);
            marker.points.push_back(arrow_end);
        }
 
        output.markers.push_back(marker);
 
        for (size_t i = 1; i < count; i++) // start id = 1 to comply with arrow marker type
        {
            marker.id = i;
            rclcpp::Time marker_cur_time(marker.header.stamp);
            //Update time by 0.1s
            rclcpp::Time updated_time =  marker_cur_time + rclcpp::Duration(0.1 * 1e9);
            marker.header.stamp = builtin_interfaces::msg::Time(updated_time);
 
            if (i >= msg.trajectory.offsets.size()) { // If we need to delete previous points
                marker.action = visualization_msgs::msg::Marker::DELETE;
                output.markers.push_back(marker);
                continue;
            }
 
            marker.points = {};
            RCLCPP_DEBUG_STREAM(get_logger(), "ECEF Point- DEBUG x: " << curr_location_msg.trajectory.location.ecef_x << ", y:" << curr_location_msg.trajectory.location.ecef_y);
            arrow_start = ECEFToMapPoint(curr_location_msg.trajectory.location); //convert from cm to m
            RCLCPP_DEBUG_STREAM(get_logger(), "Map Point- DEBUG x: " << arrow_start.x << ", y:" << arrow_start.y);
 
            curr_location_msg.trajectory.location.ecef_x += msg.trajectory.offsets[i].offset_x;
            curr_location_msg.trajectory.location.ecef_y += msg.trajectory.offsets[i].offset_y;
            curr_location_msg.trajectory.location.ecef_z += msg.trajectory.offsets[i].offset_z;
            arrow_end = ECEFToMapPoint(curr_location_msg.trajectory.location);
 
            marker.points.push_back(arrow_start);
            marker.points.push_back(arrow_end);
 
            output.markers.push_back(marker);
        }
        RCLCPP_DEBUG_STREAM(get_logger(), "Last ECEF Point- DEBUG x: " << curr_location_msg.trajectory.location.ecef_x << ", y:" << curr_location_msg.trajectory.location.ecef_y);
        RCLCPP_DEBUG_STREAM(get_logger(), "Last Map Point- DEBUG x: " << arrow_end.x << ", y:" << arrow_end.y);
        prev_marker_list_size_[msg.m_header.sender_id] = msg.trajectory.offsets.size();
 
        return output;
    }

References mobilitypath_visualizer::MarkerColor::blue, config_, ECEFToMapPoint(), mobilitypath_visualizer::MarkerColor::green, process_bag::i, prev_marker_list_size_, mobilitypath_visualizer::MarkerColor::red, mobilitypath_visualizer::Config::x, mobilitypath_visualizer::Config::y, and mobilitypath_visualizer::Config::z.

Referenced by callbackMobilityPath().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ ECEFToMapPoint()

geometry_msgs::msg::Point mobilitypath_visualizer::MobilityPathVisualizer::ECEFToMapPoint ( const carma_v2x_msgs::msg::LocationECEF & ecef_point ) const

Accepts ECEF point in cm to convert to a point in map in meters.

Parameters

ecef_point ECEF point to convert in cm

Returns: Point in map

Definition at line 263 of file mobilitypath_visualizer.cpp.

    {
 
        if (!map_projector_) {
            throw std::invalid_argument("No map projector available for ecef conversion");
        }
        geometry_msgs::msg::Point output;
 
        lanelet::BasicPoint3d map_point = map_projector_->projectECEF( { (double)ecef_point.ecef_x/100.0, (double)ecef_point.ecef_y/100.0, (double)ecef_point.ecef_z/100.0 } , -1);
        output.x = map_point.x();
        output.y = map_point.y();
        output.z = map_point.z();
 
        return output;
    }

References map_projector_.

Referenced by composeVisualizationMarker().

Here is the caller graph for this function:

◆ georeferenceCallback()

void mobilitypath_visualizer::MobilityPathVisualizer::georeferenceCallback ( std_msgs::msg::String::UniquePtr msg )

Callback for map projection string to define lat/lon -> map conversion.

msg The proj string defining the projection.

Definition at line 119 of file mobilitypath_visualizer.cpp.

    {
        if (georeference_ != msg->data)
        {
            georeference_ = msg->data;
            map_projector_ = std::make_shared<lanelet::projection::LocalFrameProjector>(msg->data.c_str());  // Build projector from proj string
        }
    }

References georeference_, and map_projector_.

Referenced by handle_on_configure().

Here is the caller graph for this function:

◆ handle_on_activate()

carma_ros2_utils::CallbackReturn mobilitypath_visualizer::MobilityPathVisualizer::handle_on_activate ( const rclcpp_lifecycle::State & )

Definition at line 87 of file mobilitypath_visualizer.cpp.

                                                                                                          {
 
        //Setup timer
        timer_ = create_timer(get_clock(), std::chrono::milliseconds(config_.timer_cb_rate),
        std::bind(&MobilityPathVisualizer::timer_callback, this));
 
        return CallbackReturn::SUCCESS;
    }

References config_, timer_, timer_callback(), and mobilitypath_visualizer::Config::timer_cb_rate.

Here is the call graph for this function:

◆ handle_on_configure()

carma_ros2_utils::CallbackReturn mobilitypath_visualizer::MobilityPathVisualizer::handle_on_configure ( const rclcpp_lifecycle::State & )

Definition at line 52 of file mobilitypath_visualizer.cpp.

    {
        //Reset Config
        config_ = Config();
 
        //Load parameters
        get_parameter<int>("timer_cb_rate", config_.timer_cb_rate);
        get_parameter<double>("x", config_.x);
        get_parameter<double>("y", config_.y);
        get_parameter<double>("z", config_.z);
        get_parameter<double>("t", config_.t);
 
        get_parameter("vehicle_id", config_.host_id);
 
        RCLCPP_INFO_STREAM(get_logger(), "Loaded params "<< config_);
 
        // init publishers
        host_marker_pub_ = create_publisher<visualization_msgs::msg::MarkerArray>("host_marker", 1);
        cav_marker_pub_ = create_publisher<visualization_msgs::msg::MarkerArray>("cav_marker", 1);
        label_marker_pub_ = create_publisher<visualization_msgs::msg::MarkerArray>("label_marker", 1);
 
        // init subscribers
        host_mob_path_sub_ = create_subscription<carma_v2x_msgs::msg::MobilityPath>("mobility_path_msg", 10,
                                                              std::bind(&MobilityPathVisualizer::callbackMobilityPath, this, std_ph::_1));
 
        cav_mob_path_sub_ = create_subscription<carma_v2x_msgs::msg::MobilityPath>("incoming_mobility_path", 10,
                                                              std::bind(&MobilityPathVisualizer::callbackMobilityPath, this, std_ph::_1));
 
        georeference_sub_ = create_subscription<std_msgs::msg::String>("georeference", 10,
                                                              std::bind(&MobilityPathVisualizer::georeferenceCallback, this, std_ph::_1));
 
        // Return success if everthing initialized successfully
        return CallbackReturn::SUCCESS;
    }

References callbackMobilityPath(), cav_marker_pub_, cav_mob_path_sub_, config_, georeference_sub_, georeferenceCallback(), mobilitypath_visualizer::Config::host_id, host_marker_pub_, host_mob_path_sub_, label_marker_pub_, mobilitypath_visualizer::Config::t, mobilitypath_visualizer::Config::timer_cb_rate, mobilitypath_visualizer::Config::x, mobilitypath_visualizer::Config::y, and mobilitypath_visualizer::Config::z.

Here is the call graph for this function:

◆ initialize()

void mobilitypath_visualizer::MobilityPathVisualizer::initialize ( )

private

◆ matchTrajectoryTimestamps()

std::vector< visualization_msgs::msg::MarkerArray > mobilitypath_visualizer::MobilityPathVisualizer::matchTrajectoryTimestamps	(	const visualization_msgs::msg::MarkerArray &	host_marker,
		const std::vector< visualization_msgs::msg::MarkerArray > &	cav_markers
	)		const

Matches timestamps of CAV's individual points of cav_markers to that of host_marker and interpolates their points using the speed between points.

Parameters

host_marker	Host marker's visualization marker as arrow type
cav_markers	Other CAV marker's visualization markers as arrow type

Note: This function assumes 0.1s between any of the points. It also drops CAV markers that start later than the first point in host_marker does. This is acceptable as the host is publishing in 0.1s and we don't need to visualize points in the future. It extrapolates the last point CAV's just to conform with

Returns: Synchronized CAV markers

Definition at line 344 of file mobilitypath_visualizer.cpp.

    {
        if (host_marker.markers.empty() || host_marker.markers[0].action == visualization_msgs::msg::Marker::DELETE)
            return cav_markers;
 
        std::vector<visualization_msgs::msg::MarkerArray> synchronized_output;
        for (auto const& curr_cav: cav_markers)
        {
            visualization_msgs::msg::MarkerArray synchronized_marker_array;
            signed int curr_idx = 0;
            double time_step = 0.1;
            // although it is very rare to reach here
            // we do not need to visualize other car that starts "in the future", so skip
 
            if (rclcpp::Time(curr_cav.markers[0].header.stamp) > rclcpp::Time(host_marker.markers[0].header.stamp) ||
            curr_cav.markers[0].action == visualization_msgs::msg::Marker::DELETE)
            {
                continue;
            }
            // this marker is outdated, drop
 
            rclcpp::Time curr_cav_marker_stamp(curr_cav.markers.back().header.stamp);
            rclcpp::Time host_marker_stamp(host_marker.markers[0].header.stamp);
 
 
            if (curr_cav_marker_stamp + rclcpp::Duration(time_step * 1e9) < host_marker_stamp){
                continue;
            }
 
            // skip points until the idx to start interpolating
            while (curr_idx < curr_cav.markers.size() && rclcpp::Time(curr_cav.markers[curr_idx].header.stamp) <= rclcpp::Time(host_marker.markers[0].header.stamp))
            {
                curr_idx++;
            }
 
            curr_idx -= 1; // carma_v2x_msg stamp is before that of host now
            // interpolate position to match the starting time (dt < time_step)
 
            if (curr_idx < 0 )
                curr_idx = 0;
 
            double dt = (rclcpp::Time(host_marker.markers[0].header.stamp) - rclcpp::Time(curr_cav.markers[curr_idx].header.stamp)).seconds();
 
            rclcpp::Time curr_time(host_marker.markers[0].header.stamp);
 
            // only update start_point of each arrow type marker for now
 
            while (curr_idx < curr_cav.markers.size())
            {
                if (curr_cav.markers[curr_idx].action == visualization_msgs::msg::Marker::DELETE)
                    break;
 
                visualization_msgs::msg::Marker curr_marker = curr_cav.markers[curr_idx]; //copy static info
                if (dt != 0.0 && !curr_cav.markers[curr_idx].points.empty()) // if not already synchronized
                {
                    double dx = (curr_cav.markers[curr_idx].points[1].x - curr_cav.markers[curr_idx].points[0].x)/time_step * dt;
                    double dy = (curr_cav.markers[curr_idx].points[1].y - curr_cav.markers[curr_idx].points[0].y)/time_step * dt;
                    double dz = (curr_cav.markers[curr_idx].points[1].z - curr_cav.markers[curr_idx].points[0].z)/time_step * dt;
 
                    curr_marker.points[0].x += dx;
                    curr_marker.points[0].y += dy;
                    curr_marker.points[0].z += dz;
 
                } //else just loop and copy
                curr_marker.header.stamp = curr_time;
                synchronized_marker_array.markers.push_back(curr_marker);
                curr_idx ++;
                curr_time += rclcpp::Duration(time_step, 0.0);
            }
 
            curr_idx = 0; // resetting idx to work on new, synchronized list
 
 
            // update end_point of each arrow type marker, except that of last point
            while (curr_idx < synchronized_marker_array.markers.size()-1)
            {
                if (curr_cav.markers[curr_idx].action == visualization_msgs::msg::Marker::DELETE)
                    break;
 
                if (!synchronized_marker_array.markers[curr_idx].points.empty())
                    synchronized_marker_array.markers[curr_idx].points[1] = synchronized_marker_array.markers[curr_idx + 1].points[0];
                curr_idx ++;
            }
 
            if (!synchronized_marker_array.markers[curr_idx].points.empty())
            {
                // extrapolate the last point just to conform with 0.1s interval between points
                double dx = (synchronized_marker_array.markers[curr_idx].points[1].x - synchronized_marker_array.markers[curr_idx].points[0].x)/(time_step - dt) * time_step;
                double dy = (synchronized_marker_array.markers[curr_idx].points[1].y - synchronized_marker_array.markers[curr_idx].points[0].y)/(time_step - dt) * time_step;
                double dz = (synchronized_marker_array.markers[curr_idx].points[1].z - synchronized_marker_array.markers[curr_idx].points[0].z)/(time_step - dt) * time_step;
 
                synchronized_marker_array.markers[curr_idx].points[1].x = synchronized_marker_array.markers[curr_idx].points[0].x + dx;
                synchronized_marker_array.markers[curr_idx].points[1].y = synchronized_marker_array.markers[curr_idx].points[0].y + dy;
                synchronized_marker_array.markers[curr_idx].points[1].z = synchronized_marker_array.markers[curr_idx].points[0].z + dz;
 
            }
 
            synchronized_output.push_back(synchronized_marker_array);
 
        }
 
        return synchronized_output;
    }

Referenced by timer_callback().

Here is the caller graph for this function:

◆ timer_callback()

void mobilitypath_visualizer::MobilityPathVisualizer::timer_callback ( )

private

Definition at line 96 of file mobilitypath_visualizer.cpp.

    {
        // match cav_markers' timestamps to that of host
        if (!host_marker_received_){
            return;
        }
 
        // publish host marker
        host_marker_pub_->publish(host_marker_);
 
        cav_markers_ = matchTrajectoryTimestamps(host_marker_, cav_markers_);
 
        for (auto const &marker: cav_markers_)
        {
            cav_marker_pub_->publish(marker);
        }
 
        // publish label
        label_marker_ = composeLabelMarker(host_marker_, cav_markers_);
        label_marker_pub_->publish(label_marker_);
 
    }