ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
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.