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