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 fk_request(self, joint_angles,
                   end_point='right_hand'):
        """
        Forward Kinematics request sent to FK Service

        @type joint_angles: dict({str:float})
        @param joint_angles: the arm's joint positions
        @type end_point: string
        @param end_point: name of the end point (should be in URDF)
        @return: Forward Kinematics response from FK service
        """
        fkreq = SolvePositionFKRequest()
        # Add desired pose for forward kinematics
        joints = JointState()
        joints.name = joint_angles.keys()
        joints.position = joint_angles.values()
        fkreq.configuration.append(joints)
        # Request forward kinematics from base to end_point
        fkreq.tip_names.append(end_point)
        try:
            resp = self._fksvc(fkreq)
        except (rospy.ServiceException, rospy.ROSException), e:
            rospy.logerr("FK Service call failed: %s" % (e,))
            return False 

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 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 call_service(self, req):
        """Calling the appropriate service depending on the given request type.
        Requests have to inherit from 'HMMRepRequestAbstractclass'.

        :param req: The request class instance for the request you want to make

        :return: The qsr_type and resulting data tuple. The data is in the data type produced by the response.
        """
        assert(issubclass(req.__class__, RepRequestAbstractclass))
        s = rospy.ServiceProxy(self.services[req.__class__],QSRProbRep)
        try:
            s.wait_for_service(timeout=10.)
            res = s(QSRProbRepRequest(data=json.dumps(req.kwargs)))
        except (rospy.ROSException, rospy.ROSInterruptException, rospy.ServiceException) as e:
            rospy.logerr(e)
            return None
        try:
            data = json.loads(res.data)
        except ValueError:
            data = str(res.data)
        return data 

def run(self):
        """
        main loop
        """
        # wait for ros-bridge to set up CARLA world
        rospy.loginfo("Waiting for CARLA world (topic: /carla/world_info)...")
        try:
            rospy.wait_for_message("/carla/world_info", CarlaWorldInfo, timeout=10.0)
        except rospy.ROSException as e:
            rospy.logerr("Timeout while waiting for world info!")
            raise e
        rospy.loginfo("CARLA world available. Spawn infrastructure...")
        client = carla.Client(self.host, self.port)
        client.set_timeout(self.timeout)
        self.world = client.get_world()
        self.restart()
        try:
            rospy.spin()
        except rospy.ROSInterruptException:
            pass

# ==============================================================================
# -- main() --------------------------------------------------------------------
# ============================================================================== 

def start(self):
        """ Start the sensor """
        # initialize subscribers
        self._image_sub = rospy.Subscriber(self.topic_image_color, sensor_msgs.msg.Image, self._color_image_callback)
        self._depth_sub = rospy.Subscriber(self.topic_image_depth, sensor_msgs.msg.Image, self._depth_image_callback)
        self._camera_info_sub = rospy.Subscriber(self.topic_info_camera, sensor_msgs.msg.CameraInfo, self._camera_info_callback)
        
        timeout = 10
        try:
            rospy.loginfo("waiting to recieve a message from the Kinect")
            rospy.wait_for_message(self.topic_image_color, sensor_msgs.msg.Image, timeout=timeout)
            rospy.wait_for_message(self.topic_image_depth, sensor_msgs.msg.Image, timeout=timeout)
            rospy.wait_for_message(self.topic_info_camera, sensor_msgs.msg.CameraInfo, timeout=timeout)
        except rospy.ROSException as e:
            print("KINECT NOT FOUND")
            rospy.logerr("Kinect topic not found, Kinect not started")
            rospy.logerr(e)

        while self._camera_intr is None:
            time.sleep(0.1)
        
        self._running = True 

def call_ros_service(service_name, service_msg_type, args):
        # Connect to service
        try:
            rospy.wait_for_service(service_name, 0.1)
        except rospy.ROSException, e:
            return

            # Call service 

def get_forward_kinematic(joints):
    try:
        rospy.wait_for_service('compute_fk', 2)
    except (rospy.ServiceException, rospy.ROSException) as e:
        rospy.logerr("Service call failed:", e)
        return None
    try:
        moveit_fk = rospy.ServiceProxy('compute_fk', GetPositionFK)
        fk_link = ['base_link', 'tool_link']
        joint_names = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6']
        header = Header(0, rospy.Time.now(), "/world")
        rs = RobotState()
        rs.joint_state.name = joint_names
        rs.joint_state.position = joints
        response = moveit_fk(header, fk_link, rs)
    except rospy.ServiceException as e:
        rospy.logerr("Service call failed:", e)
        return None

    quaternion = [response.pose_stamped[1].pose.orientation.x, response.pose_stamped[1].pose.orientation.y,
                  response.pose_stamped[1].pose.orientation.z, response.pose_stamped[1].pose.orientation.w]
    rpy = get_rpy_from_quaternion(quaternion)
    quaternion = Position.Quaternion(round(quaternion[0], 3), round(quaternion[1], 3), round(quaternion[2], 3),
                                     round(quaternion[3], 3))
    point = Position.Point(round(response.pose_stamped[1].pose.position.x, 3),
                           round(response.pose_stamped[1].pose.position.y, 3),
                           round(response.pose_stamped[1].pose.position.z, 3))
    rpy = Position.RPY(round(rpy[0], 3), round(rpy[1], 3), round(rpy[2], 3))
    rospy.loginfo("kinematic forward has been calculated ")
    return point, rpy, quaternion 

def activate_learning_mode(activate):
        try:
            rospy.wait_for_service('/niryo_one/activate_learning_mode', 1)
            srv = rospy.ServiceProxy('/niryo_one/activate_learning_mode', SetInt)
            resp = srv(int(activate))
            return resp.status == 200
        except (rospy.ServiceException, rospy.ROSException), e:
            return False 

def stop_robot_action():
        # Stop current move command
        try:
            rospy.wait_for_service('/niryo_one/commander/stop_command', 1)
            stop_cmd = rospy.ServiceProxy('/niryo_one/commander/stop_command', SetBool)
            stop_cmd()
        except (rospy.ServiceException, rospy.ROSException), e:
            pass 

def send_calibration_command():
        try:
            rospy.wait_for_service('/niryo_one/calibrate_motors', 0.1)
            start_calibration = rospy.ServiceProxy('/niryo_one/calibrate_motors', SetInt)
            start_calibration(1)  # 1 : calibration auto
        except (rospy.ServiceException, rospy.ROSException), e:
            return False 

def __calibrate(self, calib_type_int):
            """
            Call service to calibrate motors then wait for its end. If failed, raise NiryoOneException

            :param calib_type_int: 1 for auto-calibration & 2 for manual calibration
            :return: status, message
            :rtype: (GoalStatus, str)
            """
            result = self.call_service('niryo_one/calibrate_motors',
                                       SetInt, [calib_type_int])
            if result.status != OK:
                raise NiryoOneException(result.message)
            # Wait until calibration is finished
            rospy.sleep(1)
            calibration_finished = False
            while not calibration_finished:
                try:
                    hw_status = rospy.wait_for_message('niryo_one/hardware_status',
                                                       HardwareStatus, timeout=5)
                    if not hw_status.calibration_in_progress:
                        calibration_finished = True
                except rospy.ROSException as e:
                    raise NiryoOneException(str(e))

        # Calibration 

def digital_output_tool_setup(self, gpio_pin):
        try:
            rospy.wait_for_service('niryo_one/rpi/set_digital_io_mode', 2)
        except rospy.ROSException:
            return 400, "Digital IO panel service is not connected"
        try:
            resp = self.service_setup_digital_output_tool(gpio_pin, 0)  # set output
            return resp.status, resp.message
        except rospy.ServiceException, e:
            return 400, "Digital IO panel service failed" 

def digital_output_tool_activate(self, gpio_pin, activate):
        try:
            rospy.wait_for_service('niryo_one/rpi/set_digital_io_state', 2)
        except rospy.ROSException:
            return 400, "Digital IO panel service is not connected"
        try:
            resp = self.service_activate_digital_output_tool(gpio_pin, activate)
            return resp.status, resp.message
        except rospy.ServiceException, e:
            return 400, "Digital IO panel service failed" 

def send_trigger_sequence_autorun():
    rospy.loginfo("Trigger sequence autorun from button")
    try:
        rospy.wait_for_service('/niryo_one/sequences/trigger_sequence_autorun', 0.1)
        trigger = rospy.ServiceProxy('/niryo_one/sequences/trigger_sequence_autorun', SetInt)
        trigger(1)  # value doesn't matter, it will switch state on the server
    except (rospy.ServiceException, rospy.ROSException), e:
        return 

def send_shutdown_command():
    rospy.loginfo("SHUTDOWN")
    send_led_state(LedState.SHUTDOWN)
    rospy.loginfo("Activate learning mode")
    try:
        rospy.wait_for_service('/niryo_one/activate_learning_mode', 1)
    except rospy.ROSException, e:
        pass 

def send_reboot_command():
    rospy.loginfo("REBOOT")
    send_led_state(LedState.SHUTDOWN)
    rospy.loginfo("Activate learning mode")
    try:
        rospy.wait_for_service('/niryo_one/activate_learning_mode', 1)
    except rospy.ROSException, e:
        pass 

def service_request_pose(iksvc, raw_pose, side, blocking=True):
    
    ns = "ExternalTools/"+side+"/PositionKinematicsNode/IKService"
    ikreq = SolvePositionIKRequest()
    limb = baxter_interface.Limb(side)
    hdr = Header(stamp=rospy.Time.now(), frame_id='base')
    pose_stamped = PoseStamped( header = hdr, pose = raw_pose )
    
    ikreq.pose_stamp.append(pose_stamped)
    try:
        rospy.wait_for_service(ns, 5.0)
        resp = iksvc(ikreq)
    except (rospy.ServiceException, rospy.ROSException), e:
        rospy.logerr("Service call failed: %s" % (e,))
        return 

def write(self,
              value):
        """
        Call Service and receive result directly in the property.
        Blocks during service-call.
        If service is not available, writes None into the property value and returns.

        * `value`: Either Request-Object for the service or dict containing the attributes of the Request
        """
        if super().write(value):
            if self.client:
                try:
                    rospy.wait_for_service(self.service_name, timeout=self.call_timeout)
                except rospy.ROSException:
                    logger.error(f'service {self.service_name} not available')
                    super().write(None)
                    return

                logger.debug(f"{self.id()} is sending request {str(value)} to service {self.service_name}")
                if isinstance(value, dict):
                    # value is dict {"param1": value1, "param2": value2}
                    result = self.client.call(**value)
                elif isinstance(value, tuple) or isinstance(value, list):
                    # value is ordered sequence (value1, value2)
                    result = self.client.call(*value)
                else:
                    # value should be of matching Request-Type
                    result = self.client.call(value)

                super().write(result)

            elif ROS1_AVAILABLE:
                logger.error(f"Request {str(value)} to service {self.service_name} "
                             f"cannot be sent because client was not registered in ROS1") 

def main():
    """
    main function
    """
    try:
        rospy.init_node("carla_waypoint_publisher", anonymous=True)
        # wait for ros-bridge to set up CARLA world
        rospy.loginfo("Waiting for CARLA world (topic: /carla/world_info)...")
        rospy.wait_for_message("/carla/world_info", CarlaWorldInfo, timeout=10.0)
    except rospy.ROSException:
        rospy.logerr("Error while waiting for world info!")
        sys.exit(1)

    host = rospy.get_param("/carla/host", "127.0.0.1")
    port = rospy.get_param("/carla/port", 2000)
    timeout = rospy.get_param("/carla/timeout", 2)

    rospy.loginfo("CARLA world available. Trying to connect to {host}:{port}".format(
        host=host, port=port))

    try:
        carla_client = carla.Client(host=host, port=port)
        carla_client.set_timeout(timeout)

        carla_world = carla_client.get_world()

        rospy.loginfo("Connected to Carla.")

        waypointConverter = CarlaToRosWaypointConverter(carla_world)

        rospy.spin()
        waypointConverter.destroy()
        del waypointConverter
        del carla_world
        del carla_client

    finally:
        rospy.loginfo("Done") 

def twist_received(self, twist):
        """
        receive twist and convert to carla vehicle control
        """
        control = CarlaEgoVehicleControl()
        if twist == Twist():
            # stop
            control.throttle = 0.
            control.brake = 1.
            control.steer = 0.
        else:
            if twist.linear.x > 0:
                control.throttle = min(TwistToVehicleControl.MAX_LON_ACCELERATION,
                                       twist.linear.x) / TwistToVehicleControl.MAX_LON_ACCELERATION
            else:
                control.reverse = True
                control.throttle = max(-TwistToVehicleControl.MAX_LON_ACCELERATION,
                                       twist.linear.x) / -TwistToVehicleControl.MAX_LON_ACCELERATION

            if twist.angular.z > 0:
                control.steer = -min(self.max_steering_angle, twist.angular.z) / \
                    self.max_steering_angle
            else:
                control.steer = -max(-self.max_steering_angle, twist.angular.z) / \
                    self.max_steering_angle
        try:
            self.pub.publish(control)
        except rospy.ROSException as e:
            if not rospy.is_shutdown():
                rospy.logwarn("Error while publishing control: {}".format(e)) 

def run(self):
        """
        main loop
        """
        # wait for ros-bridge to set up CARLA world
        rospy.loginfo("Waiting for CARLA world (topic: /carla/world_info)...")
        try:
            rospy.wait_for_message("/carla/world_info", CarlaWorldInfo, timeout=10.0)
        except rospy.ROSException:
            rospy.logerr("Error while waiting for world info!")
            sys.exit(1)
        rospy.loginfo("CARLA world available. Waiting for ego vehicle...")

        client = carla.Client(self.host, self.port)
        client.set_timeout(self.timeout)
        self.world = client.get_world()

        try:
            r = rospy.Rate(10)  # 10hz
            while not rospy.is_shutdown():
                self.find_ego_vehicle_actor()
                r.sleep()
        except rospy.ROSInterruptException:
            pass

# ==============================================================================
# -- main() --------------------------------------------------------------------
# ============================================================================== 

def run(self):
        """
        main loop
        """
        # wait for ros-bridge to set up CARLA world
        rospy.loginfo("Waiting for CARLA world (topic: /carla/world_info)...")
        try:
            rospy.wait_for_message("/carla/world_info", CarlaWorldInfo, timeout=10.0)
        except rospy.ROSException:
            rospy.logerr("Timeout while waiting for world info!")
            sys.exit(1)

        rospy.loginfo("CARLA world available. Spawn ego vehicle...")

        client = carla.Client(self.host, self.port)
        client.set_timeout(self.timeout)
        self.world = client.get_world()
        self.restart()
        try:
            rospy.spin()
        except rospy.ROSInterruptException:
            pass

# ==============================================================================
# -- main() --------------------------------------------------------------------
# ============================================================================== 

def ik_request(self, pose,
                   end_point='right_hand', joint_seed=None,
                   nullspace_goal=None, nullspace_gain=0.4,
                   allow_collision=False):
        """
        Inverse Kinematics request sent to IK Service

        @type pose: geometry_msgs.Pose
        @param pose: Cartesian pose of the end point
        @type end_point: string
        @param end_point: name of the end point (should be in URDF)
        @type joint_seed: dict({str:float})
        @param joint_seed: the joint angles for the initial IK guess (optional)
        @type nullspace_goal: dict({str:float})
        @param nullspace_goal: desired joints, or subset of joints, to bias the solver (optional)
        @type nullspace_gain: double
        @param nullspace_gain: gain used to bias toward the nullspace goal [0.0, 1.0] (optional)
        @type allow_collision: bool
        @param allow_collision: does not check if Ik solution is in collision
        @rtype: dict({str:float})
        @return: valid joint positions if exists.  False if no solution is found.
        """
        if not isinstance(pose, Pose):
            rospy.logerr('pose is not of type geometry_msgs.msgs.Pose')
            return False

        # Add desired pose for inverse kinematics
        ikreq = SolvePositionIKRequest()
        hdr = Header(stamp=rospy.Time.now(), frame_id='base')
        ikreq.pose_stamp.append(PoseStamped(header=hdr, pose=pose))
        ikreq.tip_names.append(end_point)

        # The joint seed is where the IK position solver starts its optimization
        if joint_seed is not None:
            ikreq.seed_mode = ikreq.SEED_USER
            seed = JointState()
            seed.name = joint_seed.keys()
            seed.position = joint_seed.values()
            ikreq.seed_angles.append(seed)

        # Once the primary IK task is solved, the solver will then try to bias the
        # the joint angles toward the goal joint configuration. The null space is
        # the extra degrees of freedom the joints can move without affecting the
        # primary IK task.
        if nullspace_goal is not None:
          ikreq.use_nullspace_goal.append(True)
          # The nullspace goal can either be the full set or subset of joint angles
          goal = JointState()
          goal.names = nullspace_goal.keys()
          goal.position = nullspace_goal.values()
          ikreq.nullspace_goal.append(goal)
          # The gain used to bias toward the nullspace goal. Must be [0.0, 1.0]
          # If empty, the default gain of 0.4 will be used
          ikreq.nullspace_gain.append(nullspace_gain)

        try:
            resp = self._iksvc(ikreq)
        except (rospy.ServiceException, rospy.ROSException), e:
            rospy.logerr("IK Service call failed: %s" % (e,))
            return False