Skip to content

Instantly share code, notes, and snippets.

@pgorczak
Created October 1, 2018 19:06
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save pgorczak/7238d62faeff0cb5df446bfd9218658e to your computer and use it in GitHub Desktop.
Save pgorczak/7238d62faeff0cb5df446bfd9218658e to your computer and use it in GitHub Desktop.
Quick example publishing a mesh for mesh_tools (https://github.com/uos/mesh_tools)
import numpy as np
import geometry_msgs.msg as geo_msgs
import rospy
import mesh_msgs.msg as mesh_msgs
def publish_mesh(pub):
mesh = mesh_msgs.TriangleMeshStamped()
mesh.header.frame_id = '/map'
mesh.header.stamp = rospy.Time.now()
vertices = np.array([[0, 0, 0], [2, 0, 0], [0, 3, 0], [0, 0, 1]])
faces = np.array([[0, 1, 2], [0, 3, 1], [0, 3, 2], [1, 2, 3]])
mesh.mesh.vertices = [geo_msgs.Point(*v) for v in vertices]
mesh.mesh.triangles = [mesh_msgs.TriangleIndices(f) for f in faces]
pub.publish(mesh)
if __name__ == '__main__':
rospy.init_node('rviz_mesh_test')
pub = rospy.Publisher('/mesh', mesh_msgs.TriangleMeshStamped, queue_size=1)
rospy.Timer(rospy.Duration(1), lambda e: publish_mesh(pub))
rospy.spin()
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment