1######################################################################
2# Software License Agreement (BSD License)
3#
4#  Copyright (c) 2013, Rice University
5#  All rights reserved.
6#
7#  Redistribution and use in source and binary forms, with or without
8#  modification, are permitted provided that the following conditions
9#  are met:
10#
11#   * Redistributions of source code must retain the above copyright
12#     notice, this list of conditions and the following disclaimer.
13#   * Redistributions in binary form must reproduce the above
14#     copyright notice, this list of conditions and the following
15#     disclaimer in the documentation and/or other materials provided
16#     with the distribution.
17#   * Neither the name of the Rice University nor the names of its
18#     contributors may be used to endorse or promote products derived
19#     from this software without specific prior written permission.
20#
21#  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22#  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23#  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24#  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25#  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26#  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27#  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28#  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29#  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30#  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31#  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32#  POSSIBILITY OF SUCH DAMAGE.
33######################################################################
34
35# Author: Caleb Voss
36
37# MORSE builder script
38
39import logging
40import os
41import subprocess
42import sys
43
44import bpy
45
46import morse.builder
47import morse.blender
48
49OMPL_DIR = os.path.dirname(__file__)
50
51print("OMPL builder script invocation: " + str(sys.argv))
52
53# Determine the mode to use (third argument)
54mode = sys.argv[sys.argv.index('--') + 3]
55
56# Use wmctrl for window manipulation
57# (fails silently if wmctrl not installed)
58winID = subprocess.check_output(['bash', '-c', \
59    'wmctrl -l | grep morse_default_autorun | awk \'{ print $1 }\'']).decode()[:-1]
60# Set a meaningful title
61if mode == 'PLAN':
62    subprocess.call(['bash', '-c', 'wmctrl -i -r %s -T "OMPL MORSE Planner"' % winID])
63elif mode == 'PLAY':
64    subprocess.call(['bash', '-c', 'wmctrl -i -r %s -T "OMPL MORSE Player"' % winID])
65elif mode == 'QUERY':
66    subprocess.call(['bash', '-c', 'wmctrl -i -r %s -T "OMPL MORSE Control Query"' % winID])
67# Hide the Blender window if we're planning or querying
68if mode == 'PLAN' or mode == 'QUERY':
69    subprocess.call(['bash', '-c', 'wmctrl -i -r %s -b add,shaded' % winID])
70    subprocess.call(['bash', '-c', 'wmctrl -i -r %s -e 0,100,100,600,200' % winID])
71
72# Disable logging of socket communication because there will be a lot of it
73sockloggers = (logging.getLogger("morse.morse.core.request_manager"),
74               logging.getLogger("morse.morse.middleware.socket_request_manager"))
75sockloggers[0].setLevel(logging.ERROR)
76sockloggers[1].setLevel(logging.ERROR)
77
78# Load the .blend file (first argument after '--')
79envpath = sys.argv[sys.argv.index('--') + 1]
80print("\n* Loading scene <%s>.\n" % envpath)
81env = morse.builder.Environment(envpath)
82
83# Replace the robot(s) stand-in models with actual MORSE robot objects
84to_delete = []
85i = 0
86for obj in bpy.context.scene.objects:
87
88    # In PLAY mode, delete the goals
89    if [True for goalStr in ['.goalPose', '.goalRegion', '.goalRot'] if obj.name.endswith(goalStr)]:
90        if mode == 'PLAY':
91            to_delete.append(obj)
92        continue
93
94    # If this object is a robot
95    if obj.get('RobotType'):
96
97        rtype = obj['RobotType']
98        ctype = obj['ControllerType']
99        pos = obj.location
100        rot = obj.rotation_euler
101
102        # Make names acceptable for MORSE
103        rname = obj.name
104        rnameSafe = rname.replace('.', '_')
105        if rname != rnameSafe:
106            print("WARNING: had to rename robot %s to %s because dots not allowed in MORSE names"
107                  % (rname, rnameSafe))
108            for goalStr in ['.goalPose', '.goalRegion', '.goalRot']:
109                goal = bpy.context.scene.objects.get(obj.name + goalStr)
110                if goal:
111                    print("\t> also renamed goal %s" % goal.name)
112                    goal.name = rnameSafe + goalStr
113            rname = rnameSafe
114
115        # Avoid name collision and mark for deletion
116        obj.name += '_'
117        to_delete.append(obj)
118
119        # Add the MORSE components
120        robot = getattr(morse.builder, rtype)(rname)
121        motion = getattr(morse.builder, ctype)(robot.name+'Motion')
122
123        # Restore pose
124        robot.location = pos
125        robot.rotation_euler = rot
126        robot.append(motion)
127        motion.add_service('socket')
128        i += 1
129
130##
131# \brief Recursively delete an object and its children
132def _recurseDelete(obj):
133
134    # Call this function on all the children
135    for child in obj.children[:]:
136        _recurseDelete(child)
137
138    # Select only this object and delete it
139    bpy.ops.object.select_pattern(pattern=obj.name, case_sensitive=True, extend=False)
140    bpy.ops.object.delete()
141
142# Delete the stand-in models
143for obj in to_delete:
144    _recurseDelete(obj)
145
146# Disallow sleeping for rigid bodies
147for obj in bpy.context.scene.objects:
148    if obj.game.physics_type == 'RIGID_BODY':
149        # True means "no sleeping"
150        obj.game.use_sleep = True
151
152settings = bpy.data.objects['ompl_settings']
153
154# Determine the solution path file to use (second argument)
155outpath = sys.argv[sys.argv.index('--') + 2]
156settings['Outpath'] = outpath
157
158# Set the mode setting
159settings['Mode'] = mode
160
161# Record animation data if we're doing playback
162if mode == 'PLAY':
163    bpy.context.scene.game_settings.use_animation_record = True
164
165bpy.ops.object.select_all(action='DESELECT')
166
167# Add 'Tick' sensor
168bpy.ops.logic.sensor_add(type='DELAY', name='Tick', object='ompl_settings')
169tick = settings.game.sensors['Tick']
170tick.use_repeat = True
171
172# Add 'communicator.py' text block
173bpy.ops.text.open(filepath=OMPL_DIR + "/communicator.py")
174
175# Add 'Comm' controller for the script
176bpy.ops.logic.controller_add(type='PYTHON', name='Comm', object='ompl_settings')
177comm = settings.game.controllers['Comm']
178comm.mode = 'MODULE'
179comm.module = 'communicator.main'
180
181# Link Tick with Comm so it's run every frame
182tick.link(comm)
183
184# Create the environment
185env.create()
186