def __init__(self):
self.status_publisher = rospy.Publisher('/pr2_block_builder/status',
Status,
latch=True)
self.block_status_publisher = rospy.Publisher(
'/pr2_block_builder/completion', BlockStatus)
self.picker = Picker()
self.placer = Placer()
rospy.sleep(1)
self.sendStatus("ready")
def __init__(self, constraintSystem):
self.constraintSystem = constraintSystem
self.form = FreeCADGui.PySideUic.loadUi( ':/assembly2/ui/degreesOfFreedomAnimation.ui' )
self.form.setWindowIcon(QtGui.QIcon( ':/assembly2/icons/degreesOfFreedomAnimation.svg' ) )
self.form.groupBox_DOF.setTitle('%i Degrees-of-freedom:' % len(constraintSystem.degreesOfFreedom))
FreeCAD.DOF=constraintSystem.degreesOfFreedom
for i, d in enumerate(constraintSystem.degreesOfFreedom):
item = QtGui.QListWidgetItem('%i. %s' % (i+1, str(d)[1:-1].replace('DegreeOfFreedom ','')), self.form.listWidget_DOF)
if i == 0: item.setSelected(True)
# hack start create animation tools
import Animation
from say import *
import Placer
for i, d in enumerate(constraintSystem.degreesOfFreedom):
sayErr(i)
say(d.__class__.__name__)
say(d.objName)
p=Placer.createPlacer("DOF_" + str(i) +"_")
p.ViewObject.Visibility=False
p.target=FreeCAD.ActiveDocument.getObject(d.objName)
if d.__class__.__name__ == 'LinearMotionDegreeOfFreedom':
p.x='x0+x1*(time-0.5)*50'
p.y='y0+y1*(time-0.5)*50'
p.z='z0+z1*(time-0.5)*50'
b=p.target.Placement.Base
p.x0=b.x
p.y0=b.y
p.z0=b.z
p.x1=d.directionVector[0]
p.y1=d.directionVector[1]
p.z1=d.directionVector[2]
p.RotAxis=p.target.Placement.Rotation.Axis
p.arc=str(p.target.Placement.Rotation.Angle*180/np.pi)
if d.__class__.__name__ == 'AxisRotationDegreeOfFreedom':
p.prePlacement=p.target.Placement
p.arc='360*time'
p.x='0'
p.y='0'
p.z='0'
p.arc0=d.getValue()*180/np.pi
p.RotAxis=(d.axis[0],d.axis[1],d.axis[2])
p.RotCenter=p.target.Placement.Base
# hack end
self.form.pushButton_animateSelected.clicked.connect(self.animateSelected)
self.form.pushButton_animateAll.clicked.connect(self.animateAll)
self.form.pushButton_set_as_default.clicked.connect( self.setDefaults )
self.setIntialValues()
def __init__(self):
self.status_publisher = rospy.Publisher('/pr2_block_builder/status', Status, latch=True)
self.block_status_publisher = rospy.Publisher('/pr2_block_builder/completion', BlockStatus)
self.picker = Picker()
self.placer = Placer()
rospy.sleep(1)
self.sendStatus("ready")
def testAll(self):
metrics_list = {}
for design in designs:
json_file = os.path.join(
os.path.dirname(os.path.abspath(__file__)),
"%s.json" % (design))
params = Params.Params()
params.load(json_file)
# control numpy multithreading
os.environ["OMP_NUM_THREADS"] = "%d" % (params.num_threads)
metrics_list[design] = []
for device_name in ["gpu"] * 5 + ["cpu"] * 2:
for deterministic_name in ["indeterministic"]:
params.gpu = 0 if device_name == "cpu" else 1
params.deterministic_flag = 0 if deterministic_name == "indeterministic" else 1
params.global_place_flag = 1
params.legalize_flag = 1
params.detaield_place_flag = 1
params.detailed_place_engine = ""
logging.info("%s, %s, %s" %
(design, device_name, deterministic_name))
logging.info("parameters = %s" % (params))
# run placement
tt = time.time()
metrics = Placer.place(params)
logging.info("placement takes %.3f seconds" %
(time.time() - tt))
# verify global placement results
metrics_list[design].append((
metrics[-3][-1][-1].hpwl.cpu().numpy(),
metrics[-2].hpwl.cpu().numpy(),
metrics[-1].hpwl.cpu().numpy(),
))
m = np.array(metrics_list[design])
metrics_list[design] = m
gp, lg, dp = m[:, 0], m[:, 1], m[:, 2]
gp_mean, lg_mean, dp_mean = np.mean(gp), np.mean(lg), np.mean(dp)
rtol = lambda x, avg: max(avg - np.min(x), np.max(x) - avg) / avg
gp_rtol, lg_rtol, dp_rtol = rtol(gp,
gp_mean), rtol(lg, lg_mean), rtol(
dp, dp_mean)
logging.info(
f"Avg metrics for {design}\n{m}\nGP={gp_mean:g} ({gp_rtol}), LG={lg_mean:g} ({lg_rtol}), DP={dp_mean:g} ({dp_rtol})"
)
logging.info("Overall Summary")
for design in designs:
m = metrics_list[design]
gp, lg, dp = m[:, 0], m[:, 1], m[:, 2]
gp_mean, lg_mean, dp_mean = np.mean(gp), np.mean(lg), np.mean(dp)
rtol = lambda x, avg: max(avg - np.min(x), np.max(x) - avg) / avg
gp_rtol, lg_rtol, dp_rtol = rtol(gp,
gp_mean), rtol(lg, lg_mean), rtol(
dp, dp_mean)
logging.info(
f"Avg metrics for {design}\n{m}\nGP={gp_mean:g} ({gp_rtol}), LG={lg_mean:g} ({lg_rtol}), DP={dp_mean:g} ({dp_rtol})"
)
def runPlace(self, cktIdx, dirname):
self.p = Placer.Placer(self.mDB, cktIdx, dirname, self.gridStep,
self.halfMetWid)
self.p.run()
self.runtime += self.p.runtime
self.symAxis = self.p.symAxis
self.origin = self.p.origin
self.subShapeList = self.p.subShapeList
self.upscaleBBox(self.gridStep, self.dDB.subCkt(cktIdx), self.origin)
def testPlacer(self):
FreeCAD.Console.PrintLog('Checking Placer...\n')
b = App.activeDocument().addObject("Part::Box", "Box")
r = Placer.createPlacer("BoxPlacer", b)
m = createManager()
m.intervall = 10
m.sleeptime = 0.01
m.addObject(r)
m.Proxy.run()
self.failUnless(
isequal(b.Placement.Rotation.Angle, 0.5654866776461628),
"Rotation error")
self.failUnless(isequal(b.Placement.Base.x, 21.45749434738491),
"Move error")
def testAll(self):
for design in designs:
json_file = os.path.join(
os.path.dirname(os.path.abspath(__file__)),
"%s.json" % (design))
params = Params.Params()
params.load(json_file)
# control numpy multithreading
os.environ["OMP_NUM_THREADS"] = "%d" % (params.num_threads)
for device_name in devices:
for deterministic_name in deterministics:
params.gpu = 0 if device_name == "cpu" else 1
params.deterministic_flag = 0 if deterministic_name == "indeterministic" else 1
params.global_place_flag = 1
params.legalize_flag = 1
params.detaield_place_flag = 1
params.detailed_place_engine = ""
logging.info("%s, %s, %s" %
(design, device_name, deterministic_name))
logging.info("parameters = %s" % (params))
# run placement
tt = time.time()
metrics = Placer.place(params)
logging.info("placement takes %.3f seconds" %
(time.time() - tt))
# verify global placement results
np.testing.assert_allclose(
golden[(design, device_name,
deterministic_name)]["GP"],
metrics[-3][-1][-1].hpwl.cpu().numpy(),
rtol=tolerance[design][0],
)
np.testing.assert_allclose(
golden[(design, device_name,
deterministic_name)]["LG"],
metrics[-2].hpwl.cpu().numpy(),
rtol=tolerance[design][1],
)
np.testing.assert_allclose(
golden[(design, device_name,
deterministic_name)]["DP"],
metrics[-1].hpwl.cpu().numpy(),
rtol=tolerance[design][2],
)
App.setActiveDocument("Unnamed")
App.ActiveDocument=App.getDocument("Unnamed")
Gui.ActiveDocument=Gui.getDocument("Unnamed")
import Animation
Animation.createManager()
App.ActiveDocument.addObject("Part::Box","Box")
App.ActiveDocument.addObject("Part::Box","Box")
App.ActiveDocument.addObject("Part::Box","Box")
App.ActiveDocument.addObject("Part::Box","Box")
App.ActiveDocument.addObject("Part::Cone","Cone")
import Placer
s1=Placer.createPlacer("B1")
s1.target=App.ActiveDocument.Box001
s2=Placer.createPlacer("B2")
s2.target=App.ActiveDocument.Box002
s2.y="10"
s3=Placer.createPlacer("B3")
s3.target=App.ActiveDocument.Box003
s3.y="20"
import Diagram
c=Diagram.createDiagram("dia","0.200*time","0.2*(0.01*time-0.5)**2","10+time+1","-10*time")
c.source=s1
c.trafo="source.Placement.Rotation.Angle"
def setupContextMenu(self, obj, menu):
return
if __name__ == '__main__':
App.ActiveDocument.addObject("Part::Box", "Box")
App.ActiveDocument.addObject("Part::Box", "Box")
App.ActiveDocument.addObject("Part::Box", "Box")
App.ActiveDocument.addObject("Part::Box", "Box")
App.ActiveDocument.addObject("Part::Cone", "Cone")
import Placer
s1 = Placer.createPlacer("B1")
s1.target = App.ActiveDocument.Box001
s2 = Placer.createPlacer("B2")
s2.target = App.ActiveDocument.Box002
s2.y = "10"
s3 = Placer.createPlacer("B3")
s3.target = App.ActiveDocument.Box003
s3.y = "20"
c = createCombiner("cmb")
c.source = App.ActiveDocument.Cone
c.trafo = "source.Radius1.Value"
c.target = s1
V.ViewObject.PointSize = 10
V.ViewObject.PointColor = (1.0, .0, .0)
# slaves
s = App.ActiveDocument.addObject("Part::Sphere", "Sphere")
b = App.ActiveDocument.addObject("Part::Box", "Box")
t = App.ActiveDocument.addObject("Part::Torus", "Torus")
s.ViewObject.ShapeColor = (1.0, 1.0, .0)
b.ViewObject.ShapeColor = (.0, 1.0, 1.0)
t.ViewObject.ShapeColor = (1.0, .0, 1.0)
# ACTOR #
# move the sphere relative (10,-5,0) to the control point
p = Placer.createPlacer("Sphere Mover", s)
p.src = V
p.x = "sx+10"
p.y = "sy-5"
# rotate the cube with rotation arc 20*sx along the default z-axis
p2 = Placer.createPlacer("Box Rotator", b)
p2.src = V
p2.x = "-50"
p2.y = "-5"
p2.arc = "20*sx"
# rotate the donat with rotation arc 10*sy along the x-axis
p3 = Placer.createPlacer("Torus Rotator", t)
p3.src = V
p3.x = "50"
# testcase manager and placer
def isequal(a,b):
return abs(a-b)<1e-6
import Animation
from Animation import *
reload(Animation)
import Placer
reload(Placer)
d=App.newDocument("Unbenannt")
b=App.activeDocument().addObject("Part::Box","Box")
r=Placer.createPlacer("BoxPlacer",b)
m=createManager()
m.intervall = 10
m.sleeptime = 0.01
m.addObject(r)
m.Proxy.run()
assert(isequal(b.Placement.Rotation.Angle,0.5654866776461628))
assert(isequal(b.Placement.Base.x,21.45749434738491))
#App.closeDocument("Unbenannt")
import Placer
reload(Placer)
import Tracker
reload(Tracker)
import Trackreader
reload(Trackreader)
d = App.newDocument("Unbenannt")
b = App.activeDocument().addObject("Part::Box", "Box")
r = Placer.createPlacer("BoxPlacer", b)
t = Tracker.createTracker("BoxTracker", r, "/tmp/tracker")
m = Animation.createManager()
m.intervall = 100
m.sleeptime = 0.01
m.addObject(r)
m.addObject(t)
m.Proxy.run()
path = t.ViewObject.Proxy.showpath()
import Trackreader
FreeCAD.newDocument("Unbenannt")
box = FreeCAD.activeDocument().addObject("Part::Box", "Static")
box.Height = 40
box.Length = 100
box2 = FreeCAD.activeDocument().addObject("Part::Box", "Animated")
box2.Placement.Base = FreeCAD.Vector(.0, .0, 0.)
toucher = Toucher.createToucher("Force the Common", box)
#
# Example: track the vertexes of a changing fusion
#
placer = Placer.createPlacer("Box Placer", box2)
placer.x = "100*time-10"
placer.y = " -5 if time< 0.5 else 7"
placer.z = "5+30*(0.5-time)**2"
placer.arc = "0"
placer.time = 0
manager = Animation.createManager()
manager.intervall = 100
manager.sleeptime = 0.0
fuse = App.activeDocument().addObject("Part::MultiFuse", "Fusion")
fuse.Shapes = [box, box2]
fuse.ViewObject.ShapeColor = (1.0, 1.0, .5)
fuse.ViewObject.Transparency = 70
class Builder():
ready = True
picker = None
placer = None
status_publisher = None
block_status_publisher = None
def __init__(self):
self.status_publisher = rospy.Publisher('/pr2_block_builder/status',
Status,
latch=True)
self.block_status_publisher = rospy.Publisher(
'/pr2_block_builder/completion', BlockStatus)
self.picker = Picker()
self.placer = Placer()
rospy.sleep(1)
self.sendStatus("ready")
def start(self, blocks):
if (self.ready):
# Prevent further invocations
self.ready = False
# Send the start status
self.sendStatus("started")
# Sort the blocks
self.sortBlocks(blocks, 0)
# Do construction
status = self.construct(blocks)
# Send the completion status
self.sendStatus(status)
def stop(self):
self.ready = False
if self.picker != None:
self.picker.cancel()
if self.placer != None:
self.placer.cancel()
def restart(self):
self.stop()
self.ready = True
self.sendStatus("ready")
def construct(self, blocks):
for i in range(0, len(blocks)):
block = blocks[i]
# Send the status
self.sendBlockStatus(block, "started")
# Pick
status = self.picker.pick(i)
# Place
if status == "completed":
status = self.placer.place(block)
# Send the status
self.sendBlockStatus(block, status)
# Stop if we've failed or aborted
if status != "completed":
return status
return "completed"
def sendStatus(self, status, message=""):
rospy.loginfo("Status: %s", status)
statusObject = Status()
statusObject.status.data = status
statusObject.message.data = message
self.status_publisher.publish(statusObject)
def sendBlockStatus(self, block, status, message=""):
rospy.loginfo("Block (%s, %s, %s) %s", block.position.x,
block.position.y, block.position.z, status)
statusObject = BlockStatus()
statusObject.block = block
statusObject.status.data = status
statusObject.message.data = message
self.block_status_publisher.publish(statusObject)
def sortBlocks(self, blocks, start):
nextBlock = blocks[start]
nextBlockIndex = start
for i in range(start, len(blocks)):
if (blocks[i].position.z < nextBlock.position.z):
nextBlock = blocks[i]
nextBlockIndex = i
elif (blocks[i].position.z == nextBlock.position.z
and blocks[i].position.x > nextBlock.position.x):
nextBlock = blocks[i]
nextBlockIndex = i
elif (blocks[i].position.z == nextBlock.position.z
and blocks[i].position.x == nextBlock.position.x
and blocks[i].position.y > nextBlock.position.y):
nextBlock = blocks[i]
nextBlockIndex = i
blocks[nextBlockIndex] = blocks[start]
blocks[start] = nextBlock
if (start == len(blocks) - 1):
return blocks
return self.sortBlocks(blocks, start + 1)
def isequal(a, b):
return abs(a - b) < 1e-6
import FreeCAD
import Animation
import Placer
FreeCAD.newDocument("Unbenannt")
# zwei zu animierende Objekte erzeugen und
# ihrer Plazierer festlegen
b = FreeCAD.activeDocument().addObject("Part::Torus", "Torus")
r = Placer.createPlacer("Torus Placer", b)
r.x = '100'
r.RotAxis = FreeCAD.Vector(1.0, 0, 0)
b2 = FreeCAD.activeDocument().addObject("Part::Box", "Box")
k = Placer.createPlacer("Box Placer", b2)
m = Animation.createManager()
m.intervall = 100
m.sleeptime = 0.01
# die beiden Plazierer durch den Manager verwalten lassen
m.addObject(r)
m.addObject(k)
# den Manager starten
V.ViewObject.PointSize=10
V.ViewObject.PointColor=(1.0,.0,.0)
# slaves
s=App.ActiveDocument.addObject("Part::Sphere","Sphere")
b=App.ActiveDocument.addObject("Part::Box","Box")
t=App.ActiveDocument.addObject("Part::Torus","Torus")
s.ViewObject.ShapeColor=(1.0,1.0,.0)
b.ViewObject.ShapeColor=(.0,1.0,1.0)
t.ViewObject.ShapeColor=(1.0,.0,1.0)
# ACTOR #
# move the sphere relative (10,-5,0) to the control point
p=Placer.createPlacer("Sphere Mover",s)
p.src=V
p.x="sx+10"
p.y="sy-5"
# rotate the cube with rotation arc 20*sx along the default z-axis
p2=Placer.createPlacer("Box Rotator",b)
p2.src=V
p2.x="-50"
p2.y="-5"
p2.arc="20*sx"
# rotate the donat with rotation arc 10*sy along the x-axis
p3=Placer.createPlacer("Torus Rotator",t)
p3.src=V
p3.x="50"
points=[FreeCAD.Vector(2*x,-200,-0.1),FreeCAD.Vector(2*x,200,-0.1)]
w=Draft.makeWire(points)
w.ViewObject.LineWidth=1.0
w.ViewObject.LineColor=(.0,1.0,.0)
for y in range(101):
points=[FreeCAD.Vector(0,2*y,-0.1),FreeCAD.Vector(200,2*y,-0.1)]
w=Draft.makeWire(points)
w.ViewObject.LineWidth=1.0
w.ViewObject.LineColor=(1.0,.0,.0)
b=App.activeDocument().addObject("Part::Box","Box")
b.ViewObject.ShapeColor=(1.0,.0,.0)
r=Placer.createPlacer("Placer",b)
r.arc="0"
s=Speeder.createSpeeder("Speeder Ping Pong")
s.mode='ping pong'
s.m=50
s.target=r
s=Speeder.createSpeeder("Speeder Fade")
s.mode='fade'
s.b=0.3
s.c=0.8
s.m=50
s.g=0.5
w = Draft.makeWire(points)
w.ViewObject.LineWidth = 1.0
w.ViewObject.LineColor = (.0, 1.0, .0)
for y in range(101):
points = [
FreeCAD.Vector(0, 2 * y, -0.1),
FreeCAD.Vector(200, 2 * y, -0.1)
]
w = Draft.makeWire(points)
w.ViewObject.LineWidth = 1.0
w.ViewObject.LineColor = (1.0, .0, .0)
b = App.activeDocument().addObject("Part::Box", "Box")
b.ViewObject.ShapeColor = (1.0, .0, .0)
r = Placer.createPlacer("Placer", b)
r.arc = "0"
s = Speeder.createSpeeder("Speeder Ping Pong")
s.mode = 'ping pong'
s.m = 50
s.target = r
s = Speeder.createSpeeder("Speeder Fade")
s.mode = 'fade'
s.b = 0.3
s.c = 0.8
s.m = 50
s.g = 0.5
s.target = r
def pp(x, y, z, num):
global picker
global p
pose = Pose()
pose.position.x = x
pose.position.y = y
pose.position.z = z
picker.pick(num)
rospy.loginfo(p.place(pose))
if __name__ == '__main__':
rospy.init_node('placer_test')
global picker
global p
picker = Picker()
p = Placer()
pp(0, 0, 0, 0)
pp(0, 1, 0, 1)
pp(0, 2, 0, 2)
pp(1, 2, 0, 3)
pp(2, 2, 0, 4)
pp(0, 1, 0, 5)
pp(0, 2, 0, 6)
pp(0, 0, 1, 7)
# pather testcase
import Animation,Draft, Pather, Placer
box=App.ActiveDocument.addObject("Part::Box","Box")
points=[FreeCAD.Vector(22.0,6.0,0.0),
FreeCAD.Vector(8.,60.5,0.0),
FreeCAD.Vector(-20,-27.3,0.0),
FreeCAD.Vector(16.32,-41.3,0.0)]
bspline=Draft.makeBSpline(points)
pa=Pather.createPather('BSpline as Path')
pa.src=bspline
pl=Placer.createPlacer('Placer for Box',box)
pl.x='sx-5'
pl.y='sy-5'
pl.arc='0'
pl.src=pa
m=Animation.createManager()
m.addObject(pa)
m.addObject(pl)
m.Proxy.run()
reload(Placer)
import Speeder
reload(Speeder)
# d=App.newDocument("Unbenannt")
r = Draft.makeRectangle(length=200,
height=100.,
placement=pl,
face=True,
support=None)
r.Placement.Base.z = -0.01
b = App.activeDocument().addObject("Part::Box", "PingPong Box")
b.ViewObject.ShapeColor = (1.0, .0, .0)
r = Placer.createPlacer("BoxPlacer ping pong", b)
r.arc = "0"
s = Speeder.createSpeeder("Speeder Ping Pong")
s.mode = 'ping pong'
s.target = r
b1 = App.activeDocument().addObject("Part::Box", "Reverse moving Box")
b1.ViewObject.ShapeColor = (1.0, 1.0, .0)
r1 = Placer.createPlacer("BoxPlacer reverse", b1)
r1.arc = "0"
r1.y = "10"
s1 = Speeder.createSpeeder("Speeder backward")
s1.mode = 'backward'
s1.target = r1
import Placer
reload(Placer)
import Speeder
reload(Speeder)
# d=App.newDocument("Unbenannt")
r=Draft.makeRectangle(length=200,height=100.,placement=pl,face=True,support=None)
r.Placement.Base.z=-0.01
b=App.activeDocument().addObject("Part::Box","PingPong Box")
b.ViewObject.ShapeColor=(1.0,.0,.0)
r=Placer.createPlacer("BoxPlacer ping pong",b)
r.arc="0"
s=Speeder.createSpeeder("Speeder Ping Pong")
s.mode='ping pong'
s.target=r
b1=App.activeDocument().addObject("Part::Box","Reverse moving Box")
b1.ViewObject.ShapeColor=(1.0,1.0,.0)
r1=Placer.createPlacer("BoxPlacer reverse",b1)
r1.arc="0"
r1.y="10"
s1=Speeder.createSpeeder("Speeder backward")
s1.mode='backward'
s1.target=r1
FreeCAD.newDocument("Unbenannt")
box=FreeCAD.activeDocument().addObject("Part::Box","Static")
box.Height=40
box.Length=100
box2=FreeCAD.activeDocument().addObject("Part::Box","Animated")
box2.Placement.Base=FreeCAD.Vector(.0,.0,0.)
toucher=Toucher.createToucher("Force the Common",box)
#
# Example: track the vertexes of a changing fusion
#
placer=Placer.createPlacer("Box Placer",box2)
placer.x="100*time-10"
placer.y=" -5 if time< 0.5 else 7"
placer.z="5+30*(0.5-time)**2"
placer.arc="0"
placer.time=0
manager=Animation.createManager()
manager.intervall = 100
manager.sleeptime = 0.0
fuse=App.activeDocument().addObject("Part::MultiFuse","Fusion")
fuse.Shapes = [box,box2]
fuse.ViewObject.ShapeColor=(1.0,1.0,.5)
fuse.ViewObject.Transparency=70
class Builder():
ready = True
picker = None
placer = None
status_publisher = None
block_status_publisher = None
def __init__(self):
self.status_publisher = rospy.Publisher('/pr2_block_builder/status', Status, latch=True)
self.block_status_publisher = rospy.Publisher('/pr2_block_builder/completion', BlockStatus)
self.picker = Picker()
self.placer = Placer()
rospy.sleep(1)
self.sendStatus("ready")
def start(self, blocks):
if (self.ready):
# Prevent further invocations
self.ready = False
# Send the start status
self.sendStatus("started")
# Sort the blocks
self.sortBlocks(blocks, 0)
# Do construction
status = self.construct(blocks)
# Send the completion status
self.sendStatus(status)
def stop(self):
self.ready = False
if self.picker!=None:
self.picker.cancel()
if self.placer!=None:
self.placer.cancel()
def restart(self):
self.stop()
self.ready = True
self.sendStatus("ready")
def construct(self, blocks):
for i in range(0, len(blocks)):
block = blocks[i]
# Send the status
self.sendBlockStatus(block, "started")
# Pick
status = self.picker.pick(i)
# Place
if status=="completed":
status = self.placer.place(block, false)
# Send the status
self.sendBlockStatus(block, status);
# Stop if we've failed or aborted
if status!="completed":
return status
return "completed"
def sendStatus(self, status, message=""):
rospy.loginfo("Status: %s", status)
statusObject = Status()
statusObject.status.data = status
statusObject.message.data = message
self.status_publisher.publish(statusObject)
def sendBlockStatus(self, block, status, message=""):
rospy.loginfo("Block (%s, %s, %s) %s", block.position.x, block.position.y, block.position.z, status)
statusObject = BlockStatus()
statusObject.block = block
statusObject.status.data = status
statusObject.message.data = message
self.block_status_publisher.publish(statusObject)
def sortBlocks(self, blocks, start):
dist_block = blocks[i].position.x * blocks[i].position.x + blocks[i].position.y * blocks[i].position.y
nextBlock = blocks[start]
nextBlockIndex = start
dist_nextBlock = dist_block
for i in range(start, len(blocks)):
dist_block = blocks[i].position.x * blocks[i].position.x + blocks[i].position.y * blocks[i].position.y
if (blocks[i].position.z < nextBlock.position.z):
nextBlock = blocks[i]
nextBlockIndex = i
dist_nextBlock = dist_block
elif (blocks[i].position.z == nextBlock.position.z and dist_block < dist_nextBlock):
nextBlock = blocks[i]
nextBlockIndex = i
dist_nextBlock = dist_block
elif (blocks[i].position.z == nextBlock.position.z and dist_block == dist_nextBlock and blocks[i].position.x < nextBlock.position.x):
nextBlock = blocks[i]
nextBlockIndex = i
dist_nextBlock = dist_block
blocks[nextBlockIndex] = blocks[start]
blocks[start] = nextBlock
if (start == len(blocks)-1):
return blocks
return self.sortBlocks(blocks, start + 1)