def fk_service_client(limb = "right"):
    ns = "ExternalTools/" + limb + "/PositionKinematicsNode/FKService"
    fksvc = rospy.ServiceProxy(ns, SolvePositionFK)
    fkreq = SolvePositionFKRequest()
    joints = JointState()
    joints.name = ['right_j0', 'right_j1', 'right_j2', 'right_j3',
                   'right_j4', 'right_j5', 'right_j6']
    joints.position = [0.763331, 0.415979, -1.728629, 1.482985,
                       -1.135621, -1.674347, -0.496337]
    # Add desired pose for forward kinematics
    fkreq.configuration.append(joints)
    # Request forward kinematics from base to "right_hand" link
    fkreq.tip_names.append('right_hand')

    try:
        rospy.wait_for_service(ns, 5.0)
        resp = fksvc(fkreq)
    except (rospy.ServiceException, rospy.ROSException), e:
        rospy.logerr("Service call failed: %s" % (e,))
        return False

    # Check if result valid 

def get_endeffector_pos(self):
        fkreq = SolvePositionFKRequest()
        joints = JointState()
        joints.name = self._limb_right.joint_names()
        joints.position = [self._limb_right.joint_angle(j)
                           for j in joints.name]

        # Add desired pose for forward kinematics
        fkreq.configuration.append(joints)
        fkreq.tip_names.append('right_hand')
        try:
            rospy.wait_for_service(self.name_of_service, 5)
            resp = self.fksvc(fkreq)
        except (rospy.ServiceException, rospy.ROSException), e:
            rospy.logerr("Service call failed: %s" % (e,))
            return False 

def get_object_relative_pose(self, obj_type="SQUARE", obj_color="BLUE", ret_image=True):
        service_name = self.__services_name['object_pose_service']
        rospy.wait_for_service(service_name)

        try:
            detection_service = rospy.ServiceProxy(service_name, ObjDetection)
            response = detection_service(obj_type=obj_type, obj_color=obj_color,
                                         workspace_ratio=25. / 16, ret_image=ret_image)
            ret_status = self.__status_interpreter_obj_detection[response.status]
            if ret_status != "SUCCESSFUL":
                print "Object not detected: " + ret_status

            msg_res = response.obj_pose
            obj_type = response.obj_type
            obj_color = response.obj_color

            if ret_image:
                im_ret = extract_img_from_ros_msg(response.img)
            else:
                im_ret = None
            return ret_status, msg_res, obj_type, obj_color, im_ret

        except rospy.ServiceException, e:
            print "Service call failed: %s" % e 

def call_service(self, service_name, service_msg_type, args):
            """
            Wait for the service called service_name
            Then call the service with args

            :param service_name:
            :param service_msg_type:
            :param args: Tuple of arguments
            :raises NiryoOneException: Timeout during waiting of services
            :return: Response
            """

            # Connect to service
            try:
                rospy.wait_for_service(service_name, self.service_timeout)
            except rospy.ROSException, e:
                raise NiryoOneException(e)

            # Call service 

def __init__(self):
        rospy.wait_for_service('niryo_one/tools/ping_and_set_dxl_tool')
        rospy.wait_for_service('niryo_one/tools/open_gripper')
        rospy.wait_for_service('niryo_one/tools/close_gripper')
        rospy.wait_for_service('niryo_one/tools/pull_air_vacuum_pump')
        rospy.wait_for_service('niryo_one/tools/push_air_vacuum_pump')

        self.service_ping_dxl_tool = rospy.ServiceProxy('niryo_one/tools/ping_and_set_dxl_tool', PingDxlTool)

        self.service_open_gripper = rospy.ServiceProxy('niryo_one/tools/open_gripper', OpenGripper)
        self.service_close_gripper = rospy.ServiceProxy('niryo_one/tools/close_gripper', CloseGripper)

        self.service_pull_air_vacuum_pump = rospy.ServiceProxy('niryo_one/tools/pull_air_vacuum_pump',
                                                               PullAirVacuumPump)
        self.service_push_air_vacuum_pump = rospy.ServiceProxy('niryo_one/tools/push_air_vacuum_pump',
                                                               PushAirVacuumPump)

        self.service_setup_digital_output_tool = rospy.ServiceProxy('niryo_one/rpi/set_digital_io_mode', SetDigitalIO)
        self.service_activate_digital_output_tool = rospy.ServiceProxy('niryo_one/rpi/set_digital_io_state',
                                                                       SetDigitalIO)

        rospy.loginfo("Interface between Tools Controller and Ros Control has been started.") 

def set_dxl_leds(color):
        leds = [0, 0, 0, 8]  # gripper LED will not be used
        if color == LED_RED:
            leds = [1, 1, 1, 8]
        elif color == LED_GREEN:
            leds = [2, 2, 2, 8]
        elif color == LED_BLUE:
            leds = [4, 4, 4, 8]
        # 4 is yellow, no yellow
        elif color == LED_BLUE_GREEN:
            leds = [6, 6, 6, 8]
        elif color == LED_PURPLE:
            leds = [5, 5, 5, 8]
        elif color == LED_WHITE:
            leds = [7, 7, 7, 8]

        try:
            rospy.wait_for_service('/niryo_one/set_dxl_leds', timeout=1)
        except rospy.ROSException, e:
            rospy.logwarn("Niryo ROS control LED service is not up!") 

def execute(self, inputs, outputs, gvm):
    turtle_name = inputs["turtle_name"]
    x = inputs["x_pos"]
    y = inputs["y_pos"]
    phi = inputs["phi"]

    service = "/spawn"
    rospy.wait_for_service(service)
    spawn_turtle_service = rospy.ServiceProxy(service, Spawn)
    resp1 = spawn_turtle_service(x, y, phi, turtle_name)
    self.logger.info("ROS external module: executed the {} service".format(service))

    turtle_pos_subscriber = rospy.Subscriber("/" + turtle_name + "/pose", Pose, check_turtle_pose)

    r = rospy.Rate(10)
    global pose_received
    # wait until the first pose message was received
    while not pose_received:
        r.sleep()

    return 0 

def __init__(self, configs):
        self.configs = configs
        self.ROT_VEL = self.configs.BASE.MAX_ABS_TURN_SPEED
        self.LIN_VEL = self.configs.BASE.MAX_ABS_FWD_SPEED
        self.MAP_FRAME = self.configs.BASE.MAP_FRAME
        self.BASE_FRAME = self.configs.BASE.VSLAM.VSLAM_BASE_FRAME
        self.point_idx = self.configs.BASE.TRACKED_POINT

        rospy.wait_for_service(self.configs.BASE.PLAN_TOPIC, timeout=3)
        try:
            self.plan_srv = rospy.ServiceProxy(self.configs.BASE.PLAN_TOPIC, GetPlan)
        except rospy.ServiceException:
            rospy.logerr(
                "Timed out waiting for the planning service. \
                    Make sure build_map in script and \
                           use_map in roslauch are set to the same value"
            )
        self.start_state = build_pose_msg(0, 0, 0, self.BASE_FRAME)
        self.tolerance = self.configs.BASE.PLAN_TOL
        self._transform_listener = tf.TransformListener() 

def RequestDMP(u,dt,k_gain,d_gain,num_basis_functions):

    ndims = len(u[0])
    k_gains = [k_gain]*ndims
    d_gains = [d_gain]*ndims
    
    ex = DMPTraj()
    
    for i in range(len(u)):
        pt = DMPPoint()
        pt.positions = u[i] # always sends positions regardless of actual content
        ex.points.append(pt)
        ex.times.append(dt * i) # make sure times are reasonable

    rospy.wait_for_service('learn_dmp_from_demo')

    try:
        lfd = rospy.ServiceProxy('learn_dmp_from_demo', LearnDMPFromDemo)
        resp = lfd(ex, k_gains, d_gains, num_basis_functions)
    except rospy.ServiceException, e:
        print "Service call failed: %s"%e 

def __call__(self, **kwargs):
            rospy.loginfo('will call service {}'.format(self.service_name))
            from tts.srv import Polly
            rospy.wait_for_service(self.service_name)
            polly = rospy.ServiceProxy(self.service_name, Polly)
            return polly(polly_action='SynthesizeSpeech', **kwargs) 

def test_plain_text_to_wav_via_polly_node(self):
        rospy.wait_for_service('polly')
        polly = rospy.ServiceProxy('polly', Polly)

        test_text = 'Mary has a little lamb, little lamb, little lamb.'
        res = polly(polly_action='SynthesizeSpeech', text=test_text)
        self.assertIsNotNone(res)
        self.assertTrue(type(res) is PollyResponse)

        r = json.loads(res.result)
        self.assertIn('Audio Type', r, 'result should contain audio type')
        self.assertIn('Audio File', r, 'result should contain file path')
        self.assertIn('Amazon Polly Response Metadata', r, 'result should contain metadata')

        audio_type = r['Audio Type']
        audio_file = r['Audio File']
        md = r['Amazon Polly Response Metadata']
        self.assertTrue("'HTTPStatusCode': 200," in md)
        self.assertEqual('audio/ogg', audio_type)
        self.assertTrue(audio_file.endswith('.ogg'))

        import subprocess
        o = subprocess.check_output(['file', audio_file], stderr=subprocess.STDOUT)
        import re
        m = re.search(r'.*Ogg data, Vorbis audi.*', o, flags=re.MULTILINE)
        self.assertIsNotNone(m) 

def test_plain_text_via_synthesizer_node(self):
        rospy.wait_for_service('synthesizer')
        speech_synthesizer = rospy.ServiceProxy('synthesizer', Synthesizer)

        text = 'Mary has a little lamb, little lamb, little lamb.'
        res = speech_synthesizer(text=text)
        self.assertIsNotNone(res)
        self.assertTrue(type(res) is SynthesizerResponse)

        r = json.loads(res.result)
        self.assertIn('Audio Type', r, 'result should contain audio type')
        self.assertIn('Audio File', r, 'result should contain file path')
        self.assertIn('Amazon Polly Response Metadata', r, 'result should contain metadata')

        audio_type = r['Audio Type']
        audio_file = r['Audio File']
        md = r['Amazon Polly Response Metadata']
        self.assertTrue("'HTTPStatusCode': 200," in md)
        self.assertEqual('audio/ogg', audio_type)
        self.assertTrue(audio_file.endswith('.ogg'))

        import subprocess
        o = subprocess.check_output(['file', audio_file], stderr=subprocess.STDOUT)
        import re
        m = re.search(r'.*Ogg data, Vorbis audi.*', o, flags=re.MULTILINE)
        self.assertIsNotNone(m) 

def test_plain_text_to_mp3_via_polly_node(self):
        rospy.wait_for_service('polly')
        polly = rospy.ServiceProxy('polly', Polly)

        test_text = 'Mary has a little lamb, little lamb, little lamb.'
        res = polly(polly_action='SynthesizeSpeech', text=test_text, output_format='mp3')
        self.assertIsNotNone(res)
        self.assertTrue(type(res) is PollyResponse)

        r = json.loads(res.result)
        self.assertIn('Audio Type', r, 'result should contain audio type')
        self.assertIn('Audio File', r, 'result should contain file path')
        self.assertIn('Amazon Polly Response Metadata', r, 'result should contain metadata')

        audio_type = r['Audio Type']
        audio_file = r['Audio File']
        md = r['Amazon Polly Response Metadata']
        self.assertTrue("'HTTPStatusCode': 200," in md)
        self.assertEqual('audio/mpeg', audio_type)
        self.assertTrue(audio_file.endswith('.mp3'))

        import subprocess
        o = subprocess.check_output(['file', audio_file], stderr=subprocess.STDOUT)
        import re
        m = re.search(r'.*MPEG.*layer III.*', o, flags=re.MULTILINE)
        self.assertIsNotNone(m) 

def test_simple_ssml_via_polly_node(self):
        rospy.wait_for_service('polly')
        polly = rospy.ServiceProxy('polly', Polly)

        text = '<speak>Mary has a little lamb, little lamb, little lamb.</speak>'
        res = polly(polly_action='SynthesizeSpeech', text=text, text_type='ssml')
        self.assertIsNotNone(res)
        self.assertTrue(type(res) is PollyResponse)

        r = json.loads(res.result)
        self.assertIn('Audio Type', r, 'result should contain audio type')
        self.assertIn('Audio File', r, 'result should contain file path')
        self.assertIn('Amazon Polly Response Metadata', r, 'result should contain metadata')

        audio_type = r['Audio Type']
        audio_file = r['Audio File']
        md = r['Amazon Polly Response Metadata']
        self.assertTrue("'HTTPStatusCode': 200," in md)
        self.assertEqual('audio/ogg', audio_type)
        self.assertTrue(audio_file.endswith('.ogg'))

        import subprocess
        o = subprocess.check_output(['file', audio_file], stderr=subprocess.STDOUT)
        import re
        m = re.search(r'.*Ogg data, Vorbis audi.*', o, flags=re.MULTILINE)
        self.assertIsNotNone(m) 

def do_synthesize(goal):
    """calls synthesizer service to do the job"""
    rospy.wait_for_service('synthesizer')
    synthesize = rospy.ServiceProxy('synthesizer', Synthesizer)
    return synthesize(goal.text, goal.metadata) 

def spawn_urdf(name, description_xml, pose, reference_frame):
    rospy.wait_for_service('/gazebo/spawn_urdf_model')
    try:
        spawn_urdf = rospy.ServiceProxy('/gazebo/spawn_urdf_model', SpawnModel)
        resp_urdf = spawn_urdf(name, description_xml, "/", pose, reference_frame)
    except rospy.ServiceException as e:
        rospy.logerr("Spawn URDF service call failed: {0}".format(e)) 

def spawn_sdf(name, description_xml, pose, reference_frame):
    rospy.wait_for_service('/gazebo/spawn_sdf_model')
    try:
        spawn_sdf = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel)
        resp_sdf = spawn_sdf(name, description_xml, "/", pose, reference_frame)
    except rospy.ServiceException as e:
        rospy.logerr("Spawn SDF service call failed: {0}".format(e)) 

def start_server(limb, rate, mode, valid_limbs):
    rospy.loginfo("Initializing node... ")
    rospy.init_node("sdk_{0}_joint_trajectory_action_server{1}".format(
                        mode, "" if limb == 'all_limbs' else "_" + limb,))

    rospy.wait_for_service("/ExternalTools/right/PositionKinematicsNode/IKService")

    rospy.loginfo("Initializing joint trajectory action server...")
    robot_name = intera_interface.RobotParams().get_robot_name().lower().capitalize()
    config_module = "intera_interface.cfg"
    if mode == 'velocity':
        config_name = ''.join([robot_name,"VelocityJointTrajectoryActionServerConfig"])
    elif mode == 'position':
        config_name = ''.join([robot_name,"PositionJointTrajectoryActionServerConfig"])
    else:
        config_name = ''.join([robot_name,"PositionFFJointTrajectoryActionServerConfig"])
    cfg = importlib.import_module('.'.join([config_module,config_name]))
    dyn_cfg_srv = Server(cfg, lambda config, level: config)
    jtas = []
    if limb == 'all_limbs':
        for current_limb in valid_limbs:
            jtas.append(JointTrajectoryActionServer(current_limb, dyn_cfg_srv,
                                                    rate, mode))
    else:
        jtas.append(JointTrajectoryActionServer(limb, dyn_cfg_srv, rate, mode))


    def cleanup():
        for j in jtas:
            j.clean_shutdown()

    rospy.on_shutdown(cleanup)
    rospy.loginfo("Joint Trajectory Action Server Running. Ctrl-c to quit")
    rospy.spin() 

def set_start(self, x, y, theta):
        state = ModelState()
        state.model_name = 'robot'
        state.reference_frame = 'world'  # ''ground_plane'
        # pose
        state.pose.position.x = x
        state.pose.position.y = y
        state.pose.position.z = 0
        quaternion = tf.transformations.quaternion_from_euler(0, 0, theta)
        state.pose.orientation.x = quaternion[0]
        state.pose.orientation.y = quaternion[1]
        state.pose.orientation.z = quaternion[2]
        state.pose.orientation.w = quaternion[3]
        # twist
        state.twist.linear.x = 0
        state.twist.linear.y = 0
        state.twist.linear.z = 0
        state.twist.angular.x = 0
        state.twist.angular.y = 0
        state.twist.angular.z = 0

        rospy.wait_for_service('/gazebo/set_model_state')
        try:
            set_state = self.set_state
            result = set_state(state)
            assert result.success is True
        except rospy.ServiceException:
            print("/gazebo/get_model_state service call failed") 

def set_goal(self, x, y):
        state = ModelState()
        state.model_name = 'goal'
        state.reference_frame = 'world'  # ''ground_plane'
        state.pose.position.x = x
        state.pose.position.y = y
        state.pose.position.z = 0.1

        rospy.wait_for_service('/gazebo/set_model_state')
        try:
            set_state = self.set_state
            result = set_state(state)
            assert result.success is True
        except rospy.ServiceException:
            print("/gazebo/get_model_state service call failed") 

def set_start(x, y, theta):
    state = ModelState()
    state.model_name = 'robot'
    state.reference_frame = 'world'  # ''ground_plane'
    # pose
    state.pose.position.x = x
    state.pose.position.y = y
    state.pose.position.z = 0
    quaternion = tf.transformations.quaternion_from_euler(0, 0, theta)
    state.pose.orientation.x = quaternion[0]
    state.pose.orientation.y = quaternion[1]
    state.pose.orientation.z = quaternion[2]
    state.pose.orientation.w = quaternion[3]
    # twist
    state.twist.linear.x = 0
    state.twist.linear.y = 0
    state.twist.linear.z = 0
    state.twist.angular.x = 0
    state.twist.angular.y = 0
    state.twist.angular.z = 0

    rospy.wait_for_service('/gazebo/set_model_state')
    try:
        set_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
        result = set_state(state)
        assert result.success is True
        print("set the model state successfully")
    except rospy.ServiceException:
        print("/gazebo/get_model_state service call failed") 

def take_sim_step(self):
        """
        Executing one simulation step of 0.1 sec
        """
        msg = Float64()
        msg.data = self.__update_rate
        rospy.wait_for_service('%s/step' % self.NS)
        self.__sim_step(msg)
        return 

def set_robot_pos(self, x, y, theta):
        """
        Move robot to position (x, y, theta) in simulation
        :param x x-position of the robot
        :param y y-position of the robot
        :param theta theta-position of the robot
        """
        pose = Pose2D()
        pose.x = x
        pose.y = y
        pose.theta = theta
        rospy.wait_for_service('%s/move_model' % self.NS)
        self.__move_robot_to('robot_1', pose)
        self.take_sim_step()
        self.__pub_initial_position(x, y, theta) 

def respawn_static_objects(self, models):
        """
        Respawning a new scene of static objects. Objects from previous tasks are reused
        and simply removed to the appropriate object position.
        More efficient, because several models are spawned with one service call.
        :param  models list of models
        """
        old_model_names = []
        for old_model in self.__static_objects:
            old_model_names.append(old_model.name)
        rospy.wait_for_service('%s/respawn_models' % self.NS)
        try:
            self.__respawn_models.call(old_model_names, models)
        except (TypeError):
            print('Spawn object: TypeError.')
            return
        except (rospy.ServiceException):
            print('Spawn object: rospy.ServiceException. Closing serivce')
            try:
                self.__spawn_model.close()
            except AttributeError:
                print('Spawn object close(): AttributeError.')
                return
            return
        except AttributeError:
            print('Spawn object: AttributeError.')
            return
        self.__static_objects = models 

def remove_all_static_objects(self):
        """
         Removes all static objects, that has been spawned so far
         """
        for i in self.__static_objects:
            srv = DeleteModel()
            srv.name = i.name
            rospy.wait_for_service('%s/delete_model' % self.NS)
            ret = self.__delete_model.call(srv.name)

        self.__static_objects = [] 

def __remove_all_peds(self):
        """
        Removes all pedestrians, that has been spawned so far
        """
        srv = SetBool()
        srv.data = True
        rospy.wait_for_service('%s/pedsim_simulator/remove_all_peds' % self.NS)
        self.__remove_all_peds_srv.call(srv.data)
        self.__peds = []
        return 

def __respawn_peds(self, peds):
        """
        Spawning one pedestrian in the simulation.
        :param  start_pos start position of the pedestrian.
        :param  wps waypoints the pedestrian is supposed to walk to.
        :param  id id of the pedestrian.
        """
        srv = SpawnPeds()
        srv.peds = []
        for ped in peds:
            msg = Ped()
            msg.id = ped[0]
            msg.pos = Point()
            msg.pos.x = ped[1][0]
            msg.pos.y = ped[1][1]
            msg.pos.z = ped[1][2]
            msg.type = self.__ped_type
            msg.number_of_peds = 1
            msg.yaml_file = self.__ped_file
            msg.waypoints = []
            for pos in ped[2]:
                p = Point()
                p.x = pos[0]
                p.y = pos[1]
                p.z = pos[2]
                msg.waypoints.append(p)
            srv.peds.append(msg)

        rospy.wait_for_service('%s/pedsim_simulator/respawn_peds' % self.NS)
        try:
            # self.__spawn_ped_srv.call(srv.peds)
            self.__respawn_peds_srv.call(srv.peds)
        except rospy.ServiceException:
            print('Spawn object: rospy.ServiceException. Closing serivce')
            try:
                self.__spawn_model.close()
            except AttributeError:
                print('Spawn object close(): AttributeError.')
                return
        self.__peds = peds
        return 

def get_model_pose(model_name, relative_entity_name='world'):
    rospy.wait_for_service('gazebo/get_model_state')
    try:
        get_model_state = rospy.ServiceProxy('gazebo/get_model_state', gazebo_msgs.srv.GetModelState)
        res = get_model_state(model_name, relative_entity_name)
        return res.pose
    except rospy.ServiceException as e:
        print("Service call failed: %s"%e) 

def set_model_pose(model_name, model_pose, relative_entity_name='world'):
    rospy.wait_for_service('gazebo/set_model_state')
    try:
        set_model_state = rospy.ServiceProxy('gazebo/set_model_state', gazebo_msgs.srv.SetModelState)
        model_state = gazebo_msgs.msg.ModelState()
        model_state.model_name = model_name
        model_state.pose = model_pose
        model_state.reference_frame = relative_entity_name
        set_model_state(model_state)
    except rospy.ServiceException as e:
        print("Service call failed: %s"%e) 

def update_current_pose_from_gazebo(self):
        rospy.wait_for_service('/gazebo/get_model_state')
        loc = self.get_model_state(self.robot_model_name,'')

        qtn=loc.pose.orientation
        roll,pitch,yaw=quaternion_to_euler_angle(qtn.w, qtn.x, qtn.y, qtn.z)
        self.current_pose = Pose2d(theta=yaw, x=loc.pose.position.x, y=loc.pose.position.y)