def startRelativeScan(xpos, ypos):
global bigsensorvalue
limitx = -2.0
limitX = 2.0
limity = -1.57
limitY = 1.57
with pymorse.Morse() as simu:
simu.robot.arm.probeviz.subscribe(probedata)
speed = 0.1
for x in range(10):
if bigsensorvalue:
bigsensorvalue = False
jump_to_new_scan()
break
if (xpos+x*0.5 > limitX):
break
if (xpos+x*0.5 < limitx):
break
simu.robot.arm.rotate("kuka_2", xpos + (x*0.5), 2 )
# simu.sleep(1)
for y in range(10):
if bigsensorvalue:
jump_to_new_scan()
break
if (ypos+y*0.5 > limitY):
break
if (ypos+y*0.5 < limity):
break
simu.robot.arm.rotate("kuka_2", xpos+ (x*0.1), speed)
simu.robot.arm.rotate("kuka_1", ypos+ (y*0.1), speed)
# simu.sleep(0.3)
jump_to_new_scan()
def main():
print("Test")
with pymorse.Morse() as simu, Morse() as morse:
""" Main behaviour """
mousePose = morse.mouse.mousePose
motion = morse.mouse.motion
destination = set_pos(simu, morse)
start = time.time()
bat = morse.mouse.mouse_battery
waypoint = destination
motion.publish(waypoint)
first_attack = True
while motion.get_status()!= 'Arrived':
print_res(bat, start)
mousePosition = where_is(mousePose)
if(check_speed(simu, morse) == 1 and check_hide(simu, morse) != 1 and first_attack == True):
print("running")
destination = {'x' : -6.363116264343262, 'y': 45.8295783996582, 'z': 0.0,
'tolerance' : 0.5,'speed': 4.0}
elif(check_hide(simu, morse) == 1 and first_attack == True):
print("hiding")
motion.publish(go_where(mousePosition))
first_attack = False
for i in range(90):
time.sleep(1)
print_suc_res(bat, start, "HIDING")
elif(check_speed(simu, morse) == 1 and first_attack == False):
print("Running 2")
# motion.publish({'x' : -6.363116264343262, 'y': 45.8295783996582, 'z': 0.0,
# 'tolerance' : 0.5,
# 'speed' : 4.0})
motion.publish(destination)
print("Here we are!")
print_suc_res(bat, start, "SUCCESS")
def list_scan_limits():
## to better map the values of the scan in a 2D space it's usefull to get the bounds, as detected by this function
with pymorse.Morse() as simu:
speed = 2
# simu.robot.arm.probeviz.subscribe(probedatalimits)
simu.robot.arm.rotate("kuka_1", -2.0670610427856445, speed)
simu.robot.arm.set_rotation("kuka_2", -1.5943961143493652)#, speed)
print(simu.robot.arm.probeviz.get_local_data())
simu.sleep(5)
print(simu.robot.arm.probeviz.get_local_data())
simu.robot.arm.rotate("kuka_1", -2.0670610427856445,speed)
simu.robot.arm.rotate("kuka_2", 1.5943961143493652,speed)
print(simu.robot.arm.probeviz.get_local_data())
simu.sleep(5)
print(simu.robot.arm.probeviz.get_local_data())
simu.robot.arm.rotate("kuka_1", 2.0670610427856445,speed)
simu.robot.arm.rotate("kuka_2", -1.5943961143493652,speed)
print(simu.robot.arm.probeviz.get_local_data())
simu.sleep(5)
print(simu.robot.arm.probeviz.get_local_data())
simu.robot.arm.rotate("kuka_1", 2.0670610427856445,speed)
simu.robot.arm.rotate("kuka_2", 1.3943961143493652,speed)
print(simu.robot.arm.probeviz.get_local_data())
simu.sleep(5)
print(simu.robot.arm.probeviz.get_local_data())
def handleMoveGripper(req):
'''
This service moves the gripper (changing their location in 3D simu env) by calling a morse service (set_object_position)
Parameters:
req: double x, double y, double z
Returns:
boolean success
'''
global mutex1
#rospy.loginfo(req.gripper_id)
while mutex1 == 1:
sleep(0.01)
mutex1 = 1
success = True
try:
with pymorse.Morse() as sim:
sim.rpc('simulation', 'set_object_position', req.gripper_id, [req.x, req.y, req.z])
except:
success = False
if not success:
rospy.loginfo("[gripper manipulator]: MoveGripper {} success: {}!".format(req.gripper_id, success))
mutex1 = 0
return MoveGripperResponse(success)
def scanArea():
global bigsensorvalue
ax1 = random.random()
ax2 = random.random()
# limitax1 = -2.0
# limitAX1 = 2.0
# limitax2 = -1.57
# limitAX2 = 1.57
# axe1 = (abs(limitax1) - abs(limitAX1))*ax1 + limitax1
# axe2 = (abs(limitax2) - abs(limitAX2))*ax2 + limitax2
axe1 = ax1 * 2
axe2 = ax2 * 1
print(axe1,axe2)
with pymorse.Morse() as simu:
simu.robot.arm.probeviz.subscribe(probedata)
# goto start axis 1&2
simu.robot.arm.set_rotation("kuka_1",axe1)
simu.sleep(1)
simu.robot.arm.set_rotation("kuka_2",axe2)
# start scan
for i in range(10):
print(simu.robot.arm.probeviz.get_local_data())
if bigsensorvalue:
bigsensorvalue = False
break
for j in range(10):
if bigsensorvalue:
bigsensorvalue = False
break
simu.robot.arm.set_rotation("kuka_1",axe1 + i*0.03)
simu.sleep(0.01)
simu.robot.arm.set_rotation("kuka_2",axe2 + j*0.03)
simu.sleep(0.01)
scanArea()
def create_publishers(rpc_type, rpc_commands):
rpc_publishers = []
for rpc in rpc_commands:
pub = rospy.Publisher("/morse_internals/" + rpc[0],
String,
queue_size=1)
pub.morseCall = functools.partial(pymorse.Morse().rpc, rpc_type, *rpc)
rpc_publishers.append(pub)
return (rpc_publishers)
def jump_to_new_scan():
xpos = (random.random()*2) - 1
ypos = (random.random()*2) - 1
with pymorse.Morse() as simu:
simu.robot.arm.set_rotation("kuka_2", xpos) ##, 2 ) ### limits [-2.9670610427856445, 2.9670610427856445]
simu.robot.arm.set_rotation("kuka_1", ypos )##, 2) ### limits [-2.0943961143493652, 2.0943961143493652]
simu.sleep(1)
print(xpos,ypos)
startRelativeScan(xpos,ypos)
def sendmessage(self, message):
pm = PlayMorse(self.conf.WPM, self.conf)
trans = pymorse.Morse()
for char in message.split('~'):
display = trans.morsedecode(str(char))
pm.playchar(char)
self.displaychar(display)
if self.conf.SpeedAdjust is True:
if self.checkadjustspeed():
break # If speed adjustment change pop out and restart
def go_to():
with pymorse.Morse() as simu, Morse() as morse:
# subscribes to updates from the Pose sensor by passing a callback
nearObj = morse.mouse.proximity
mousePose = morse.mouse.mousePose
# sends a destination
simu.mouse.motion.publish({
'x': -6.363116264343262,
'y': 45.8295783996582,
'z': 0.0,
'tolerance': 0.5,
'speed': 1.0
})
# Leave a couple of millisec to the simulator to start the action
simu.sleep(0.1)
curr = 0
while simu.mouse.motion.get_status() != "Arrived":
# waits until we reach the target
mousePosition = where_is(mousePose)
print_pos(mousePosition)
prev = curr
curr = near_robot(nearObj)
#bat_lev = battery_life(bat)
##################tests##################################
#print("Battery: %s" % bat_lev)
#print(prev)
#print(curr)
#################tests###################################
if (prev - curr > 10 and curr < 20 and prev is not 0
and curr is not 0):
######tests######
print("Too fast")
######tests######
simu.sleep(0.5)
go_three()
elif (curr and 1 < curr < 10):
######tests######
print("Too close")
######tests######
simu.sleep(0.5)
go_three()
else:
simu.sleep(0.5)
print("Here we are!")
def run(self):
# Program starts thread to monitor button presses to adjust speed
# seems that threading ans i2c don't mix... maybe add speed adjust to character write loop
# Load messages
self.welcome()
wol = WorkOrder(self._wopath)
# Wait for someone to move/come in/what ever
while True:
det = rpimorsedrv.Detector(self.conf)
self.lcd.set_backlight(0)
self.lcd.clear()
print(self.conf.WPM)
det.detect()
# Start Display
self.lcd.set_backlight(1)
self.lcd.set_color(self.conf.Color[0], self.conf.Color[1],
self.conf.Color[2])
# Select message to play
msgnum = self.selectmessage(wol)
self.getmessage(wol, msgnum)
# Encode message to code
trans = pymorse.Morse()
tonum_morse = trans.morseencode('ORDER NUMBER:' + wol.TONum)
locissued_morse = trans.morseencode('LOCATION:' + wol.LocIssued)
date_morse = trans.morseencode("DATE:" + wol.Date)
to_morse = trans.morseencode('TO:' + wol.To)
at_morse = trans.morseencode('AT:' + wol.At)
text_morse = trans.morseencode('WO:' + wol.Text)
status_morse = trans.morseencode('STATUS:' + wol.Status)
time_morse = trans.morseencode('TIME:' + wol.Time)
dispatcher_morse = trans.morseencode('DISPATCHER:' +
wol.Dispatcher)
operator_morse = trans.morseencode('OP:' + wol.Operator)
paragraph_sep_morse = trans.morseencode(self._paragraph_sep)
completemessage = self.assemblemessage(
paragraph_sep_morse, tonum_morse, locissued_morse, date_morse,
to_morse, at_morse, text_morse, status_morse, time_morse,
dispatcher_morse, operator_morse)
# Display message and Play message one character at a time until message done
self.sendmessage(completemessage)
# Shutoff Display
# time.sleep(5)
self.lcd.set_backlight(0)
def update(self, time: 'Optional[float]' = None):
"""Update wildfire texture at current time in morse"""
if self.fire_map is None:
raise ValueError("The wildfire ignition time map is not set")
if time is None:
time = datetime.now().timestamp()
self._update_fire_image(time)
with tempfile.NamedTemporaryFile(suffix=".png", delete=True) as t_file:
skimage.io.imsave(t_file, np.transpose(self.fire_image, (1,0,2))[::-1,::1,...])
if self.morse_conn is None or not self.morse_conn.is_up():
self.morse_conn = pymorse.Morse(*self.address)
self.morse_conn.rpc_t(self._morse_timeout, "simulation", "set_texture",
str(self.terrain_object), str(t_file.name))
def go_three():
#Fleeing reaction called when p button hit
with pymorse.Morse() as simu, Morse() as morse:
# subscribes to updates from the Pose sensor by passing a callback
mousePose = morse.mouse.mousePose
nearObj = morse.mouse.proximity
bat = morse.mouse.mouse_battery
mousePosition = where_is(mousePose)
destination = go_where(mousePosition)
simu.mouse.motion.publish(destination)
simu.sleep(20)
# sends a destination
simu.mouse.motion.publish({
'x': -6.363116264343262,
'y': 45.8295783996582,
'z': 0.0,
'tolerance': 0.5,
'speed': 4.0
})
# Leave a couple of millisec to the simulator to start the action
simu.sleep(0.1)
# waits until we reach the target
while simu.mouse.motion.get_status() != "Arrived":
curr = near_robot(nearObj)
if (curr and curr < 1):
print("CAPTURE")
simu.sleep(0.5)
print("Here we are!")
go_to()
def frighten_mouse():
""" Use the mouse pose sensor to locate and "chase" it """
with pymorse.Morse() as simu, Morse() as morse:
catPose = morse.cat.catPose
mousePose = morse.mouse.mousePose
motion = simu.cat.waypoint
bat = morse.cat.cat_battery
start = time.time()
bat_lev = battery_life(bat)
is_free = True
cnt = 0
while (bat_lev > 50 and is_free == True):
bat_lev = battery_life(bat)
catPosition = where_is(catPose)
mousePosition = where_is(mousePose)
# go behind the mouse
if (is_hiding(mousePosition['y'], mousePosition['x']) == False):
if (how_far(catPosition['y'], mousePosition['y'],
catPosition['x'], mousePosition['x']) == True):
is_free = False
print("CAPTURE")
print_res(bat_lev, start)
else:
pursue_mouse(mousePosition['y'], mousePosition['x'],
mousePosition['z'], mousePosition['yaw'],
motion)
else:
is_free = True
# send the command through the socket
catPosition = where_is(catPose)
motion.publish({
"x": 51.0,
"y": -59,
"z": height,
"yaw": catPosition['yaw'],
"tolerance": 0.5,
"speed": 5
})
while (mousePosition['x'] != -6.363116264343262
or mousePosition['y'] != 45.8295783996582):
time.sleep(1)
print_res(bat_lev, start)
def startMorsePublisher(nodeName="morse_internals", pubFrequency=1):
"""
First inits a node withe the specified name, then creates publishers and
starts publishing the information.
Careful: Publishers interface with MORSE via sockets and then publish the
information under the respective topic. The frequency directly impacts the
number of mesages sent via sockets and might have an impact on performance.
@param nodeName: name of node
@type nodeName str
@param pubFrequency: publication frequency in hz
@type pubFrequency int
"""
with pymorse.Morse(
) as morseSim: #To make sure that the sockets are closed after ros is shutdown
rospy.init_node(nodeName)
read_parameters()
if _frequency is not None:
pubFrequency = _frequency
allMorsePublishers = init_morse_publishers(morseSim)
publishMessages(allMorsePublishers, pubFrequency)
def go_to():
with pymorse.Morse() as simu, Morse() as morse:
# subscribes to updates from the Pose sensor by passing a callback
nearObj = morse.mouse.proximity
mousePose = morse.mouse.mousePose
#mouseColl = morse.mouse.collision
#mouseOrient = morse.mouse.orientation
# sends a destination
simu.mouse.motion.publish({'x' : -6.363116264343262, 'y': 45.8295783996582, 'z': 0.0,
'tolerance' : 0.5,
'speed' : 1.0})
# Leave a couple of millisec to the simulator to start the action
simu.sleep(0.1)
curr = 0
while simu.mouse.motion.get_status() != "Arrived":
# waits until we reach the target
mousePosition = where_is(mousePose)
#if(is_collision(mouseColl) != 0):
#mouseOrient.set_property(yaw, 1)
# print("1")
prev = curr
curr = near_robot(nearObj)
if (curr and 1 < curr < 30):
######tests######
print("Too close")
######tests######
simu.sleep(0.5)
go_three()
else:
simu.sleep(0.5)
print("Here we are!")
def frighten_mouse():
""" Use the mouse pose sensor to locate and "chase" it """
with pymorse.Morse() as simu, Morse() as morse:
catPose = morse.cat.catPose
mousePose = morse.mouse.mousePose
motion = morse.cat.waypoint
bat = morse.cat.cat_battery
bat_lev = battery_life(bat)
is_free = True
while (bat_lev > 50 and is_free == True):
catPosition = where_is(catPose)
mousePosition = where_is(mousePose)
if mousePosition and catPosition:
# go behind the mouse
waypoint = { "x": mousePosition['x'] - minDist*math.cos(mousePosition['yaw']), \
"y": mousePosition['y'] - minDist*math.sin(mousePosition['yaw']), \
"z": height, \
"yaw": catPosition['yaw'], \
"tolerance": 0.5 \
}
# look at the mouse
if((catPosition['y'] - mousePosition['y']) < 1 and (catPosition['x'] - mousePosition['x']) < 1 and is_hiding(mousePosition) = False):
is_free = False
print("CAPTURE")
#print("%s" % is_free)
elif mousePosition['x']==catPosition['x']:
waypoint['yaw']= math.sign(mousePosition['y']-catPosition['y']) * math.pi
else:
waypoint['yaw']= math.atan2(mousePosition['y']-catPosition['y'],mousePosition['x']-catPosition['x'])
# send the command through the socket
motion.publish(waypoint)
cnt = 0
while True:
cnt+1
def moveTo4corners():
cx = 0
cy = 0
cz = 0.6
x1 = 0.4
y1 = -0.02
z1 = 0.2
x2 = x1
y2 = -0.02
z2 = -0.2
x3 = x1
y3 = 0.02
z3 = -0.2
x4 = x1
y4 = 0.02
z4 = 0.2
with pymorse.Morse() as simu:
# simu.robot.arm.probeviz.subscribe(probedata)
# move_IK_target(name, translation, rotation, relative, linear_speed, radial_speed) (non blocking)
simu.robot.arm.move_IK_target('ik_target.robot.arm.kuka_7', [cx+x1,cy+y1,cz+z1], [0.0,0.0,0.0], False, 1,1 )
# simu.robot.arm.place_IK_target('ik_target.robot.arm.kuka_7', [cx+x1,cy+y1,cz+z1], [0.0,0.0,0.0], False)
print(simu.robot.arm.probeviz.get_local_data())
# simu.robot.arm.place_IK_target('ik_target.robot.arm.kuka_7', [0.02,0.05*x,0], [0.0,0.0,0.0], True)
simu.sleep(3)
simu.robot.arm.move_IK_target('ik_target.robot.arm.kuka_7', [cx+x2,cy+y2,cz+z2], [0.0,0.0,0.0], False, 1,1 )
# simu.robot.arm.place_IK_target('ik_target.robot.arm.kuka_7', [cx+x2,cy+y2,cz+z2], [0.0,0.0,0.0], False )
print(simu.robot.arm.probeviz.get_local_data())
simu.sleep(3)
simu.robot.arm.move_IK_target('ik_target.robot.arm.kuka_7', [cx+x3,cy+y3,cz+z3], [0.0,0.0,0.0], False, 1,1 )
# simu.robot.arm.place_IK_target('ik_target.robot.arm.kuka_7', [cx+x3,cy+y3,cz+z3], [0.0,0.0,0.0], False )
print(simu.robot.arm.probeviz.get_local_data())
simu.sleep(3)
simu.robot.arm.move_IK_target('ik_target.robot.arm.kuka_7', [cx+x4,cy+y4,cz+z4], [0.0,0.0,0.0], False, 1,1 )
# simu.robot.arm.place_IK_target('ik_target.robot.arm.kuka_7', [cx+x4,cy+y4,cz+z4], [0.0,0.0,0.0], False)
print(simu.robot.arm.probeviz.get_local_data())
def main():
print("Test")
with pymorse.Morse() as simu, Morse() as morse:
""" Main behaviour """
mousePose = morse.mouse.mousePose
motion = morse.mouse.motion
nearObj = morse.mouse.proximity
destination = set_pos(simu, morse)
start = time.time()
bat = morse.mouse.mouse_battery
waypoint = destination
motion.publish(waypoint)
first_attack = True
while motion.get_status() != 'Arrived':
print_res(bat, start)
mousePosition = where_is(mousePose)
while (near_robot(nearObj) == 1):
motion.publish(go_where(mousePosition))
time.sleep(1)
print_res(bat, start)
motion.publish(destination)
print("Here we are!")
print_suc_res(bat, start)
def scanArea():
ax1 = random.random()
ax2 = random.random()
limitax1 = -2.0
limitAX1 = 2.0
limitax2 = -1.57
limitAX2 = 1.57
# axe1 = (abs(limitax1) - abs(limitAX1))*ax1 + limitax1
# axe2 = (abs(limitax2) - abs(limitAX2))*ax2 + limitax2
axe1 = ax1 * 2
axe2 = ax2 * 1
print(axe1, axe2)
with pymorse.Morse() as simu:
# goto start axis 1&2
simu.robot.arm.set_rotation("kuka_1", axe1)
simu.sleep(1)
simu.robot.arm.set_rotation("kuka_2", axe2)
# start scan
for i in range(10):
for j in range(10):
simu.robot.arm.set_rotation("kuka_1", axe1 + i * 0.03)
simu.sleep(0.01)
simu.robot.arm.set_rotation("kuka_2", axe2 + j * 0.03)
simu.sleep(0.01)
def print_pos(pose):
print("I'm currently at %s" % pose)
with pymorse.Morse() as simu:
# subscribes to updates from the Pose sensor by passing a callback
simu.r2d2.pose.subscribe(print_pos)
# sends a destination
simu.r2d2.motion.publish({
'x': 10.0,
'y': 5.0,
'z': 0.0,
'tolerance': 0.5,
'speed': 1.0
})
# Leave a couple of millisec to the simulator to start the action
simu.sleep(0.1)
# waits until we reach the target
while simu.r2d2.motion.get_status() != "Arrived":
simu.sleep(0.5)
print("Here we are!")
def startScan():
with pymorse.Morse() as simu:
simu.robot.arm.probeviz.subscribe(probedata)
# print(simu.robot.arm.depthvideocamera.get_properties())
# print(dir(simu.robot.arm))
# print(simu.robot.pa10.get_joints())
print(simu.robot.arm.get_IK_limits("kuka_1"))
print(simu.robot.arm.get_IK_limits("kuka_2"))
# move_IK_target(name, translation, rotation, relative, linear_speed, radial_speed) (non blocking)
for x in range(100):
# simu.robot.arm.move_IK_target('ik_target.robot.arm.kuka_7', [0.2,0.05*x,0.7], [0.0,0.0,0.0], False, 1,1 )
# simu.robot.arm.place_IK_target('ik_target.robot.arm.kuka_7', [0.02,0.05*x,0], [0.0,0.0,0.0], True)
simu.robot.arm.rotate("kuka_1", 0, 1)
simu.sleep(1)
for y in range(100):
# simu.robot.arm.set_rotations(x*0.01, y*0.01, 0,0,0,0,0)
# simu.robot.pa10.set_rotation_array(x*0.01,y*0.01,0,0,0)
simu.robot.arm.set_rotation("kuka_2", x*0.01)
simu.robot.arm.set_rotation("kuka_1", y*0.01)
# simu.robot.arm.depthvideocamera.capture(1)
# print(simu.robot.arm.depthvideocamera.get_local_data())
# simu.robot.arm.depthvideocamera.subscribe(depthdata)
# probe_value()
simu.sleep(0.1)
def handleMovePackage(req):
'''
This service moves the packages (changing their location in 3D simu env) by calling a morse service (set_object_position)
Parameters:
req: double x, double y, double z
Returns:
boolean success
'''
global mutex
rospy.loginfo(req.package_id)
while mutex == 1:
rospy.loginfo("waiting")
mutex = 1
success = True
try:
with pymorse.Morse() as sim:
sim.rpc('simulation', 'set_object_position', req.package_id,
[req.x, req.y, req.z])
except:
success = False
rospy.logdebug("[package manipulator]: MovePackage {} success: {}!".format(
req.package_id, success))
mutex = 0
return MovePackageResponse(success)
def go_three():
#Fleeing reaction called when p button hit
with pymorse.Morse() as simu, Morse() as morse:
# subscribes to updates from the Pose sensor by passing a callback
mousePose = morse.mouse.mousePose
nearObj = morse.mouse.proximity
bat = morse.mouse.mouse_battery
mousePosition = where_is(mousePose)
destination = go_where(mousePosition)
simu.mouse.motion.publish(destination)
cnt = 0
while nearObj:
cnt+1
print(cnt)
go_to()
# Check if rules file exists and load it
rules_name = simulation['simulation']['name']
try:
rules_name = findFileInPath(rules_name, 'toml', [SIMUPATH, GAMESPATH])
except:
raise
# variables about bandwidth
with open(rules_name, 'r') as rules_file:
rules = toml.loads(rules_file.read())
frequency = rules['simulation']['bandwidth']['frequency']
length = rules['simulation']['bandwidth']['length']
# connect to Morse to obtain list of robots, also controller.py is a client
with pymorse.Morse() as simu:
robots = {}
robots['CONTROLLER'] = {}
robots['CONTROLLER']['flag'] = False
for x in simu.robots:
robots[x] = {}
robots[x]['flag'] = False
# open connection to clients
try:
address = {}
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.bind((HOST, PORT))
s.listen(1)
i = 0
while i < len(robots.keys()):
def main():
# Parse arguments
parser = argparse.ArgumentParser()
parser.add_argument("-n",
"--name",
help="name of robot in MORSE environment")
parser.add_argument("-s",
"--maxSpeed",
help="max speed of QCOORD",
default=3)
args = parser.parse_args()
# A robot is a dictionary of info
# Will fail if args not set => implicit argument verification
robot = {}
# Init message passing interface (via Redis)
r = redis.StrictRedis(host='localhost', port=6379, db=0)
p = r.pubsub()
# Subscribe to messages from GCS
GCSconnStr = 'GCS'
p.subscribe(GCSconnStr)
# Publish to own channel
connStr = 'Morse-QCOORD-' + args.name
r.publish(connStr, 'Boot')
with pymorse.Morse() as simu:
# Init robot
robot = {
'name': args.name,
'simu': simu,
# Traits
'MAX_SPEED': float(args.maxSpeed),
'destination': {
'x': None,
'y': None
}
}
# Get camera specs
properties = getattr(robot['simu'],
robot['name']).PTU.videocamera.get_properties()
properties = str(properties)
cam_height = re.findall('\'cam_height\': \[[0-9]*,', properties)[0]
cam_height = float(
cam_height.replace('\'cam_height\': [', "").replace(',', ""))
cam_width = re.findall('\'cam_width\': \[[0-9]*,', properties)[0]
cam_width = float(
cam_width.replace('\'cam_width\': [', "").replace(',', ""))
cam_focallen = re.findall('\'cam_focal\': \[[.0-9]*,', properties)[0]
cam_focallen = float(
cam_focallen.replace('\'cam_focal\': [', "").replace(',', ""))
current = getattr(robot['simu'],
robot['name']).PTU.videocamera.get_configurations()
rotation = re.findall('rotation[^]]*', str(current))[0]
rotation = rotation.replace('rotation\': [[', '')
rotation = rotation.split(", ")
x_angle = float(rotation[0])
y_angle = float(rotation[1])
z_angle = float(rotation[2])
camera = {
'xSensor_mm': cam_height,
'ySensor_mm': cam_width,
'focallen_mm': cam_focallen,
'xGimbal_deg': math.degrees(z_angle),
'yGimbal_deg': math.degrees(y_angle)
}
robot['camera'] = camera
#print (camera)
# Ensure expected robot configuraton
systems_check(robot)
# Listen for messages
read = False
for msg in p.listen():
if (msg['type'] == 'message'):
msg_data = json.loads(msg['data'].decode("utf-8"))
if (msg_data['tag'] == Command.ping):
pong(r, connStr)
elif (msg_data['tag'] == Command.modeWaypoints):
pong(r, connStr)
receiveWaypoints(robot, 1, r, connStr, p)
elif (msg_data['tag'] == Command.halt):
halt(robot)
elif (msg_data['tag'] == Command.terminate):
halt(robot)
break
time.sleep(0.001)
def set_horizontal():
with pymorse.Morse() as simu:
simu.robot.arm.set_rotation("kuka_2", -3.14/4)
simu.robot.arm.set_rotation("kuka_4", 3.14/4)
simu.robot.arm.depthvideocamera.capture(-1)
simu.robot.arm.depthvideocamera.subscribe(depthdata)
floor_name+="LG"
elif f=="G":
floor_name+="UG"
else:
floor_name+=f
self._morse.rpc('simulation', 'set_object_visibility',
floor_name, show, True)
def call_button_click(self):
sender = self.sender()
f=sender.text()[-1]
print("Call to ",f)
self._call_floor(f)
def command_button_click(self):
sender = self.sender()
f=sender.text()[-1]
print("Command to ",f)
self._cmd_floor(f)
if __name__ == '__main__':
rospy.init_node("BHAM_CONTROL_GUI")
app = QtGui.QApplication(sys.argv)
# morse=None
with pymorse.Morse() as morse:
ex = ControlGUI(morse)
sys.exit(app.exec_())
#!/usr/local/bin/python3.5
import time
import pymorse
num_mod = 16
# morse.vehicle.arm.set_rotation("kuka_2", math.radians(-30)).result()
morse = pymorse.Morse()
def get_pose(mod_id):
# morse = pymorse.Morse()
module = getattr(morse, mod_id)
pose = module.position.get()
return pose
def set_dest(mod_id, x=0.0, y=0.0, z=0.0):
# morse = pymorse.Morse()
module = getattr(morse, mod_id)
destclient = module.destination
destclient.publish({'x': x, 'y': y, 'z': z})
def main():
mod_ids = [mod_id for mod_id in dir(morse) if mod_id[:3] == 'mod']
print(mod_ids)
num = 0
for mod_id in mod_ids:
set_dest(mod_id, x=1.0, y=1.0, z=2.0 - num*0.1)
def __init__(self, nengo):
"""
Class variables are created and initialized
"""
"""
Goal variables
"""
self.goal_world = [-28.0, 0.0, 0.0] # form: (x, y, theta)
self.goal_odom = [100.0, 100.0, 0.0] # form: (x, y, theta)
"""
Simulation specific variables
"""
self.simu = 0
self.nengo_sim = 0
"Data set booleans - one time"
self.goal_set = False
"Data set booleans - several times"
self.odom_set = self.laser_set = False
"""
Robot specific variables
"""
self.odom = self.rad_value = 0
self.position = [0.0, 0.0, 0.0]
"Laser specific variables"
self.laser = 0
self.scan_window = 180.0
self.resolution = 2.0
"""
Time specific variables
"""
self.zaehler = 0
self.firststamp = 0
self.secondstamp = 0
self.firstset = False
self.start_time = time.time()
self.model = nengo
self.model.set_robot_sim(self)
with pymorse.Morse() as self.simu:
# subscribes to updates from the Pose sensor by passing a callback
self.simu.robot.pose.subscribe(self.update_pose)
self.simu.robot.odom.subscribe(self.update_odom)
self.simu.robot.laser.subscribe(self.update_laser)
# sends a destination
self.motion_publisher(self.simu, self.model.vel)
# Leave a couple of millisec to the simulator to start the action
self.simu.sleep(0.1)
# waits until we reach the target
while True:
# checks whether the robot is in the goal area - if so the robot is stopped and the simulation finished
x_dist = self.goal_odom[0] - self.position[0]
y_dist = self.goal_odom[1] - self.position[1]
if -2.5 <= x_dist <= 2.5 and -2.5 <= y_dist <= 2.5:
self.model.vel = [0.0, 0.0]
self.motion_publisher(self.simu, self.model.vel)
break
# sends the data if all informations are set
if self.odom_set and self.laser_set:
self.model.set_odom(self.odom)
self.model.set_laser(self.laser, self.scan_window, self.resolution)
self.odom_set = self.laser_set = False
# makes one step with the model or the Dynamic Window Approach
self.model.step()
#self.model.move()
self.zaehler += .05
act_time = time.time() - self.start_time
print("Here we are! \n")
print("Nengo s: %s \n" % self.zaehler)
print("Python s: %s \n" % act_time)
print("Morse s: %s \n" % (self.secondstamp - self.firststamp))
def main():
# Parse arguments
parser = argparse.ArgumentParser()
parser.add_argument("-n",
"--name",
help="variable name of robot in MORSE simulator")
parser.add_argument("-w", "--waypoints", help="file containing waypoints")
parser.add_argument("-v",
"--verbose",
help="print robot info to stdout",
action='store_true')
args = parser.parse_args()
# Read waypoints
if (args.waypoints is not None):
with open(args.waypoints) as f:
waypoints = f.readlines()
waypoints = [x.strip() for x in waypoints]
waypoints = [(float(x.split(',')[0]), float(x.split(',')[1]))
for x in waypoints]
print(waypoints)
# A robot is a dictionary of info
# Will fail if args not set => implicit argument verification
robot = {}
# Init message passing interface (via Redis)
r = redis.StrictRedis(host='localhost', port=6379, db=0)
p = r.pubsub()
channelString = 'Morse-Marisa-' + args.name
#r.publish (channelString, 'Boot')
r.set('foo', 'bazz')
with pymorse.Morse() as simu:
try:
# Init robot using args and a Null destination
robot = {
'name': args.name,
'destination': {
'x': None,
'y': None
},
'simu': simu
}
systems_check(robot, simu)
print(str(get_status(robot)))
if (args.waypoints):
for w in waypoints:
goto_target(robot, {'x': w[0], 'y': w[1]}, 2.0)
while (getattr(
simu,
robot['name']).waypoint.get_status() == "Transit"):
status_json = json.dumps(get_status(robot),
separators=(',', ':'))
if (args.verbose):
print(str(get_status(robot)))
r.publish(channelString, status_json)
time.sleep(0.5)
#r.publish (channelString, "Arrived!")
#halt(robot)
else:
while (True == True):
status = get_status(robot)
status["dest_x"] = status["pos_x"]
status["dest_y"] = status["pos_y"]
print(str(status))
status_json = json.dumps(status, separators=(',', ':'))
r.publish(channelString, status_json)
time.sleep(1)
except pymorse.MorseServerError as mse:
print('Oops! An error occured!')
print(mse)