def handle_scanning_poses_request(req):
rospy.loginfo('PoseSettingRequest Received array length %s', len(req.path_x))
if len(req.path_x) == epochs:
success = True
for i in range(0, epochs):
print "epochs = %d" %(i+1)
global Pose_x
Pose_x = req.path_x[i]
global Pose_y
Pose_y = req.path_y[i]
global Pose_z
Pose_z = req.path_z[i]
print("pose x = %f") %Pose_x
print("pose y = %f") %Pose_y
print("pose z = %f") %Pose_z
# Create an exporter
exporter = scan_exporter.ScanExporter()
# Create a scanner base
base = scanner_base.ScannerBase()
#settings for sweep
settings = scan_settings.ScanSettings()
# Create sweep sensor, and perform scan
with Sweep('/dev/ttyUSB0') as sweep:
# Create a scanner object
time.sleep(1.0)
scanner = Scanner( \
device=sweep, base=base, settings=settings, exporter=exporter)
# Setup the scanner
scanner.setup()
# Perform the scan
scanner.perform_scan()
global epochs
epochs -= 1
# Stop the scanner
time.sleep(1.0)
#scanner.idle()
base.turn_off_motors()
else:
success = False
rate = rospy.Rate(0.5)
cloud_publisher = rospy.Publisher('/sweep_node/cloudpoint', PointCloud2, queue_size = 1)
while not rospy.is_shutdown():
point_cloud2 = pcl2.create_cloud(HEADER, FIELDS, Cloud_Points) #scanner.Points
#publish the point cloud message via ros
cloud_publisher.publish(point_cloud2)
rate.sleep()
return Set_Scanning_PoseResponse(success)
def main(arg_dict):
"""Creates a 3D scanner and gather a scan"""
# Create a scan settings obj
settings = scan_settings.ScanSettings(
int(arg_dict['motor_speed']), # desired motor speed setting
int(arg_dict['sample_rate']), # desired sample rate setting
int(arg_dict['dead_zone']), # starting angle of deadzone
int(arg_dict['angular_range']), # angular range of scan
# mount angle of device relative to horizontal
int(arg_dict['mount_angle']))
# Create an exporter
exporter = scan_exporter.ScanExporter(
file_name=socket.gethostname() + " - " + arg_dict['output'] + " " +
" __ MS-" + str(arg_dict['motor_speed']) + " __ SR-" +
str(arg_dict['sample_rate']) + " __ AR-" +
str(arg_dict['angular_range']) + " __ MA-" +
str(arg_dict['mount_angle']) + " __ DZ-" + str(arg_dict['dead_zone']) +
".csv")
use_dummy = arg_dict['use_dummy']
# Create a scanner base
base = scanner_base.ScannerBase(use_dummy=use_dummy)
# Create sweep sensor, and perform scan
with sweep_helpers.create_sweep_w_error('/dev/ttyUSB0',
use_dummy) as (sweep, err):
if err:
output_json_message({
'type':
"update",
'status':
"failed",
'msg':
"Failed to connect to sweep device... make sure it is plugged in."
})
time.sleep(0.1)
return
# Create a scanner object
time.sleep(1.0)
scanner = Scanner(device=sweep,
base=base,
settings=settings,
exporter=exporter)
# Setup the scanner
scanner.setup()
# Perform the scan
scanner.perform_scan()
# Stop the scanner
time.sleep(1.0)
scanner.idle()
def __init__(self, device=None, base=None, settings=None, exporter=None):
"""Return a Scanner object
:param base: the scanner base
:param device: the sweep device
:param settings: the scan settings
:param exporter: the scan exporter
"""
if base is None:
base = scanner_base.ScannerBase()
if settings is None:
settings = scan_settings.ScanSettings()
if exporter is None:
exporter = scan_exporter.ScanExporter()
self.base = base
self.settings = settings
self.exporter = exporter
def __init__(self, device=None, base=None, settings=None, exporter=None):
"""Return a Scanner object
:param base: the scanner base
:param device: the sweep device
:param settings: the scan settings
:param exporter: the scan exporter
"""
if device is None:
self.shutdown("Please provide a device to scanner constructor.")
if base is None:
base = scanner_base.ScannerBase()
if settings is None:
settings = scan_settings.ScanSettings()
if exporter is None:
exporter = scan_exporter.ScanExporter()
self.base = base
self.device = device
self.settings = settings
self.exporter = exporter
self.received_scan = False
def set_base(self, base=None):
"""Sets the base for this scanner to the provided ScannerBase object"""
if base is None:
base = scanner_base.ScannerBase()
self.base = base