1import logging; logger = logging.getLogger("morse.builder." + __name__)
2import math
3from morse.builder.creator import SensorCreator, bpymorse
4from morse.builder.blenderobjects import *
5
6class Accelerometer(SensorCreator):
7    _classpath = "morse.sensors.accelerometer.Accelerometer"
8
9    def __init__(self, name=None):
10        SensorCreator.__init__(self, name)
11        mesh = Cube("AccelerometerCube")
12        mesh.scale = (.04, .04, .02)
13        mesh.color(.3, .9, .6)
14        self.append(mesh)
15
16class ArmaturePose(SensorCreator):
17    _classpath = "morse.sensors.armature_pose.ArmaturePose"
18
19class Attitude(SensorCreator):
20    _classpath = "morse.sensors.attitude.Attitude"
21
22class Barometer(SensorCreator):
23    _classpath = "morse.sensors.barometer.Barometer"
24
25class Battery(SensorCreator):
26    _classpath = "morse.sensors.battery.Battery"
27
28    def __init__(self, name=None):
29        SensorCreator.__init__(self, name)
30        mesh = Cylinder("BatteryCylinder")
31        mesh.scale = (.01, .01, .04)
32        mesh.color(.2, .2, .2)
33        self.append(mesh)
34        self.properties(DischargingRate = 0.05)
35
36class CompoundSensor(SensorCreator):
37    _classpath = "morse.sensors.compound.CompoundSensor"
38
39    def __init__(self, sensors, name=None):
40        SensorCreator.__init__(self, name)
41
42        self.sensors = sensors
43
44    def after_renaming(self):
45        # Ensures this is called only when all components have been (automatically) renamed.
46        self.properties(sensors = ",".join([str(s) for s in self.sensors]))
47
48class GPS(SensorCreator):
49    _classpath  = "morse.sensors.gps.GPS"
50
51    def __init__(self, name=None):
52        SensorCreator.__init__(self, name)
53        mesh = Sphere("GPSSphere")
54        mesh.scale = (.04, .04, .01)
55        mesh.color(.5, .5, .5)
56        self.append(mesh)
57
58class Gyroscope(SensorCreator):
59    _classpath = "morse.sensors.gyroscope.Gyroscope"
60
61    def __init__(self, name=None):
62        SensorCreator.__init__(self, name)
63        mesh = Sphere("GyroscopeSphere")
64        mesh.scale = (.04, .04, .01)
65        mesh.color(.8, .4, .1)
66        self.append(mesh)
67
68class IMU(SensorCreator):
69    _classpath = "morse.sensors.imu.IMU"
70
71    def __init__(self, name=None):
72        SensorCreator.__init__(self, name)
73        mesh = Cube("IMUCube")
74        mesh.scale = (.04, .04, .02)
75        mesh.color(.3, .9, .6)
76        self.append(mesh)
77
78class Magnetometer(SensorCreator):
79    _classpath = "morse.sensors.magnetometer.Magnetometer"
80
81class Odometry(SensorCreator):
82    _classpath = "morse.sensors.odometry.Odometry"
83
84    def __init__(self, name=None):
85        SensorCreator.__init__(self, name)
86        mesh = Cylinder("OdometryCylinder")
87        mesh.scale = (.02, .02, .02)
88        mesh.color(.5, .5, .5)
89        self.append(mesh)
90
91class Pose(SensorCreator):
92    _classpath = "morse.sensors.pose.Pose"
93
94    def __init__(self, name=None):
95        SensorCreator.__init__(self, name)
96        mesh = Cube("PoseCube")
97        mesh.scale = (.04, .04, .02)
98        mesh.color(.8, .3, .1)
99        self.append(mesh)
100
101class Proximity(SensorCreator):
102    _classpath = "morse.sensors.proximity.Proximity"
103
104    def __init__(self, name=None):
105        SensorCreator.__init__(self, name)
106        mesh = Cylinder("ProximityCylinder")
107        mesh.scale = (.02, .02, .02)
108        mesh.color(.5, .5, .5)
109        self.append(mesh)
110        self.properties(Range = 30.0, Track = "Robot_Tag")
111        self.frequency(12)
112
113class PTUPosture(SensorCreator):
114    _classpath = "morse.sensors.ptu_posture.PTUPosture"
115
116class SearchAndRescue(SensorCreator):
117    _classpath = "morse.sensors.search_and_rescue.SearchAndRescue"
118
119    def __init__(self, name=None):
120        SensorCreator.__init__(self, name)
121        mesh = Cylinder("SearchAndRescueCylinder")
122        mesh.scale = (.15, .04, .04)
123        mesh.color(.2, .8, .6)
124        mesh.rotate(y = math.pi / 2)
125        self.append(mesh)
126        self.frequency(6)
127        self.properties(Heal_range = 2.0, Abilities = "1,2")
128        # add Radar freq: 3 Hz, prop: Injured, angle: 60.0, distance: 10.0
129        bpymorse.add_sensor(type="RADAR", name="Radar")
130        obj = bpymorse.get_context_object()
131        sensor = obj.game.sensors[-1]
132        sensor.angle = 60.0
133        sensor.distance = 10.0
134        sensor.use_pulse_true_level = True
135        self._set_sensor_frequency(sensor, 20)
136        sensor.property = "Injured"
137        # link it to the Python controller
138        controller = obj.game.controllers[-1]
139        controller.link(sensor = sensor)
140
141    def properties(self, **kwargs):
142        self.select()
143        # We may be use it before the definition of radar
144        # But angle and distance can only be defined by user, in case
145        # where we are sure that Radar is well defined
146        try:
147            radar = self._bpy_object.game.sensors["Radar"]
148            if 'Angle' in kwargs:
149                radar.angle = kwargs['Angle']
150            if 'Distance' in kwargs:
151                radar.distance = kwargs['Distance']
152            if 'Freq' in kwargs:
153                radar.freq = kwargs['Freq']
154        except KeyError:
155            pass
156        SensorCreator.properties(self, **kwargs)
157
158class StereoUnit(SensorCreator):
159    _classpath = "morse.sensors.stereo_unit.StereoUnit"
160
161    def __init__(self, name=None):
162        SensorCreator.__init__(self, name)
163        mesh = Cube("StereoUnitCube")
164        mesh.scale = (.025, .24, .01)
165        mesh.color(.8, .8, .8)
166        self.append(mesh)
167
168class Thermometer(SensorCreator):
169    _classpath = "morse.sensors.thermometer.Thermometer"
170
171    def __init__(self, name=None):
172        SensorCreator.__init__(self, name)
173        mesh = Cylinder("ThermometerCylinder")
174        mesh.scale = (.02, .02, .04)
175        mesh.color(0, .6, .5)
176        self.append(mesh)
177
178# abstract class
179class LaserSensorWithArc(SensorCreator):
180    _classpath  = "morse.sensors.laserscanner.LaserScanner"
181
182    def get_ray_material(self):
183        if 'RayMat' in bpymorse.get_materials():
184            return bpymorse.get_material('RayMat')
185
186        ray_material = bpymorse.create_new_material()
187        ray_material.diffuse_color = (.9, .05, .2)
188        ray_material.use_shadeless = True
189        ray_material.use_raytrace = False # ? is it needed ?
190        ray_material.use_cast_buffer_shadows = False # ? is it needed ?
191        ray_material.use_cast_approximate = False # ? is it needed ?
192        ray_material.use_transparency = True
193        ray_material.transparency_method = 'Z_TRANSPARENCY'
194        ray_material.alpha = 0.3
195        ray_material.name = 'RayMat'
196        # link material to object as: Arc_XXX.active_material = ray_material
197        return ray_material
198
199    def create_laser_arc(self):
200        """ Create an arc for use with the laserscanner sensor
201
202        The arc is created using the parameters in the laserscanner Empty.
203        'resolution and 'scan_window' are used to determine how many points
204        will be added to the arc.
205        """
206        scene = bpymorse.get_context_scene()
207        laserscanner_obj = self._bpy_object
208
209        # Delete previously created arc
210        for child in laserscanner_obj.children:
211            if child.name.startswith("Arc_"):
212                scene.objects.unlink( child )
213
214        # Read the parameters to create the arc
215        properties = laserscanner_obj.game.properties
216        resolution = properties['resolution'].value
217        window = properties['scan_window'].value
218        # Parameters for multi layer sensors
219        try:
220            layers = properties['layers'].value
221            layer_separation = properties['layer_separation'].value
222            layer_offset = properties['layer_offset'].value
223        except KeyError as detail:
224            layers = 1
225            layer_separation = 0.0
226            layer_offset = 0.0
227        logger.debug ("Creating %d arc(s) of %.2f degrees, resolution %.2f" % \
228                      (layers, window, resolution))
229        mesh = bpymorse.new_mesh( "ArcMesh" )
230        # Add the center vertex to the list of vertices
231        verts = [ [0.0, 0.0, 0.0] ]
232        faces = []
233        vertex_index = 0
234
235        # Set the vertical angle, in case of multiple layers
236        if layers > 1:
237            v_angle = layer_separation * (layers-1) / 2.0
238        else:
239            v_angle = 0.0
240
241        # Initialise the parameters for every layer
242        for layer_index in range(layers):
243            start_angle = -window / 2.0
244            end_angle = window / 2.0
245            # Offset the consecutive layers
246            if (layer_index % 2) == 0:
247                start_angle += layer_offset
248                end_angle += layer_offset
249            logger.debug ("Arc from %.2f to %.2f" % (start_angle, end_angle))
250            logger.debug ("Vertical angle: %.2f" % v_angle)
251            arc_angle = start_angle
252
253            # Create all the vertices and faces in a layer
254            while arc_angle <= end_angle:
255                # Compute the coordinates of the new vertex
256                new_vertex = [ math.cos(math.radians(arc_angle)),
257                               math.sin(math.radians(arc_angle)),
258                               math.sin(math.radians(v_angle)) ]
259                verts.append(new_vertex)
260                vertex_index += 1
261                # Add the faces after inserting the 2nd vertex
262                if arc_angle > start_angle:
263                    faces.append([0, vertex_index-1, vertex_index])
264                # Increment the angle by the resolution
265                arc_angle = arc_angle + resolution
266
267            v_angle -= layer_separation
268
269        mesh.from_pydata( verts, [], faces )
270        mesh.update()
271        # Compose the name of the arc
272        arc_name = "Arc_%d" % window
273        arc = bpymorse.new_object( arc_name, mesh )
274        arc.data = mesh
275        # Remove collision detection for the object
276        arc.game.physics_type = 'NO_COLLISION'
277        arc.hide_render = True
278        # Link the new object in the scene
279        scene.objects.link( arc )
280        # Set the parent to be the laserscanner Empty
281        arc.parent = laserscanner_obj
282        # Set the material of the arc
283        arc.active_material = self.get_ray_material()
284        return arc
285
286    def after_renaming(self):
287        arc = [child for child in self._bpy_object.children
288               if child.name.startswith("Arc_")]
289        if not arc:
290            self.create_laser_arc()
291
292class Hokuyo(LaserSensorWithArc):
293    """
294    A laser scanner configured to mimick the Hokuyo sensor.
295
296    See :doc:`the laser scanners general documentation <../sensors/laserscanner>` for details.
297    """
298    _blendname = "sick"
299    _name = "Hokuyo"
300    _short_desc = "Hokuyo laser scanner"
301
302    def __init__(self, name=None):
303        LaserSensorWithArc.__init__(self, name)
304        mesh = Cylinder("HokuyoCylinder")
305        mesh.scale = (.04, .04, .08)
306        mesh.color(0, 0, 0)
307        self.append(mesh)
308        # set components-specific properties
309        self.properties(Visible_arc = False, laser_range = 30.0,
310                scan_window = 270.0, resolution = 0.25)
311        # set the frequency to 10 Hz
312        self.frequency(10)
313
314class Sick(LaserSensorWithArc):
315    """
316    A laser scanner configured to mimick the SICK sensor.
317
318    See :doc:`the laser scanners general documentation <../sensors/laserscanner>` for details.
319    """
320    _blendname = "sick"
321    _name = "SICK"
322    _short_desc = "SICK laser scanner"
323
324
325    def __init__(self, name=None):
326        LaserSensorWithArc.__init__(self, name)
327        # set components-specific properties
328        self.properties(Visible_arc = False, laser_range = 30.0,
329                scan_window = 180.0, resolution = 1.0)
330        # set the frequency to 10 Hz
331        self.frequency(10)
332        # append sick mesh, from MORSE_COMPONENTS/sensors/sick.blend
333        self.append_meshes(['SickMesh'])
334
335class SickLDMRS(LaserSensorWithArc):
336    """
337    A laser scanner configured to mimick the SICK LD-MRS sensor.
338
339    See :doc:`the laser scanners general documentation <../sensors/laserscanner>` for details.
340    """
341    _blendname = "sick"
342    _name = "SICK LD-MRS"
343    _short_desc = "SICK LD-MRS laser scanner"
344
345
346    def __init__(self, name=None):
347        LaserSensorWithArc.__init__(self, name)
348        # set components-specific properties
349        self.properties(Visible_arc = False, laser_range = 30.0,
350                scan_window = 100.0, resolution = 0.25, layers = 4,
351                layer_separation = 0.8, layer_offset = 0.125)
352        mesh = Cube("SickMesh")
353        mesh.scale = (.05, .0825, .044)
354        mesh.color(1., 1., .9)
355        self.append(mesh)
356        # set the frequency to 4 Hz
357        self.frequency(4)
358
359class Infrared(LaserSensorWithArc):
360    """
361    A laser scanner configured to mimick a infra-red proximity sensor.
362
363    See :doc:`the laser scanners general documentation <../sensors/laserscanner>` for details.
364    """
365    _name = "Infrared Proximity Sensor"
366    _short_desc = "Infra-red (IR) proximity sensor."
367
368    def __init__(self, name=None):
369        LaserSensorWithArc.__init__(self, name)
370        mesh = Cube("InfraredCube")
371        mesh.scale = (.02, .02, .02)
372        mesh.color(.8, .8, .8)
373        self.append(mesh)
374        # set components-specific properties
375        self.properties(Visible_arc = True, laser_range = 2.0,
376                scan_window = 20.0, resolution = 1.0)
377        # set the frequency to 10 Hz
378        self.frequency(10)
379
380class Velocity(SensorCreator):
381    _classpath = "morse.sensors.velocity.Velocity"
382
383    def __init__(self, name=None):
384        SensorCreator.__init__(self, name)
385        mesh = Sphere("VelocitySphere")
386        mesh.scale = (.04, .04, .01)
387        mesh.color(.5, .5, .5)
388        self.append(mesh)
389
390class VideoCamera(SensorCreator):
391    _classpath = "morse.sensors.video_camera.VideoCamera"
392    _blendname = "camera"
393    def __init__(self, name=None):
394        SensorCreator.__init__(self, name)
395        self.camera = Camera("CameraRobot")
396        self.camera.name = "CameraRobot"
397        self.append(self.camera)
398        self.properties(cam_width = 256, cam_height = 256, cam_focal = 35.0,
399                        capturing = True, Vertical_Flip = True)
400        # set the frequency to 20 Hz
401        self.frequency(20)
402        # add toggle capture action (`Space` key)
403        bpymorse.add_sensor(type="KEYBOARD")
404        obj = bpymorse.get_context_object()
405        sensor = obj.game.sensors[-1]
406        sensor.key = 'SPACE'
407        bpymorse.add_controller(type='LOGIC_AND')
408        controller = obj.game.controllers[-1]
409        bpymorse.add_actuator(type='PROPERTY')
410        actuator = obj.game.actuators[-1]
411        actuator.mode = 'TOGGLE'
412        actuator.property = 'capturing'
413        controller.link(sensor = sensor, actuator = actuator)
414        # looking in +X
415        SensorCreator.rotate(self, x=math.pi/2, z=math.pi/2)
416        # append CameraMesh with its textures
417        self.mesh = self.append_meshes(['CameraMesh'], "camera")[0]
418        self.rotate(z=math.pi)
419    def rotate(self, x=0, y=0, z=0):
420        SensorCreator.rotate(self, x=y, y=z, z=x)
421    def hide_mesh(self, hide=True):
422        """ Hide the camera mesh
423
424        Can be used to hide a third person camera attached to a robot.
425        """
426        self.mesh.hide_render = hide
427
428class DepthCamera(VideoCamera):
429    _classpath = "morse.sensors.depth_camera.DepthCamera"
430    _blendname = "camera"
431
432    def __init__(self, name=None):
433        VideoCamera.__init__(self, name)
434        self.properties(cam_near=1.0, cam_far=20.0, retrieve_depth=True,
435                        Vertical_Flip=False)
436
437class Velodyne(DepthCamera):
438    _classpath = "morse.sensors.depth_camera.DepthCameraRotationZ"
439    _blendname = "velodyne"
440
441    def __init__(self, name=None):
442        DepthCamera.__init__(self, name)
443        self.camera.properties(NOT_F9_ABLE=1)
444        self.properties(rotation=self.camera._bpy_object.data.angle)
445        self.mesh = self.append_meshes(['VelodyneMesh'])[0]
446        self.mesh.rotation_euler.x = math.pi / 2
447        self.mesh.rotation_euler.y = -math.pi / 2
448        self.mesh.scale = [1.1]*3
449
450
451VelodyneZB = Velodyne # morse 1.1 compatible
452
453class SemanticCamera(VideoCamera):
454    _classpath = "morse.sensors.semantic_camera.SemanticCamera"
455    _blendname = "camera"
456
457    def __init__(self, name=None):
458        VideoCamera.__init__(self, name)
459
460
461class VelodyneRayCast(LaserSensorWithArc):
462    _classpath = "morse.sensors.laserscanner.LaserScannerRotationZ"
463    _blendname = "velodyne"
464
465    def __init__(self, name=None):
466        LaserSensorWithArc.__init__(self, name)
467        # set components-specific properties
468        self.properties(Visible_arc = True, laser_range = 50.0,
469                        scan_window = 31.500, resolution = 0.5)
470        # append velodyne mesh, from MORSE_COMPONENTS/sensors/velodyne.blend
471        arc = self.create_laser_arc()
472        # Select only arc (active)
473        bpymorse.select_only(arc)
474        # Rotate Arc to scan vertically
475        arc.rotation_euler = (math.radians(90), math.radians(12), 0)
476        bpymorse.apply_transform(rotation=True)
477        self.append_meshes(['VelodyneMesh'])
478
479class Clock(SensorCreator):
480    _classpath = "morse.sensors.clock.Clock"
481
482class Kinect(CompoundSensor):
483    """
484    Microsoft Kinect RGB-D camera, implemented as a pair of depth camera and video
485    camera.
486
487    See the general documentation for :doc:`video cameras
488    <../sensors/video_camera>` and :doc:`depth cameras
489    <../sensors/depth_camera>` for details.
490
491    """
492    _name = "Kinect"
493    _short_desc="Microsoft Kinect RGB-D sensor"
494
495    def __init__(self, name="Kinect"):
496        # meta sensor composed of 2 cameras (rgb and depth)
497        CompoundSensor.__init__(self, [], name)
498        mesh = Cube("KinectMesh")
499        mesh.scale = (.02, .1, .02)
500        mesh.color(.8, .8, .8)
501        self.append(mesh)
502        self.video_camera = VideoCamera('rgb')
503        self.video_camera.properties(cam_width = 128, cam_height=128)
504        self.depth_camera = DepthCamera('depth')
505        self.depth_camera.properties(classpath='morse.sensors.depth_camera.DepthVideoCamera')
506        self.depth_camera.properties(cam_width = 128, cam_height=128, Vertical_Flip=True)
507        self.append(self.video_camera)
508        self.append(self.depth_camera)
509        # TODO find Kinect spec for cameras positions
510        self.video_camera.location = (.06, +.08, .04)
511        self.depth_camera.location = (.06, -.08, .04)
512        self.sensors = [self.video_camera, self.depth_camera]
513    def add_stream(self, *args, **kwargs):
514        # Override AbstractComponent method
515        self.video_camera.add_stream(*args, **kwargs)
516        self.depth_camera.add_stream(*args, **kwargs)
517    def profile(self):
518        # Override AbstractComponent method
519        self.video_camera.profile()
520        self.depth_camera.profile()
521    def frequency(self, frequency):
522        # Override AbstractComponent method
523
524        # XXX frequency() is called in SensorCreator, while the
525        # sub-objects are not yet created. Through, it is not too bad,
526        # as the different sub-objects have specific default frequency.
527        if hasattr(self, 'video_camera'):
528            self.video_camera.frequency(frequency)
529            self.depth_camera.frequency(frequency)
530
531class Collision(SensorCreator):
532    _classpath = "morse.sensors.collision.Collision"
533
534    def __init__(self, name=None):
535        """ Sensor to detect objects colliding with the current object.
536
537        Doc: https://www.blender.org/manual/game_engine/logic/sensors/collision.html
538        """
539        SensorCreator.__init__(self, name)
540        obj = bpymorse.get_context_object()
541        # Sensor, Collision Sensor, detects static and dynamic objects but
542        # not the other collision sensor objects.
543        obj.game.physics_type = 'SENSOR'
544        # Specify a collision bounds type other than the default
545        obj.game.use_collision_bounds = True
546        obj.scale = (0.02,0.02,0.02)
547        # replace Always sensor by Collision sensor
548        sensor = obj.game.sensors[-1]
549        sensor.type = 'COLLISION'
550        # need to get the new Collision Sensor object
551        sensor = obj.game.sensors[-1]
552        sensor.use_pulse_true_level = True # FIXME doesnt seems to have any effect
553        sensor.use_material = False # we want to filter by property, not by material
554        # Component mesh (eye sugar)
555        mesh = Cube("CollisionMesh")
556        mesh.color(.8, .2, .1)
557        self.append(mesh)
558    def properties(self, **kwargs):
559        SensorCreator.properties(self, **kwargs)
560        if 'only_objects_with_property' in kwargs:
561            try:
562                sensor = self._bpy_object.game.sensors[-1]
563                sensor.property = kwargs['only_objects_with_property']
564            except KeyError:
565                pass
566
567class RadarAltimeter(SensorCreator):
568    _classpath = "morse.sensors.radar_altimeter.RadarAltimeter"
569
570class Airspeed(SensorCreator):
571    _classpath = "morse.sensors.airspeed.Airspeed"
572