#!/usr/bin/env python
# coding: utf-8

import os
import sys
import atexit
import pybullet
from qibullet.camera import Camera
from qibullet.camera import CameraRgb
from qibullet.camera import CameraDepth
from qibullet.nao_virtual import NaoVirtual
from qibullet.romeo_virtual import RomeoVirtual
from qibullet.pepper_virtual import PepperVirtual
from qibullet.base_controller import PepperBaseController
from threading import Thread

try:
    import rospy
    import roslib
    import roslaunch
    import tf2_ros
    from cv_bridge import CvBridge
    from sensor_msgs.msg import Image
    from sensor_msgs.msg import CameraInfo
    from sensor_msgs.msg import JointState
    from sensor_msgs.msg import LaserScan
    from std_msgs.msg import Header
    from std_msgs.msg import Empty
    from naoqi_bridge_msgs.msg import JointAnglesWithSpeed
    from geometry_msgs.msg import TransformStamped
    from geometry_msgs.msg import Twist
    from nav_msgs.msg import Odometry

    try:
        from naoqi_bridge_msgs.msg import PoseStampedWithSpeed as MovetoPose
        OFFICIAL_DRIVER = False
        print("Using softbankrobotics-research forked version of NAOqi driver")

    except ImportError as e:
        from geometry_msgs.msg import PoseStamped as MovetoPose
        OFFICIAL_DRIVER = True

    MISSING_IMPORT = None

except ImportError as e:
    MISSING_IMPORT = str(e)

TOP_OPTICAL_FRAME = "CameraTop_optical_frame"
BOTTOM_OPTICAL_FRAME = "CameraBottom_optical_frame"
DEPTH_OPTICAL_FRAME = "CameraDepth_optical_frame"


class RosWrapper:
    """
    Virtual class defining the basis of a robot ROS wrapper
    """

    def __init__(self):
        """
        Constructor
        """
        if MISSING_IMPORT is not None:
            raise pybullet.error(MISSING_IMPORT)

        self.spin_thread = None
        self._wrapper_termination = False
        self.image_bridge = CvBridge()
        self.front_info_msg = dict()
        self.bottom_info_msg = dict()
        self.depth_info_msg = dict()
        self.roslauncher = None
        self.transform_broadcaster = tf2_ros.TransformBroadcaster()
        atexit.register(self.stopWrapper)

    def stopWrapper(self):
        """
        Stops the ROS wrapper
        """
        self._wrapper_termination = True

        try:
            assert self.spin_thread is not None
            assert isinstance(self.spin_thread, Thread)
            assert self.spin_thread.isAlive()
            self.spin_thread.join()

        except AssertionError:
            pass

        if self.roslauncher is not None:
            self.roslauncher.stop()
            print("Stopping roslauncher")

    def launchWrapper(self, virtual_robot, ros_namespace, frequency=200):
        """
        Launches the ROS wrapper

        Parameters:
            virtual_robot - The instance of the simulated model
            ros_namespace - The ROS namespace to be added before the ROS topics
            advertized and subscribed
            frequency - The frequency of the ROS rate that will be used to pace
            the wrapper's main loop
        """
        if MISSING_IMPORT is not None:
            raise pybullet.error(MISSING_IMPORT)

        self.robot = virtual_robot
        self.ros_namespace = ros_namespace
        self.frequency = frequency

        rospy.init_node(
            "qibullet_wrapper",
            anonymous=True,
            disable_signals=False)

        # Upload the robot description to the ros parameter server
        try:
            if isinstance(self.robot, PepperVirtual):
                robot_name = "pepper"
            elif isinstance(self.robot, NaoVirtual):
                robot_name = "nao"
            elif isinstance(self.robot, RomeoVirtual):
                robot_name = "romeo"
            else:
                raise pybullet.error(
                    "Unknown robot type, wont set robot description")

            package_path = roslib.packages.get_pkg_dir("naoqi_driver")
            urdf_path = package_path + "/share/urdf/" + robot_name + ".urdf"

            with open(urdf_path, 'r') as file:
                robot_description = file.read()

            rospy.set_param("/robot_description", robot_description)

        except IOError as e:
            raise pybullet.error(
                "Could not retrieve robot descrition: " + str(e))

        # Launch the robot state publisher
        robot_state_publisher = roslaunch.core.Node(
            "robot_state_publisher",
            "robot_state_publisher")

        self.roslauncher = roslaunch.scriptapi.ROSLaunch()
        self.roslauncher.start()
        self.roslauncher.launch(robot_state_publisher)

        # Initialize the ROS publisher and subscribers
        self._initPublishers()
        self._initSubscribers()

        # Launch the wrapper's main loop
        self._wrapper_termination = False
        self.spin_thread = Thread(target=self._spin)
        self.spin_thread.start()

    def _initPublishers(self):
        """
        ABSTRACT INTERNAL METHOD, needs to be implemented in each daughter
        class. Initializes the ROS publishers
        """
        raise NotImplementedError

    def _initSubscribers(self):
        """
        ABSTRACT INTERNAL METHOD, needs to be implemented in each daughter
        class. Initializes the ROS subscribers
        """
        raise NotImplementedError

    def _spin(self):
        """
        ABSTRACT INTERNAL METHOD, needs to be implemented in each daughter
        class. Designed to emulate a ROS spin method
        """
        raise NotImplementedError

    def _broadcastOdometry(self, odometry_publisher):
        """
        INTERNAL METHOD, computes an odometry message based on the robot's
        position, and broadcast it

        Parameters:
            odometry_publisher - The ROS publisher for the odometry message
        """
        # Send Transform odom
        x, y, theta = self.robot.getPosition()
        odom_trans = TransformStamped()
        odom_trans.header.frame_id = "odom"
        odom_trans.child_frame_id = "base_link"
        odom_trans.header.stamp = rospy.get_rostime()
        odom_trans.transform.translation.x = x
        odom_trans.transform.translation.y = y
        odom_trans.transform.translation.z = 0
        quaternion = pybullet.getQuaternionFromEuler([0, 0, theta])
        odom_trans.transform.rotation.x = quaternion[0]
        odom_trans.transform.rotation.y = quaternion[1]
        odom_trans.transform.rotation.z = quaternion[2]
        odom_trans.transform.rotation.w = quaternion[3]
        self.transform_broadcaster.sendTransform(odom_trans)
        # Set up the odometry
        odom = Odometry()
        odom.header.stamp = rospy.get_rostime()
        odom.header.frame_id = "odom"
        odom.pose.pose.position.x = x
        odom.pose.pose.position.y = y
        odom.pose.pose.position.z = 0.0
        odom.pose.pose.orientation = odom_trans.transform.rotation
        odom.child_frame_id = "base_link"
        [vx, vy, vz], [wx, wy, wz] = pybullet.getBaseVelocity(
            self.robot.getRobotModel(),
            self.robot.getPhysicsClientId())
        odom.twist.twist.linear.x = vx
        odom.twist.twist.linear.y = vy
        odom.twist.twist.angular.z = wz
        odometry_publisher.publish(odom)

    def _broadcastCamera(self, camera, image_publisher, info_publisher):
        """
        INTERNAL METHOD, computes the image message and the info message of the
        given camera and publishes them into the ROS framework

        Parameters:
            camera - The camera used for broadcasting
            image_publisher - The ROS publisher for the Image message,
            corresponding to the image delivered by the active camera
            info_publisher - The ROS publisher for the CameraInfo message,
            corresponding to the parameters of the active camera
        """
        try:
            frame = camera.getFrame()
            assert frame is not None

            # Fill the camera info message
            info_msg = CameraInfo()
            info_msg.distortion_model = "plumb_bob"
            info_msg.header.frame_id = camera.getCameraLink().getName()
            info_msg.width = camera.getResolution().width
            info_msg.height = camera.getResolution().height
            info_msg.D = [0.0, 0.0, 0.0, 0.0, 0.0]
            info_msg.K = camera._getCameraIntrinsics()
            info_msg.R = [1, 0, 0, 0, 1, 0, 0, 0, 1]
            info_msg.P = list(info_msg.K)
            info_msg.P.insert(3, 0.0)
            info_msg.P.insert(7, 0.0)
            info_msg.P.append(0.0)

            # Fill the image message
            image_msg = self.image_bridge.cv2_to_imgmsg(frame)
            image_msg.header.frame_id = camera.getCameraLink().getName()

            # Check if the retrieved image is RGB or a depth image            
            if isinstance(camera, CameraDepth):
                image_msg.encoding = "16UC1"
            else:
                image_msg.encoding = "bgr8"

            # Publish the image and the camera info
            image_publisher.publish(image_msg)
            info_publisher.publish(info_msg)

        except AssertionError:
            pass

    def _broadcastJointState(self, joint_state_publisher, extra_joints=None):
        """
        INTERNAL METHOD, publishes the state of the robot's joints into the ROS
        framework

        Parameters:
            joint_state_publisher - The ROS publisher for the JointState
            message, describing the state of the robot's joints
            extra_joints - A dict, describing extra joints to be published. The
            dict should respect the following syntax:
            {"joint_name": joint_value, ...}
        """
        msg_joint_state = JointState()
        msg_joint_state.header = Header()
        msg_joint_state.header.stamp = rospy.get_rostime()
        msg_joint_state.name = list(self.robot.joint_dict)
        msg_joint_state.position = self.robot.getAnglesPosition(
            msg_joint_state.name)

        try:
            assert isinstance(extra_joints, dict)

            for name, value in extra_joints.items():
                msg_joint_state.name += [name]
                msg_joint_state.position += [value]

        except AssertionError:
            pass

        joint_state_publisher.publish(msg_joint_state)

    def _jointAnglesCallback(self, msg):
        """
        INTERNAL METHOD, callback triggered when a message is received on the
        /joint_angles topic

        Parameters:
            msg - a ROS message containing a pose stamped with a speed
            associated to it. The type of the message is the following:
            naoqi_bridge_msgs::JointAnglesWithSpeed. That type can be found in
            the ros naoqi software stack
        """
        joint_list = msg.joint_names
        position_list = list(msg.joint_angles)

        # If the "non official" driver (softbankrobotics-research fork) is
        # used, will try to detect if multiple speeds have been provided. If
        # not, or if the "official" driver is used, the speed attribute of the
        # message will be used
        try:
            assert not OFFICIAL_DRIVER

            if len(msg.speeds) != 0:
                velocity = list(msg.speeds)
            else:
                velocity = msg.speed

        except AssertionError:
            velocity = msg.speed

        self.robot.setAngles(joint_list, position_list, velocity)


class NaoRosWrapper(RosWrapper):
    """
    Class describing a ROS wrapper for the virtual model of Nao, inheriting
    from the RosWrapperClass
    """
    def __init__(self):
        """
        Constructor
        """
        RosWrapper.__init__(self)

    def launchWrapper(self, virtual_nao, ros_namespace, frequency=200):
        """
        Launches the ROS wrapper for the virtual_nao instance

        Parameters:
            virtual_nao - The instance of the simulated model
            ros_namespace - The ROS namespace to be added before the ROS topics
            advertized and subscribed
            frequency - The frequency of the ROS rate that will be used to pace
            the wrapper's main loop
        """
        RosWrapper.launchWrapper(
            self,
            virtual_nao,
            ros_namespace,
            frequency)

    def _initPublishers(self):
        """
        INTERNAL METHOD, initializes the ROS publishers
        """
        self.front_cam_pub = rospy.Publisher(
            self.ros_namespace + '/camera/front/image_raw',
            Image,
            queue_size=10)

        self.front_info_pub = rospy.Publisher(
            self.ros_namespace + '/camera/front/camera_info',
            CameraInfo,
            queue_size=10)

        self.bottom_cam_pub = rospy.Publisher(
            self.ros_namespace + '/camera/bottom/image_raw',
            Image,
            queue_size=10)

        self.bottom_info_pub = rospy.Publisher(
            self.ros_namespace + '/camera/bottom/camera_info',
            CameraInfo,
            queue_size=10)

        self.joint_states_pub = rospy.Publisher(
            '/joint_states',
            JointState,
            queue_size=10)

        self.odom_pub = rospy.Publisher(
            'odom',
            Odometry,
            queue_size=10)

    def _initSubscribers(self):
        """
        INTERNAL METHOD, initializes the ROS subscribers
        """
        rospy.Subscriber(
            '/joint_angles',
            JointAnglesWithSpeed,
            self._jointAnglesCallback)

    def _broadcastCamera(self):
        """
        INTERNAL METHOD, overloading @_broadcastCamera in RosWrapper
        """
        if self.robot.camera_dict[NaoVirtual.ID_CAMERA_TOP].isActive():
            RosWrapper._broadcastCamera(
                self,
                self.robot.camera_dict[NaoVirtual.ID_CAMERA_TOP],
                self.front_cam_pub,
                self.front_info_pub)

        elif self.robot.camera_dict[NaoVirtual.ID_CAMERA_BOTTOM].isActive():
            RosWrapper._broadcastCamera(
                self,
                self.robot.camera_dict[NaoVirtual.ID_CAMERA_BOTTOM],
                self.bottom_cam_pub,
                self.bottom_info_pub)

    def _broadcastJointState(self, joint_state_publisher):
        """
        INTERNAL METHOD, publishes the state of the robot's joints into the ROS
        framework, overloading @_broadcastJointState in RosWrapper

        Parameters:
            joint_state_publisher - The ROS publisher for the JointState
            message, describing the state of the robot's joints (for API
            consistency)
        """
        RosWrapper._broadcastJointState(self, joint_state_publisher)

    def _spin(self):
        """
        INTERNAL METHOD, designed to emulate a ROS spin method
        """
        rate = rospy.Rate(self.frequency)

        try:
            while not self._wrapper_termination:
                rate.sleep()
                self._broadcastJointState(self.joint_states_pub)
                self._broadcastOdometry(self.odom_pub)
                self._broadcastCamera()

        except Exception as e:
            print("Stopping the ROS wrapper: " + str(e))


class RomeoRosWrapper(RosWrapper):
    """
    Class describing a ROS wrapper for the virtual model of Romeo, inheriting
    from the RosWrapperClass
    """
    def __init__(self):
        """
        Constructor
        """
        RosWrapper.__init__(self)

    def launchWrapper(self, virtual_romeo, ros_namespace, frequency=200):
        """
        Launches the ROS wrapper for the virtual_romeo instance

        Parameters:
            virtual_romeo - The instance of the simulated model
            ros_namespace - The ROS namespace to be added before the ROS topics
            advertized and subscribed
            frequency - The frequency of the ROS rate that will be used to pace
            the wrapper's main loop
        """
        RosWrapper.launchWrapper(
            self,
            virtual_romeo,
            ros_namespace,
            frequency)

    def _initPublishers(self):
        """
        INTERNAL METHOD, initializes the ROS publishers
        """
        self.right_cam_pub = rospy.Publisher(
            self.ros_namespace + '/camera/right/image_raw',
            Image,
            queue_size=10)

        self.right_info_pub = rospy.Publisher(
            self.ros_namespace + '/camera/right/camera_info',
            CameraInfo,
            queue_size=10)

        self.left_cam_pub = rospy.Publisher(
            self.ros_namespace + '/camera/left/image_raw',
            Image,
            queue_size=10)

        self.left_info_pub = rospy.Publisher(
            self.ros_namespace + '/camera/left/camera_info',
            CameraInfo,
            queue_size=10)

        self.depth_cam_pub = rospy.Publisher(
            self.ros_namespace + '/camera/depth/image_raw',
            Image,
            queue_size=10)

        self.depth_info_pub = rospy.Publisher(
            self.ros_namespace + '/camera/depth/camera_info',
            CameraInfo,
            queue_size=10)

        self.joint_states_pub = rospy.Publisher(
            '/joint_states',
            JointState,
            queue_size=10)

        self.odom_pub = rospy.Publisher(
            'odom',
            Odometry,
            queue_size=10)

    def _initSubscribers(self):
        """
        INTERNAL METHOD, initializes the ROS subscribers
        """
        rospy.Subscriber(
            '/joint_angles',
            JointAnglesWithSpeed,
            self._jointAnglesCallback)

    def _broadcastCamera(self):
        """
        INTERNAL METHOD, overloading @_broadcastCamera in RosWrapper
        """
        if self.robot.camera_dict[RomeoVirtual.ID_CAMERA_RIGHT].isActive():
            RosWrapper._broadcastCamera(
                self,
                self.robot.camera_dict[RomeoVirtual.ID_CAMERA_RIGHT],
                self.right_cam_pub,
                self.right_info_pub)

        elif self.robot.camera_dict[RomeoVirtual.ID_CAMERA_LEFT].isActive():
            RosWrapper._broadcastCamera(
                self,
                self.robot.camera_dict[RomeoVirtual.ID_CAMERA_LEFT],
                self.left_cam_pub,
                self.left_info_pub)

        elif self.robot.camera_dict[RomeoVirtual.ID_CAMERA_DEPTH].isActive():
            RosWrapper._broadcastCamera(
                self,
                self.robot.camera_dict[RomeoVirtual.ID_CAMERA_DEPTH],
                self.depth_cam_pub,
                self.depth_info_pub)

    def _broadcastJointState(self, joint_state_publisher):
        """
        INTERNAL METHOD, publishes the state of the robot's joints into the ROS
        framework, overloading @_broadcastJointState in RosWrapper

        Parameters:
            joint_state_publisher - The ROS publisher for the JointState
            message, describing the state of the robot's joints (for API
            consistency)
        """
        RosWrapper._broadcastJointState(self, joint_state_publisher)

    def _spin(self):
        """
        INTERNAL METHOD, designed to emulate a ROS spin method
        """
        rate = rospy.Rate(self.frequency)

        try:
            while not self._wrapper_termination:
                rate.sleep()
                self._broadcastJointState(self.joint_states_pub)
                self._broadcastOdometry(self.odom_pub)
                self._broadcastCamera()

        except Exception as e:
            print("Stopping the ROS wrapper: " + str(e))


class PepperRosWrapper(RosWrapper):
    """
    Class describing a ROS wrapper for the virtual model of Pepper, inheriting
    from the RosWrapperClass
    """

    def __init__(self):
        """
        Constructor
        """
        RosWrapper.__init__(self)

    def launchWrapper(self, virtual_pepper, ros_namespace, frequency=200):
        """
        Launches the ROS wrapper for the virtual_pepper instance

        Parameters:
            virtual_pepper - The instance of the simulated model
            ros_namespace - The ROS namespace to be added before the ROS topics
            advertized and subscribed
            frequency - The frequency of the ROS rate that will be used to pace
            the wrapper's main loop
        """
        RosWrapper.launchWrapper(
            self,
            virtual_pepper,
            ros_namespace,
            frequency)

    def _initPublishers(self):
        """
        INTERNAL METHOD, initializes the ROS publishers
        """
        self.front_cam_pub = rospy.Publisher(
            self.ros_namespace + '/camera/front/image_raw',
            Image,
            queue_size=10)

        self.front_info_pub = rospy.Publisher(
            self.ros_namespace + '/camera/front/camera_info',
            CameraInfo,
            queue_size=10)

        self.bottom_cam_pub = rospy.Publisher(
            self.ros_namespace + '/camera/bottom/image_raw',
            Image,
            queue_size=10)

        self.bottom_info_pub = rospy.Publisher(
            self.ros_namespace + '/camera/bottom/camera_info',
            CameraInfo,
            queue_size=10)

        self.depth_cam_pub = rospy.Publisher(
            self.ros_namespace + '/camera/depth/image_raw',
            Image,
            queue_size=10)

        self.depth_info_pub = rospy.Publisher(
            self.ros_namespace + '/camera/depth/camera_info',
            CameraInfo,
            queue_size=10)

        self.laser_pub = rospy.Publisher(
            self.ros_namespace + "/laser",
            LaserScan,
            queue_size=10)

        self.joint_states_pub = rospy.Publisher(
            '/joint_states',
            JointState,
            queue_size=10)

        self.odom_pub = rospy.Publisher(
            '/naoqi_driver/odom',
            Odometry,
            queue_size=10)

    def _initSubscribers(self):
        """
        INTERNAL METHOD, initializes the ROS subscribers
        """
        rospy.Subscriber(
            '/joint_angles',
            JointAnglesWithSpeed,
            self._jointAnglesCallback)

        rospy.Subscriber(
            '/cmd_vel',
            Twist,
            self._velocityCallback)

        rospy.Subscriber(
            '/move_base_simple/goal',
            MovetoPose,
            self._moveToCallback)

        rospy.Subscriber(
            '/move_base_simple/cancel',
            Empty,
            self._killMoveCallback)

    def _broadcastLasers(self, laser_publisher):
        """
        INTERNAL METHOD, publishes the laser values in the ROS framework

        Parameters:
            laser_publisher - The ROS publisher for the LaserScan message,
            corresponding to the laser info of the pepper robot (for API
            consistency)
        """
        if not self.robot.laser_manager.isActive():
            return

        scan = LaserScan()
        scan.header.stamp = rospy.get_rostime()
        scan.header.frame_id = "base_footprint"
        # -120 degres, 120 degres
        scan.angle_min = -2.0944
        scan.angle_max = 2.0944

        # 240 degres FoV, 61 points (blind zones inc)
        scan.angle_increment = (2 * 2.0944) / (15.0 + 15.0 + 15.0 + 8.0 + 8.0)

        # Detection ranges for the lasers in meters, 0.1 to 3.0 meters
        scan.range_min = 0.1
        scan.range_max = 3.0

        # Fill the lasers information
        right_scan = self.robot.getRightLaserValue()
        front_scan = self.robot.getFrontLaserValue()
        left_scan = self.robot.getLeftLaserValue()

        if isinstance(right_scan, list):
            scan.ranges.extend(list(reversed(right_scan)))
            scan.ranges.extend([-1]*8)
        if isinstance(front_scan, list):
            scan.ranges.extend(list(reversed(front_scan)))
            scan.ranges.extend([-1]*8)
        if isinstance(left_scan, list):
            scan.ranges.extend(list(reversed(left_scan)))

        laser_publisher.publish(scan)

    def _broadcastCamera(self):
        """
        INTERNAL METHOD, overloading @_broadcastCamera in RosWrapper
        """
        if self.robot.camera_dict[PepperVirtual.ID_CAMERA_TOP].isActive():
            RosWrapper._broadcastCamera(
                self,
                self.robot.camera_dict[PepperVirtual.ID_CAMERA_TOP],
                self.front_cam_pub,
                self.front_info_pub)

        elif self.robot.camera_dict[PepperVirtual.ID_CAMERA_BOTTOM].isActive():
            RosWrapper._broadcastCamera(
                self,
                self.robot.camera_dict[PepperVirtual.ID_CAMERA_BOTTOM],
                self.bottom_cam_pub,
                self.bottom_info_pub)

        elif self.robot.camera_dict[PepperVirtual.ID_CAMERA_DEPTH].isActive():
            RosWrapper._broadcastCamera(
                self,
                self.robot.camera_dict[PepperVirtual.ID_CAMERA_DEPTH],
                self.depth_cam_pub,
                self.depth_info_pub)

    def _broadcastJointState(self, joint_state_publisher):
        """
        INTERNAL METHOD, publishes the state of the robot's joints into the ROS
        framework, overloading @_broadcastJointState in RosWrapper

        Parameters:
            joint_state_publisher - The ROS publisher for the JointState
            message, describing the state of the robot's joints (for API
            consistency)
        """
        RosWrapper._broadcastJointState(
            self,
            joint_state_publisher,
            extra_joints={"WheelFL": 0.0, "WheelFR": 0.0, "WheelB": 0.0})

    def _velocityCallback(self, msg):
        """
        INTERNAL METHOD, callback triggered when a message is received on the
        /cmd_vel topic

        Parameters:
            msg - a ROS message containing a Twist command
        """
        self.robot.move(msg.linear.x, msg.linear.y, msg.angular.z)

    def _moveToCallback(self, msg):
        """
        INTERNAL METHOD, callback triggered when a message is received on the
        '/move_base_simple/goal' topic. It allows to move the robot's base

        Parameters:
            msg - a ROS message containing a pose stamped with a speed, or a
            simple pose stamped (depending on which version of the naoqi_driver
            is used, the "official" one from ros-naoqi or the "non official"
            softbankrobotics-research fork). The type of the message is the
            following: geometry_msgs::PoseStamped for the "official",
            naoqi_bridge_msgs::PoseStampedWithSpeed for the "non-official".
            An alias is given to the message type: MovetoPose
        """

        if OFFICIAL_DRIVER:
            pose = msg.pose
            frame = 0
            frame_id = msg.header.frame_id
            speed = None
        else:
            pose = msg.pose_stamped.pose
            frame = msg.referenceFrame
            frame_id = msg.pose_stamped.header.frame_id
            speed = msg.speed_percentage *\
                PepperBaseController.MAX_LINEAR_VELOCITY +\
                PepperBaseController.MIN_LINEAR_VELOCITY

        try:
            assert frame not in [
                PepperVirtual.FRAME_ROBOT,
                PepperVirtual.FRAME_WORLD]

            if frame_id == "odom":
                frame = PepperVirtual.FRAME_WORLD
            elif frame_id == "base_footprint":
                frame = PepperVirtual.FRAME_ROBOT
            else:
                raise pybullet.error(
                    "Incorrect reference frame for move_base_simple, please "
                    "modify the content of your message")

        except AssertionError:
            pass

        x = pose.position.x
        y = pose.position.y
        theta = pybullet.getEulerFromQuaternion([
            pose.orientation.x,
            pose.orientation.y,
            pose.orientation.z,
            pose.orientation.w])[-1]

        self.robot.moveTo(
            x,
            y,
            theta,
            frame=frame,
            speed=speed,
            _async=True)

    def _killMoveCallback(self, msg):
        """
        INTERNAL METHOD, callback triggered when a message is received on the
        '/move_base_simple/cancel' topic. This callback is used to stop the
        robot's base from moving

        Parameters:
            msg - an empty ROS message, with the Empty type
        """
        self.robot.moveTo(0, 0, 0, _async=True)

    def _spin(self):
        """
        INTERNAL METHOD, designed to emulate a ROS spin method
        """
        rate = rospy.Rate(self.frequency)

        try:
            while not self._wrapper_termination:
                rate.sleep()
                self._broadcastJointState(self.joint_states_pub)
                self._broadcastOdometry(self.odom_pub)
                self._broadcastLasers(self.laser_pub)
                self._broadcastCamera()

        except Exception as e:
            print("Stopping the ROS wrapper: " + str(e))
