ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Revision history [back]

include "marker/marker.hpp"

namespace koordinat_publisher { KoordinatPublisher::KoordinatPublisher(ros::NodeHandle &nodeHandle) : nodeHandle_(nodeHandle) {

    if (!readParameters())
    {
        ROS_ERROR("Could not read parameters.");
        ros::requestShutdown();
    }

    // load the map
    m_map_ = lanelet::load(harita, lanelet::Origin({0,0}));

    for (auto point : m_map_->pointLayer)
    {
        point.x() = point.attribute("local_x").asDouble().value();
        point.y() = point.attribute("local_y").asDouble().value();
    }
    path_subscriber_ = nodeHandle_.createTimer(ros::Duration(1), &KoordinatPublisher::markerCallback, this);
    marker_pub_ = nodeHandle_.advertise<visualization_msgs::MarkerArray>("visualization_marker", 0);

    ROS_INFO("Successfully launched node.");
}

bool KoordinatPublisher::readParameters()
{
    if (!nodeHandle_.getParam("harita", harita))
    {
        return false;
    }
    return true;
}

}

void koordinat_publisher::KoordinatPublisher::markerCallback(const ros::TimerEvent &) { int i = 785;

visualization_msgs::MarkerArray marker_array;
for (auto lanelet : m_map_->laneletLayer)
{
    visualization_msgs::Marker marker;
    marker.header.frame_id = "map";
    marker.header.stamp = ros::Time();
    marker.ns = "my_namespace";
    marker.type = visualization_msgs::Marker::LINE_STRIP;
    marker.action = visualization_msgs::Marker::ADD;
    marker.scale.x = 1.0;
    marker.scale.y = 1.0;
    marker.scale.z = 1.0;
    marker.color.a = 1.0;
    marker.color.r = 1.0;
    marker.color.g = 0.0;
    marker.color.b = 0.0;

    std::vector<geometry_msgs::Point> right_bound;
    for (auto point : lanelet.rightBound())
    {
        geometry_msgs::Point tmp;
        tmp.x = point.x();
        tmp.y = point.y();
        tmp.z = 0.0;
        right_bound.push_back(tmp);
    }

    marker.id = i;
    marker.points = right_bound;
    marker_array.markers.push_back(marker);
    i++;

    std::vector<geometry_msgs::Point> left_bound;
    for (auto point : lanelet.leftBound())
    {
        geometry_msgs::Point tmp;
        tmp.x = point.x();
        tmp.y = point.y();
        tmp.z = 0.0;
        left_bound.push_back(tmp);
    }

    marker.id = i;
    marker.points = left_bound;
    marker_array.markers.push_back(marker);
    i++;
}
marker_pub_.publish(marker_array);

}

ps: i hope it can help you. i just added the code. there are also one header file and one node(cpp) file.