def set_number_of_active_sensors(self, howmany):
self.number_of_active_sensors = howmany
self.number_of_data_points = self.number_of_active_sensors * 1
self.ranges = [0] * self.number_of_active_sensors
self.point_cloud = [pi_point.Point()] * self.number_of_active_sensors
self.simulated_origins = [pi_point.Point()] * self.number_of_active_sensors
self.populate_simulated_origins()
def get_relative_measured_point(self):
x = self.relative_transform.position.x + self.distance_measured * math.sin(
self.relative_transform.rotation)
y = self.relative_transform.position.y + self.distance_measured * math.cos(
self.relative_transform.rotation)
return pi_point.Point(x, y)
def __init__(self, position = None, rotation = None):
self.position = position
self.rotation = rotation
if self.position is None:
self.position = pi_point.Point()
if self.rotation is None:
self.rotation = 0.0
def compute(self):
self.ranges = [0] * self.number_of_active_sensors
self.point_cloud = [pi_point.Point()] * self.number_of_active_sensors
self.simulated_origins = [pi_point.Point()
] * self.number_of_active_sensors
self.populate_simulated_origins()
# pre-populate the LaserScan message
self.scan_msg = LaserScan()
self.scan_msg.header.frame_id = self.relative_frame_id
self.scan_msg.angle_min = -math.pi
self.scan_msg.angle_max = math.pi
self.scan_msg.angle_increment = (2.0 *
math.pi) / self.number_of_data_points
self.scan_msg.time_increment = 0
self.scan_msg.scan_time = 0
self.scan_msg.range_min = 0.01
self.scan_msg.range_max = 0.50
def populate_simulated_origins(self):
sim_radius = 100
angle_increment = 2.0 * math.pi / self.number_of_data_points
for i in range(self.number_of_data_points):
angle = i * angle_increment
x_position = self.relative_transform.position.x + sim_radius * math.sin(self.relative_transform.rotation + angle)
y_position = self.relative_transform.position.y + sim_radius * math.cos(self.relative_transform.rotation + angle)
self.simulated_origins[i] = pi_point.Point(x_position, y_position)
def set_from_ros_transform(self, ros_stamped_transform):
x = ros_stamped_transform.transform.translation.x
y = ros_stamped_transform.transform.translation.y
self.rotation = euler_from_quaternion(
(ros_stamped_transform.transform.rotation.x,
ros_stamped_transform.transform.rotation.y,
ros_stamped_transform.transform.rotation.z,
ros_stamped_transform.transform.rotation.w))[-1]
self.position = pi_point.Point(x, y)