Python find_path_through_point_cloud 예제들

예제 #1

파일: initialization_service.py 프로젝트: warriorarmentaix/lfd

def handle_initialization_request(req):
    "rope initialization service"

    xyz, _ = pc2xyzrgb(req.cloud)
    xyz = np.squeeze(xyz)
    obj_type = determine_object_type(xyz, plotting=False)
    print "object type:", obj_type

    if obj_type == "towel":
        xyz = xyz[(xyz[:, 2] - np.median(xyz[:, 2])) < .05, :]
        center, (height, width), angle = cv2.minAreaRect(
            xyz[:, :2].astype('float32').reshape(1, -1, 2))
        angle *= np.pi / 180
        corners = np.array(center)[None, :] + np.dot(
            np.array([[-height / 2, -width / 2], [-height / 2, width / 2],
                      [height / 2, width / 2], [height / 2, -width / 2]]),
            np.array([[np.cos(angle), np.sin(angle)],
                      [-np.sin(angle), np.cos(angle)]]))
        resp = bs.InitializationResponse()
        resp.objectInit.type = "towel_corners"
        resp.objectInit.towel_corners.polygon.points = [
            gm.Point(corner[0], corner[1], np.median(xyz[:, 2]))
            for corner in corners
        ]
        resp.objectInit.towel_corners.header = req.cloud.header
        print req.cloud.header
        poly_pub.publish(resp.objectInit.towel_corners)

    if obj_type == "rope":
        total_path_3d = find_path_through_point_cloud(xyz, plotting=False)
        resp = bs.InitializationResponse()
        resp.objectInit.type = "rope"
        rope = resp.objectInit.rope = bm.Rope()

        rope.header = req.cloud.header
        rope.nodes = [gm.Point(x, y, z) for (x, y, z) in total_path_3d]
        rope.radius = .006
        rospy.logwarn(
            "TODO: actually figure out rope radius from data. setting to .4cm")

        pose_array = gm.PoseArray()
        pose_array.header = req.cloud.header
        pose_array.poses = [
            gm.Pose(position=point, orientation=gm.Quaternion(0, 0, 0, 1))
            for point in rope.nodes
        ]
        marker_handles[0] = rviz.draw_curve(pose_array, 0)

    return resp

예제 #2

파일: initialization_service.py 프로젝트: benkehoe/python

def handle_initialization_request(req):
    "rope initialization service"
    
    xyz, _ = pc2xyzrgb(req.cloud)
    xyz = np.squeeze(xyz)
    obj_type = determine_object_type(xyz)
    print "object type:", obj_type
    
    
    if obj_type == "towel":
        xyz = xyz[(xyz[:,2]-np.median(xyz[:,2]))<.05,:]
        center, (height, width), angle = cv2.minAreaRect(xyz[:,:2].astype('float32').reshape(1,-1,2))
        angle *= np.pi/180
        corners = np.array(center)[None,:] + np.dot(
            np.array([[-height/2,-width/2],
                      [-height/2,width/2],
                      [height/2, width/2],
                      [height/2, -width/2]]),
            np.array([[np.cos(angle), np.sin(angle)],
                      [-np.sin(angle),np.cos(angle)]])) 
        resp = bs.InitializationResponse()
        resp.objectInit.type = "towel_corners"
        resp.objectInit.towel_corners.polygon.points = [gm.Point(corner[0], corner[1], np.median(xyz[:,2])) for corner in corners]
        resp.objectInit.towel_corners.header = req.cloud.header
        print req.cloud.header
        poly_pub.publish(resp.objectInit.towel_corners)
    
    if obj_type == "rope":
        total_path_3d = find_path_through_point_cloud(xyz, plotting=False)            
        resp = bs.InitializationResponse()
        resp.objectInit.type = "rope"
        rope = resp.objectInit.rope = bm.Rope()
        
        rope.header = req.cloud.header
        rope.nodes = [gm.Point(x,y,z) for (x,y,z) in total_path_3d]    
        rope.radius = .005
        rospy.logwarn("TODO: actually figure out rope radius from data. setting to .5cm")
        
        pose_array = gm.PoseArray()
        pose_array.header = req.cloud.header
        pose_array.poses = [gm.Pose(position=point, orientation=gm.Quaternion(0,0,0,1)) for point in rope.nodes]
        marker_handles[0] = rviz.draw_curve(pose_array,0)

        
    
    return resp

예제 #3

파일: test_rope_init.py 프로젝트: warriorarmentaix/lfd

from jds_image_proc.pcd_io import load_xyzrgb
import rope_vision.rope_initialization as ri
import numpy as np
import sys
import networkx as nx
import itertools
from mayavi import mlab
from brett2 import ros_utils
import sensor_msgs.msg as sm

import rospy
rospy.init_node("test_rope_init")
cloud = rospy.wait_for_message("/preprocessor/points", sm.PointCloud2)
xyz, rgb = ros_utils.pc2xyzrgb(cloud)
# xyz, rgb = load_xyzrgb("data000000000004.pcd")
# xyz = xyz.reshape(-1,3)
#xyz = np.squeeze(np.loadtxt("/tmp/rope.txt"))
total_path_3d = ri.find_path_through_point_cloud(xyz, plotting=True)

예제 #4

파일: test_rope_init.py 프로젝트: benkehoe/python

from image_proc.pcd_io import load_xyzrgb
import rope_vision.rope_initialization as ri
import numpy as np
import sys
import networkx as nx
import itertools
from mayavi import mlab
from brett2 import ros_utils
import sensor_msgs.msg as sm
            
    
import rospy
rospy.init_node("test_rope_init")
cloud = rospy.wait_for_message("/preprocessor/points", sm.PointCloud2)
xyz,rgb = ros_utils.pc2xyzrgb(cloud)
# xyz, rgb = load_xyzrgb("data000000000004.pcd")
# xyz = xyz.reshape(-1,3)
#xyz = np.squeeze(np.loadtxt("/tmp/rope.txt"))
total_path_3d = ri.find_path_through_point_cloud(xyz, plotting=True)