Commits

German Larrain committed 0162104

demos.sensors.laser: added VisualLaser, a visually more interesting simulation

Comments (0)

Files changed (2)

 	('sensors.joint_torque1', 'JointTorque'),
 	('sensors.kinetic_energy1', 'KineticEnergy'),
 	('sensors.laser', 'LaserSensor'),
+	('sensors.laser', 'VisualLaser'),
 	('sensors.potential_energy1', 'PotentialEnergy'),
 	('sensors.rotary_joint', 'RotaryJointSensor'),
 	('sensors.system_total_energy1', 'SystemTotalEnergy'),

demos/sensors/laser.py

+from ars.graphics import adapters as graphics_adapter
 from ars.lib.pydispatch import dispatcher
 from ars.model.robot import sensors
 from ars.model.simulator import signals
 from ars.utils import geometry as gmt
+from ars.utils import mathematical as mut
 
 from .base import FallingBalls, PrintDataMixin
 
 			self.sensor.on_change(time)
 		except Exception as e:
 			print('Exception when executing on_post_step: %s' % str(e))
+
+
+class VisualLaser(LaserSensor):
+
+	"""A simulation identical to :class:`LaserSensor` but more
+	interesting visually. For each intersection of the laser and an object,
+	a small colored sphere is shown.
+
+	.. warning::
+		The viewer may need to rotate the scene to see these spheres. To do
+		that, just click anywhere on the visualization, hold, and drag.
+
+	"""
+
+	SIGNAL = sensors.signals.SENSOR_POST_ON_CHANGE
+	SPHERE_RADIUS = 0.05
+	SPHERE_COLOR = (1.0, 1.0, 0.0)
+
+	def __init__(self):
+		super(VisualLaser, self).__init__()
+		self.intersection_point = None
+		dispatcher.connect(self.on_post_on_change, self.SIGNAL)
+
+	def on_post_step(self):
+		if self.intersection_point is not None:
+			self.gAdapter.remove_object(self.intersection_point)
+			self.intersection_point = None
+		super(VisualLaser, self).on_post_step()
+
+	def on_post_on_change(self, sender, *args, **kwargs):
+		"""Create and paint laser's closest contact point.
+
+		The sensor data is included in ``kwargs``. This method is to be
+		called at the the end of :meth:`sensors.Sensor.on_change`, when
+		the measurement has already been calculated.
+
+		:param sender: signal sender
+		:param args: signal data
+		:param kwargs: signal data
+
+		"""
+		distance = kwargs.get('data').get_kwarg('distance')
+		if distance is not None:
+			position = self._calc_intersection(distance)
+			self.intersection_point = graphics_adapter.Sphere(
+				radius=self.SPHERE_RADIUS, center=position)
+			self.intersection_point.set_color(self.SPHERE_COLOR)
+			self.gAdapter.add_object(self.intersection_point)
+
+	def _calc_intersection(self, distance):
+		ray = self.sensor.get_ray()
+		laser_pos = ray.get_position()
+		laser_rot = ray.get_rotation()
+
+		distance_vector = mut.mult_by_scalar3(mut.Z_AXIS, distance)
+		offset = mut.rotate3(laser_rot, distance_vector)
+		position = mut.add3(laser_pos, offset)
+		return position