print("Collected points", len(collected_points))
if len(collected_points) == 4:
box = BoundingBoxFromFourPoints(collected_points)
collected_boxes.append(box)
collected_points = []
#⬢⬢⬢⬢⬢➤ NODE
node = RosNode("arp_live_viewer")
#⬢⬢⬢⬢⬢➤ Sets HZ from parameters
node.setHz(node.setupParameter("hz", 30))
debug = node.setupParameter("debug", True)
new_point_publisher = node.createPublisher("~new_point_event", Bool)
new_point_publisher_callback = node.createSubscriber(
"~new_point_event", Bool, newPointCallback)
#⬢⬢⬢⬢⬢➤ Creates Camera Proxy
camera_topic = node.setupParameter(
"camera_topic",
"/usb_cam/image_raw/compressed"
)
camera_file = node.getFileInPackage(
'roars',
'data/camera_calibrations/microsoft_hd_focus2.yml'
)
camera = CameraRGB(
configuration_file=camera_file,
import numpy as np
import math
from roars.rosutils.rosnode import RosNode
import roars.geometry.transformations as transformations
import roars.vision.colors as colors
from visualization_msgs.msg import MarkerArray, Marker
from geometry_msgs.msg import Point
from std_msgs.msg import ColorRGBA
import random
import tools
node = RosNode("ciao", disable_signals=True)
node.setHz(10)
#⬢⬢⬢⬢⬢➤ Visualization topic
vis_topic = node.createPublisher('/showmap/clusters', MarkerArray)
map_file = node.setupParameter("map_file", "")
instance_folder = node.setupParameter("instance_folder", "")
files = glob.glob(os.path.join(instance_folder, "*.txt"))
box_file = ''
box_opt_file = ''
instance_files = []
instances = []
for f in files:
if 'boxes.txt' in f:
box_file = f
elif 'boxes_opt' in f:
min_w = node.setupParameter("min_w", 0)
min_cluster_size = node.setupParameter("min_cluster_size", 0)
debug = node.setupParameter("debug", True)
is_gt = node.setupParameter("is_gt", False)
output_path = node.setupParameter("output_path", "/tmp/daniele/")
output_name = node.setupParameter("output_name", "")
#⬢⬢⬢⬢⬢➤ Map Rototranslation
roll = node.setupParameter("roll", 0.0)
pitch = node.setupParameter("pitch", 0.0)
yaw = node.setupParameter("yaw", 0.0)
map_transform = PyKDL.Frame()
map_transform.M = PyKDL.Rotation.RPY(roll, pitch, yaw)
#⬢⬢⬢⬢⬢➤ Visualization topic
vis_topic = node.createPublisher('/showmap/map', MarkerArray)
#⬢⬢⬢⬢⬢➤ Read Map file
ff = open(map_file, 'r')
lines = ff.readlines()
voxels = []
skip_counter = -1
map_resolution = 0.01
for l in lines:
skip_counter += 1
if skip_counter == 1:
data = np.fromstring(l, sep=' ')
map_resolution = float(data[2])
print "Set resolution", data, data[2]
#create tensorflow wrapper class
detector = tensorflow_detector_wrapper(args.graph, args.labelMap)
if args.visualization:
classes = detector.getClassDictionary()
c_map = cv_show_detection.getColorMap(classes)
#node creation
node = RosNode("tf_obj_detector")
#⬢⬢⬢⬢⬢➤ Sets HZ from parameters
node.setHz(node.setupParameter("hz", 60))
#create publisher
prediction_topic = node.setupParameter("prediction_topic", "/detections")
publisher = node.createPublisher(prediction_topic, numpy_msg(Floats))
#⬢⬢⬢⬢⬢➤ Creates Camera Proxy
camera_topic = node.setupParameter("camera_topic",
"/camera/rgb/image_raw/compressed")
camera_file = node.getFileInPackage(
'roars', 'data/camera_calibrations/asus_xtion.yml')
camera = CameraRGB(configuration_file=camera_file,
rgb_topic=camera_topic,
compressed_image="compressed" in camera_topic)
#⬢⬢⬢⬢⬢➤ NODE
def image_callback(frame):
#grab image from frame
img = frame.rgb_image.copy()
if args.visualization:
classes = detector.getClassDictionary()
c_map = cv_show_detection.getColorMap(classes)
#node creation
node = RosNode("tf_obj_detector")
#⬢⬢⬢⬢⬢➤ Sets HZ from parameters
node.setHz(node.setupParameter("hz", 60))
#create publisher
prediction_topic = node.setupParameter(
"prediction_topic",
"/detections"
)
publisher = node.createPublisher(prediction_topic,numpy_msg(Floats))
#⬢⬢⬢⬢⬢➤ Creates Camera Proxy
camera_topic = node.setupParameter(
"camera_topic",
"/camera/rgb/image_raw/compressed"
)
camera_file = node.getFileInPackage(
'roars',
'data/camera_calibrations/asus_xtion.yml'
)
camera = CameraRGB(
configuration_file=camera_file,
rgb_topic=camera_topic,
compressed_image="compressed" in camera_topic
