rviz TRIANGLE_LIST三角网的画法

最新推荐文章于 2023-10-31 11:48:02 发布

河桐

最新推荐文章于 2023-10-31 11:48:02 发布

阅读量825

点赞数 1

分类专栏： c++ ROS

原文链接：https://github.com/iralabdisco/ira_open_street_map/blob/master/src/osm_query_node.cpp

版权

c++ 同时被 2 个专栏收录

33 篇文章 0 订阅

订阅专栏

ROS

7 篇文章 0 订阅

订阅专栏

https://github.com/iralabdisco/ira_open_street_map/blob/master/src/osm_query_node.cpp

这里面有个例子，可以实现

void load_buildinglist()
	{
	buildings.header.frame_id = "map";
	buildings.header.stamp = ros::Time();
	buildings.ns = "buildings";
	buildings.id = 3;
	buildings.type = visualization_msgs::Marker::TRIANGLE_LIST;
	buildings.action = visualization_msgs::Marker::ADD;
	buildings.scale.x = 1.0;
	buildings.scale.y = 1.0;
	buildings.scale.z = 1.0;
	buildings.color.r = 0.0f;
	buildings.color.g = 0.9f;
	buildings.color.b = 0.7f;
	buildings.color.a = 0.75f;

	Xy coords_A, coords_B;
	float height = 3.0f;
	geometry_msgs::Point A, h_A, B, h_B;

	for(std::set<shared_ptr<Osmium::OSM::Way const> >::iterator way_itr = oh.m_ways.begin(); way_itr != oh.m_ways.end(); way_itr++)
	{
	// Get way-node list
	Osmium::OSM::WayNodeList waylist = (*way_itr)->nodes();

	// Use only ways with Key:building
	const char* building_tag = (*way_itr)->tags().get_value_by_key("building");

	if(!building_tag)
	continue;

	for(Osmium::OSM::WayNodeList::iterator node_list_itr = waylist.begin(); node_list_itr != waylist.end() - 1; node_list_itr++ )
	{
	// Get way-node coordinates
	coords_A = latlon2xy_helper(node_list_itr->position().lat(),node_list_itr->position().lon());
	coords_B = latlon2xy_helper((node_list_itr + 1)->position().lat(),(node_list_itr + 1)->position().lon());

	A.x = coords_A.x;
	A.y = coords_A.y;
	A.z = 0.0;

	h_A.x = A.x;
	h_A.y = A.y;
	h_A.z = A.z + height;

	B.x = coords_B.x;
	B.y = coords_B.y;
	B.z = 0.0;

	h_B.x = B.x;
	h_B.y = B.y;
	h_B.z = B.z + height;

	buildings.points.push_back(A);
	buildings.points.push_back(h_A);
	buildings.points.push_back(B);
	buildings.points.push_back(B);
	buildings.points.push_back(h_A);
	buildings.points.push_back(h_B);
	}
	}
	}