More precise Pepper LaserScan driver (with UTS Unleashed! libraries used)

pepper_laser_driver_precise.py

#!/usr/bin/env python
from math import cos, radians, sin, sqrt, atan2

# These are internal wrappers to make ROS easier to use
from magic_ros import InitROS, MessagePublisher, ROSNode

from rospy import Rate, Time
from sensor_msgs.msg import LaserScan

# Internal wrapper to make Qi easier to user
from qimate import QiMate

"""
Authors: Sammy Pfeiffer <Sammy.Pfeiffer at student.uts.edu.au>
         Suwen Leong
From UTS Unleashed! RoboCup@Home SSPL.
"""

laser_keys = [
    # Right X
    "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg01/X/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg02/X/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg03/X/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg04/X/Sensor/Value",
    "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg05/X/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg06/X/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg07/X/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg08/X/Sensor/Value",
    "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg09/X/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg10/X/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg11/X/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg12/X/Sensor/Value",
    "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg13/X/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg14/X/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg15/X/Sensor/Value",
    # Right Y
    "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg01/Y/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg02/Y/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg03/Y/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg04/Y/Sensor/Value",
    "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg05/Y/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg06/Y/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg07/Y/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg08/Y/Sensor/Value",
    "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg09/Y/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg10/Y/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg11/Y/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg12/Y/Sensor/Value",
    "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg13/Y/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg14/Y/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg15/Y/Sensor/Value",
    # Front X
    "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg01/X/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg02/X/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg03/X/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg04/X/Sensor/Value",
    "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg05/X/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg06/X/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg07/X/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg08/X/Sensor/Value",
    "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg09/X/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg10/X/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg11/X/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg12/X/Sensor/Value",
    "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg13/X/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg14/X/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg15/X/Sensor/Value",
    # Front Y
    "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg01/Y/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg02/Y/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg03/Y/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg04/Y/Sensor/Value",
    "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg05/Y/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg06/Y/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg07/Y/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg08/Y/Sensor/Value",
    "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg09/Y/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg10/Y/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg11/Y/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg12/Y/Sensor/Value",
    "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg13/Y/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg14/Y/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg15/Y/Sensor/Value",
    # Left X
    "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg01/X/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg02/X/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg03/X/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg04/X/Sensor/Value",
    "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg05/X/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg06/X/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg07/X/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg08/X/Sensor/Value",
    "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg09/X/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg10/X/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg11/X/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg12/X/Sensor/Value",
    "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg13/X/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg14/X/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg15/X/Sensor/Value",
    # Left Y
    "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg01/Y/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg02/Y/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg03/Y/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg04/Y/Sensor/Value",
    "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg05/Y/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg06/Y/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg07/Y/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg08/Y/Sensor/Value",
    "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg09/Y/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg10/Y/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg11/Y/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg12/Y/Sensor/Value",
    "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg13/Y/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg14/Y/Sensor/Value", "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg15/Y/Sensor/Value"
]


class PepperLaserDriver(ROSNode):
    def __init__(self, qi_ip='localhost'):
        super(PepperLaserDriver, self).__init__('pepper_laser_pub')
        self._qimate = QiMate(qi_ip)
        self._almemory = self._qimate.ALMemory
        self.laser_pub = MessagePublisher('/pepper/laser_scan', LaserScan)
        self.right_cos = cos(-1.757)
        self.right_sin = sin(-1.757)
        self.left_cos = cos(1.757)
        self.left_sin = sin(1.757)
        self.laser_msg = LaserScan()
        self.laser_msg.header.frame_id = "base_footprint"
        multiply_num_rays = 8
        self.num_rays = 61 * multiply_num_rays
        self.laser_msg.angle_min = radians(-120.)
        self.laser_msg.angle_max = radians(120.)
        self.laser_msg.angle_increment = (
            radians(120.) * 2.0) / float(self.num_rays)
        self.laser_msg.range_min = 0.1
        self.laser_msg.range_max = 3.0

        self.loginfo('PepperLaserDriver started.')

    def get_dist(self, x0, y0, x1=0.0, y1=0.0):
        return sqrt((x1 - x0) ** 2 + (y1 - y0) ** 2)

    def get_laser_data(self, num_rays, min_angle, angle_increment):
        laser_ranges = [float('nan')] * num_rays
        # Get laser readings
        laser_data = self._almemory.getListData(laser_keys)

        for idx in range(15):
            # Right lasers
            x_reading = laser_data[idx]
            y = laser_data[15 + idx]
            # Transform to base_footprint
            bx = x_reading * self.right_cos - y * self.right_sin - 0.018
            by = x_reading * self.right_sin + y * self.right_cos - 0.090
            dist = self.get_dist(bx, by)

            tmp_angle = atan2(by, bx) - min_angle
            # Add the laser reading to the closest beam
            closest_index = int(tmp_angle / angle_increment)
            if closest_index >= num_rays:
                laser_ranges[-1] = dist
            elif closest_index < 0:
                laser_ranges[0] = dist
            else:
                laser_ranges[closest_index] = dist

            # front lasers
            x_reading = laser_data[30 + idx]
            y = laser_data[45 + idx]
            dist = self.get_dist(x_reading, y)
            tmp_angle = atan2(y, x_reading) - min_angle
            closest_index = int(tmp_angle / angle_increment)
            if closest_index >= num_rays:
                laser_ranges[-1] = dist
            elif closest_index < 0:
                laser_ranges[0] = dist
            else:
                laser_ranges[closest_index] = dist

            # left lasers
            x_reading = laser_data[60 + idx]
            y = laser_data[75 + idx]
            # Transform to base_footprint
            bx = x_reading * self.left_cos - y * self.left_sin - 0.018
            by = x_reading * self.left_sin + y * self.left_cos + 0.090
            dist = self.get_dist(bx, by)

            tmp_angle = atan2(by, bx) - min_angle
            closest_index = int(tmp_angle / angle_increment)
            if closest_index >= num_rays:
                laser_ranges[-1] = dist
            elif closest_index < 0:
                laser_ranges[0] = dist
            else:
                laser_ranges[closest_index] = dist

        return laser_ranges

    def publish_laser(self):
        self.laser_msg.ranges = self.get_laser_data(self.num_rays,
                                                    self.laser_msg.angle_min,
                                                    self.laser_msg.angle_increment)
        self.laser_msg.header.stamp = Time.now()
        self.laser_pub.publish(self.laser_msg)

    def run(self):
        # publishes evey 10 Hz
        # The lasers update every 6Hz~, but they do independently, this is a good trade-off
        rate = Rate(10)
        while not self.is_ros_shutdown():
            self.publish_laser()
            rate.sleep()


if __name__ == "__main__":
    from os import getenv
    InitROS('pepper_laser_pub')
    pld = PepperLaserDriver(qi_ip=getenv('ROS_IP'))
    pld.run()