/udom_utils/udom_visualization/src/udom_visualization/mesh_publisher_gui_node.py

#!/usr/bin/env python
# -*- encoding: utf-8 -*-
"""
This node publishes a configuration of a mesh based on .ele and .node files.
The mesh configuration can be changed at run-time using via a GUI.

**Output(s):**
  * `mesh`: The published mesh.
    - *type:* `udom_modeling_msgs/Mesh`

**Parameter(s):**
  * `loop_rate`: Node cycle rate (in Hz).
  * `zero_index`: Whether the elements index start from zero (false means they start from one).
  * `mesh_config`: The name of the folder containing a .ele and a .node file with the same
    name as the directory, which describe the configuration of the mesh. The path is assumed
    to be the `config` directory of this package.

"""

import threading
import Tkinter
import numpy as np
import csv
import rospy
import rospkg
import std_msgs.msg
import geometry_msgs.msg
import udom_modeling_msgs.msg


ros_pack = rospkg.RosPack()
package_path = ros_pack.get_path('udom_visualization')

mesh = udom_modeling_msgs.msg.Mesh()
mesh_file = 'bar_rest'
event = std_msgs.msg.String()

global lock
lock = threading.Lock()


def create_window():
    """
    Creates a GUI window to publish a pose.

    """
    master = Tkinter.Tk()

    # Mesh entry.
    mesh_entry = Tkinter.Entry(master)
    mesh_entry.insert(Tkinter.END, mesh_file)
    mesh_entry.grid(row=1, column=0)

    upload_button = Tkinter.Button(
        master, text='Update', command=(lambda e=mesh_entry: load_mesh_cb(e)))
    upload_button.grid(row=2, column=0)

    # Event buttons.
    start_button = Tkinter.Button(master, command=start_cb, text="START")
    stop_button = Tkinter.Button(master, command=stop_cb, text="STOP")
    reset_button = Tkinter.Button(master, command=reset_cb, text="RESET")
    start_button.grid(row=0, column=1)
    stop_button.grid(row=1, column=1)
    reset_button.grid(row=2, column=1)

    master.title("Mesh selector")
    master.mainloop()
    rospy.signal_shutdown("GUI closed")


def load_mesh_cb(entry):
    """
    Loads the mesh.

    :param entry: The entry object containing the mesh to be uploaded.
    :type entry: Tkinter.Entry

    """
    global lock
    lock.acquire()
    mesh_file = entry.get()
    config_path = package_path + '/config/' + mesh_file + '/'
    # TODO: handle the case where zero_index is False
    update_mesh(mesh, config_path, mesh_file)
    event.data = 'e_start'
    lock.release()


def start_cb():
    """
    Updates the event to 'e_start'.

    """
    global lock
    lock.acquire()
    event.data = 'e_start'
    lock.release()


def stop_cb():
    """
    Updates the event to 'e_stop'.

    """
    global lock
    lock.acquire()
    event.data = 'e_stop'
    lock.release()


def reset_cb():
    """
    Updates the event to 'e_reset'.

    """
    global lock
    lock.acquire()
    event.data = 'e_reset'
    lock.release()


def publish_nodal_forces():
    """
    Publishes the nodal forces.

    """
    # Node cycle rate (in Hz).
    loop_rate = rospy.Rate(rospy.get_param('~loop_rate', 100))

    # Publishers
    pub_mesh = rospy.Publisher(
        "~mesh", udom_modeling_msgs.msg.Mesh, queue_size=10)
    pub_event_out = rospy.Publisher('~event_out', std_msgs.msg.String, queue_size=10)

    mesh_path = package_path + '/config/' + mesh_file + '/'
    update_mesh(mesh, mesh_path, mesh_file)

    while not rospy.is_shutdown():
        pub_mesh.publish(mesh)
        if event.data:
            pub_event_out.publish(event)
        event.data = ""
        loop_rate.sleep()


def update_mesh(mesh_, file_dir, filename):
    """
    Updates the configuration of the mesh from 'file_dir' assuming it
    contains the .ele and .nodes files. It returns the mesh as a
    udom_modeling_msgs.msg.Mesh message.

    :param mesh_: The mesh to update.
    :type mesh_: udom_modeling_msgs.msg.Mesh

    :param file_dir: Absolute path to the directory with the .ele and .node
        files of the mesh to upload.
    :type file_dir: str

    :param filename: Filename of the .ele and .node files.
    :type filename: str

    :return: The uploaded mesh.
    :rtype: udom_modeling_msgs.msg.Mesh()

    """
    elements_file = file_dir + filename + '.ele'
    nodes_file = file_dir + filename + '.node'

    try:
        open(elements_file)
        open(nodes_file)
    except IOError as e:
        rospy.logerr("Error: {}".format(e))
        rospy.logwarn("Keeping current mesh.")
        return mesh_

    # Reset mesh.
    mesh_.vertices = []
    mesh_.tetrahedra = []

    first_line = True
    index = 0
    # Process elements
    with open(elements_file) as csv_file:
        reader = csv.reader(csv_file, delimiter=' ')
        # Skip header
        next(reader)
        for row in reader:
            if first_line:
                index = int(row[0])
                first_line = False
            # Skip empty lines
            if not ''.join(row).strip():
                continue
            # Get the values for the element columns and ignore the rest.
            rows = np.array(map(int, row[1:5]))
            if index:
                rows -= index

            tetrahedra = udom_modeling_msgs.msg.MeshTetrahedron(rows)
            mesh_.tetrahedra.append(tetrahedra)

    # Process nodes
    with open(nodes_file) as csv_file:
        reader = csv.reader(csv_file, delimiter=' ')
        # Skip header
        next(reader)
        for row in reader:
            # Skip empty lines
            if not ''.join(row).strip():
                continue
            rows = np.array(map(float, row[-3:]))
            point = geometry_msgs.msg.Point(x=rows[0], y=rows[1], z=rows[2])
            mesh_.vertices.append(point)

    return mesh_


def main():
    rospy.init_node('nodal_forces_mock_up')

    import thread
    try:
        thread.start_new_thread(create_window, tuple())

        publish_nodal_forces()
    except rospy.ROSInterruptException:
        pass