#!/usr/bin/env python
import rospy
from roslib import message
import sensor_msgs.point_cloud2 as pc2
from std_msgs.msg import String
from sensor_msgs.msg import PointCloud2
import plotly.graph_objects as go
import numpy as np

PLOTTED = False

def ros_to_pcl(ros_cloud):
    """ Converts a ROS PointCloud2 message to a pcl PointXYZRGB

        Args:
            ros_cloud (PointCloud2): ROS PointCloud2 message

        Returns:
            pcl.PointCloud_PointXYZRGB: PCL XYZRGB point cloud
    """
    points_list = []

    for data in pc2.read_points(ros_cloud, skip_nans=True):
        points_list.append([data[0], data[1], data[2], data[3]])

    #pcl_data = pcl.PointCloud_PointXYZRGB()
    #pcl_data.from_list(points_list)

    return np.array(points_list) 

def save_cloud(message):
    
    #get_caller_id(): Get fully resolved name of local node
    #cose  = np.array(message.data)

    global PLOTTED
    print("A")

    #for a in message.data:
    #    print(a)
    point_cloud =  ros_to_pcl(message)
    print(point_cloud.shape)
    with open('/home/riccardo.franceschin/Phd/ros_code/map_cloud/map.npy', 'wb') as f:
        np.save(f, point_cloud)
    exit()

    #if not PLOTTED:
    #    PLOTTED = True
    #    plot_cloud(point_cloud)


    #rospy.loginfo(rospy.get_caller_id() + "I heard %s", message)
    
def plot_cloud(point_cloud):

    fig = go.Figure(data=[go.Scatter3d(
                    x=point_cloud[:,0],
                    y=point_cloud[:,1],
                    z=point_cloud[:,2],
                    mode='markers',
                    marker=dict(
                        size=8,
                        color=point_cloud[:,2],                # set color to an array/list of desired values
                        colorscale='Viridis',   # choose a colorscale
                        opacity=0.8
                    )
                    )])
    fig.show()

def listener():

    # In ROS, nodes are uniquely named. If two nodes with the same
    # node are launched, the previous one is kicked off. The
    # anonymous=True flag means that rospy will choose a unique
    # name for our 'listener' node so that multiple listeners can
    # run simultaneously.
    rospy.init_node('listener', anonymous=True)

    rospy.Subscriber("/voxblox_rrt_planner/surface_pointcloud", PointCloud2, save_cloud)

    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()

if __name__ == '__main__':
    listener()

#!/usr/bin/env python
import rospy
from roslib import message
import sensor_msgs.point_cloud2 as pc2
from std_msgs.msg import String
from sensor_msgs.msg import PointCloud2
import plotly.graph_objects as go
import numpy as np

PLOTTED = False

def ros_to_pcl(ros_cloud):
    """ Converts a ROS PointCloud2 message to a pcl PointXYZRGB

        Args:
            ros_cloud (PointCloud2): ROS PointCloud2 message

        Returns:
            pcl.PointCloud_PointXYZRGB: PCL XYZRGB point cloud
    """
    points_list = []

    for data in pc2.read_points(ros_cloud, skip_nans=True):
        points_list.append([data[0], data[1], data[2], data[3]])

    #pcl_data = pcl.PointCloud_PointXYZRGB()
    #pcl_data.from_list(points_list)

    return np.array(points_list) 

def save_cloud(message):
    
    #get_caller_id(): Get fully resolved name of local node
    #cose  = np.array(message.data)

    global PLOTTED
    print("A")

    #for a in message.data:
    #    print(a)
    point_cloud =  ros_to_pcl(message)
    print(point_cloud.shape)
    with open('/home/riccardo.franceschin/Phd/ros_code/map_cloud/map.npy', 'wb') as f:
        np.save(f, point_cloud)
    exit()

    #if not PLOTTED:
    #    PLOTTED = True
    #    plot_cloud(point_cloud)


    #rospy.loginfo(rospy.get_caller_id() + "I heard %s", message)
    
def plot_cloud(point_cloud):

    fig = go.Figure(data=[go.Scatter3d(
                    x=point_cloud[:,0],
                    y=point_cloud[:,1],
                    z=point_cloud[:,2],
                    mode='markers',
                    marker=dict(
                        size=8,
                        color=point_cloud[:,2],                # set color to an array/list of desired values
                        colorscale='Viridis',   # choose a colorscale
                        opacity=0.8
                    )
                    )])
    fig.show()

def listener():

    # In ROS, nodes are uniquely named. If two nodes with the same
    # node are launched, the previous one is kicked off. The
    # anonymous=True flag means that rospy will choose a unique
    # name for our 'listener' node so that multiple listeners can
    # run simultaneously.
    rospy.init_node('listener', anonymous=True)

    rospy.Subscriber("/voxblox_rrt_planner/surface_pointcloud", PointCloud2, save_cloud)

    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()

if __name__ == '__main__':
    listener()

#!/usr/bin/env python Converts a ROS PointCloud2 message to a pcl PointXYZRGB          Args:             ros_cloud (PointCloud2): ROS PointCloud2 message          Returns:             pcl.PointCloud_PointXYZRGB: PCL XYZRGB point cloud #pcl_data = pcl.PointCloud_PointXYZRGB() #pcl_data.from_list(points_list) #get_caller_id(): Get fully resolved name of local node #cose  = np.array(message.data) #for a in message.data: #    print(a) #if not PLOTTED: #    PLOTTED = True #    plot_cloud(point_cloud) #rospy.loginfo(rospy.get_caller_id() + "I heard %s", message) # set color to an array/list of desired values # choose a colorscale # In ROS, nodes are uniquely named. If two nodes with the same # node are launched, the previous one is kicked off. The # anonymous=True flag means that rospy will choose a unique # name for our 'listener' node so that multiple listeners can # run simultaneously. # spin() simply keeps python from exiting until this node is stopped