Python rospy.ServiceException() Examples

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 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 _connect_to_sensor(self):
        """Connect to the sensor.
        """
        name = self._device_name
        try:
            # Check if device is actively in list
            rospy.wait_for_service('phoxi_camera/get_device_list')
            device_list = rospy.ServiceProxy('phoxi_camera/get_device_list', GetDeviceList)().out
            if not str(name) in device_list:
                logging.error('PhoXi sensor {} not in list of active devices'.format(name))
                return False

            success = rospy.ServiceProxy('phoxi_camera/connect_camera', ConnectCamera)(name).success
            if not success:
                logging.error('Could not connect to PhoXi sensor {}'.format(name))
                return False

            logging.debug('Connected to PhoXi Sensor {}'.format(name))
            return True

        except rospy.ServiceException as e:
            logging.error('Service call failed: {}'.format(e))
            return False 

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 on_button_reset_pressed(self):
    self._num_pattern_detections = 0
    self._calibration_output = ""
    self._update_required = True

    self.button_start.setEnabled( False )

    try:
      rospy.wait_for_service('dvs_calibration/reset', 1)
      try:
        reset_service = rospy.ServiceProxy('dvs_calibration/reset', Empty)
        resp = reset_service()
      except rospy.ServiceException, e:
        print "Service call failed: %s"%e
    except:
      print "service not available..."
      pass 

def reset(self):
        rospy.wait_for_service('gazebo/reset_simulation')
        try:
            self.reset_proxy()
        except (rospy.ServiceException) as e:
            print("gazebo/reset_simulation service call failed")

        data = None
        while data is None:
            try:
                data = rospy.wait_for_message('scan', LaserScan, timeout=5)
            except:
                pass

        if self.initGoal:
            self.goal_x, self.goal_y = self.respawn_goal.getPosition()
            self.initGoal = False

        self.goal_distance = self.getGoalDistace()
        state, done = self.getState(data)

        return np.asarray(state) 

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 reset(self):
        rospy.wait_for_service('gazebo/reset_simulation')
        try:
            self.reset_proxy()
        except (rospy.ServiceException) as e:
            print("gazebo/reset_simulation service call failed")

        data = None
        while data is None:
            try:
                data = rospy.wait_for_message('scan', LaserScan, timeout=5)
            except:
                pass

        if self.initGoal:
            self.goal_x, self.goal_y = self.respawn_goal.getPosition()
            self.initGoal = False

        self.goal_distance = self.getGoalDistace()
        state, done = self.getState(data)

        return np.asarray(state) 

def manage_controller(controller_name, port_namespace, controller_type, command, deps, start, stop, restart):
    try:
        controller = rospy.get_param(controller_name + '/controller')
        package_path = controller['package']
        module_name = controller['module']
        class_name = controller['type']
    except KeyError as ke:
        rospy.logerr('[%s] configuration error: could not find controller parameters on parameter server' % controller_name)
        sys.exit(1)
    except Exception as e:
        rospy.logerr('[%s]: %s' % (controller_name, e))
        sys.exit(1)
        
    if command.lower() == 'start':
        try:
            response = start(port_namespace, package_path, module_name, class_name, controller_name, deps)
            if response.success: rospy.loginfo(response.reason)
            else: rospy.logerr(response.reason)
        except rospy.ServiceException, e:
            rospy.logerr('Service call failed: %s' % e) 

def reset(self):
        rospy.wait_for_service('gazebo/reset_simulation')
        try:
            self.reset_proxy()
        except (rospy.ServiceException) as e:
            print("gazebo/reset_simulation service call failed")

        data = None
        while data is None:
            try:
                data = rospy.wait_for_message('scan', LaserScan, timeout=5)
            except:
                pass

        if self.initGoal:
            self.goal_x, self.goal_y = self.respawn_goal.getPosition()
            self.initGoal = False

        self.goal_distance = self.getGoalDistace()
        state, done = self.getState(data)

        return np.asarray(state) 

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 _return_home(self):
        print(self.namespace, 'land')
        req = CommandTOLRequest()
        req.min_pitch = 0.0
        req.yaw = -math.pi if self.color == 'blue' else 0.0
        req.latitude = self.start_position.latitude
        req.longitude = self.start_position.longitude
        req.altitude = self.start_position.altitude

        service_name = '%s/mavros/cmd/land' % self.namespace
        rospy.wait_for_service(service_name)
        try:
            service = rospy.ServiceProxy(service_name, CommandTOL)
            resp = service.call(req)
        except rospy.ServiceException as e:
            print(self.namespace, 'service call to land failed:', str(e),
                  file=sys.stderr)
            return False
        if not resp.success:
            print(self.namespace, 'failed to land', file=sys.stderr)
            return False
        print(self.namespace, 'landing')
        self._set_state(STATE_LANDING)
        return True 

def _arm(self):
        print(self.namespace, 'arming')
        service_name = '%s/mavros/cmd/arming' % self.namespace
        rospy.wait_for_service(service_name)
        try:
            service = rospy.ServiceProxy(service_name, CommandBool)
            resp = service(True)
        except rospy.ServiceException as e:
            print(self.namespace, 'service call to arm failed:', str(e),
                  file=sys.stderr)
            return False
        if not resp.success:
            print(self.namespace, 'failed to arm', file=sys.stderr)
            return False
        print(self.namespace, 'armed')
        return True 

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 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 __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 emo_client(str):
            rospy.wait_for_service("/roboy/cognition/face/emotion")
            try:
                show_emotion = rospy.ServiceProxy(
                    "/roboy/cognition/face/emotion", ShowEmotion)
                resp = show_emotion(str)
                if resp:
                    return True
            except rospy.ServiceException as e:
                print('Service call failed: ', e)

        # general states 

def switch_motors(onoff):
    rospy.wait_for_service('/switch_motors')
    try:
        p = rospy.ServiceProxy('/switch_motors', SwitchMotors)
        res = p(onoff)
        return res.accepted
    except rospy.ServiceException, e:
        print "Service call failed: %s"%e 

def compute_calibration(self):
        """
        Computes the calibration through the ViSP service and returns it.

        :rtype: easy_handeye.handeye_calibration.HandeyeCalibration
        """
        if len(self.samples) < HandeyeCalibrator.MIN_SAMPLES:
            rospy.logwarn("{} more samples needed! Not computing the calibration".format(
                HandeyeCalibrator.MIN_SAMPLES - len(self.samples)))
            return

        # Update data
        hand_world_samples, camera_marker_samples = self.get_visp_samples()

        if len(hand_world_samples.transforms) != len(camera_marker_samples.transforms):
            rospy.logerr("Different numbers of hand-world and camera-marker samples!")
            raise AssertionError

        rospy.loginfo("Computing from %g poses..." % len(self.samples))

        try:
            result = self.calibrate(camera_marker_samples, hand_world_samples)
            rospy.loginfo("Computed calibration: {}".format(str(result)))
            transl = result.effector_camera.translation
            rot = result.effector_camera.rotation
            result_tuple = ((transl.x, transl.y, transl.z),
                            (rot.x, rot.y, rot.z, rot.w))

            ret = HandeyeCalibration(self.eye_on_hand,
                                     self.robot_base_frame,
                                     self.robot_effector_frame,
                                     self.tracking_base_frame,
                                     result_tuple)

            return ret

        except rospy.ServiceException as ex:
            rospy.logerr("Calibration failed: " + str(ex))
            return None 

def pos_control(left_hz,right_hz,time_ms):
    rospy.wait_for_service('/put_motor_freqs')
    try:
        p = rospy.ServiceProxy('/put_motor_freqs', PutMotorFreqs)
        res = p(left_hz,right_hz,time_ms)
        return res.accepted
    except rospy.ServiceException, e:
        print "Service call failed: %s"%e 

def recognize_srv_callback(self, req):
        """
        Method callback for the Recognize.srv
        :param req: The service request
        """
        self._response.recognitions = []
        self._recognizing = True

        try:
            cv_image = self.bridge.imgmsg_to_cv2(req.image, "bgr8")
        except CvBridgeError as e:
            rospy.logerr(e)

        self._image_widget.set_image(cv_image)
        self._done_recognizing_button.setDisabled(False)

        timeout = 60.0  # Maximum of 60 seconds
        future = rospy.Time.now() + rospy.Duration(timeout)
        rospy.loginfo("Waiting for manual recognition, maximum of %d seconds", timeout)
        while not rospy.is_shutdown() and self._recognizing:
            if rospy.Time.now() > future:
                raise rospy.ServiceException("Timeout of %d seconds exceeded .." % timeout)
            rospy.sleep(rospy.Duration(0.1))

        self._done_recognizing_button.setDisabled(True)

        return self._response 

def __init__(self):
        super(Robobo, self).__init__()
        # Duration of the "FORWARD" action
        self.time_forward = 1.7

        self.speed = 10
        # Angle that robobo achieve in one second
        # at a given speed
        self.angle_offset = 38
        # Degree per s when turning after the 1st second
        # at a given speed
        # From calibration
        self.angle_coeff = 50
        # Robobo's position on the grid
        self.position = [0, 0]

        # Initialize the different for moving the robot
        self.directions = {'left': 90, 'right': -90}
        self.faces = ['west', 'north', 'east']
        self.current_face_idx = 1
        self.yaw_error = 0
        self.yaw_target = 0
        self.yaw_north = 0
        self.yaw = 0
        self.left_encoder_pos = 0
        self.right_encoder_pos = 0
        self.angles = {}

        # Attempt connection to Robobo's service
        try:
            self.robobo_command = rospy.ServiceProxy('/command', Command)
        except rospy.ServiceException as e:
            print("Service exception", str(e))
            exit(1)

        self.status_sub = rospy.Subscriber("/status", Status, self.statusCallback) 

def mln_interface_client(query, config=None):
    rospy.wait_for_service('mln_interface')
    try:
        mln_interface = rospy.ServiceProxy('mln_interface', MLNInterface)
        resp1 = mln_interface(query, config)
        return resp1.response
    except rospy.ServiceException, e:
        print('Service call failed: %s'%e) 

def mln_interface_client(query, config=None):
    rospy.wait_for_service('mln_interface')
    try:
        mln_interface = rospy.ServiceProxy('mln_interface', MLNInterface)
        resp1 = mln_interface(query, config)
        return resp1.response.results
    except rospy.ServiceException, e:
        print "Service call failed: %s"%e 

def rigid_transform_from_ros(from_frame, to_frame, service_name='rigid_transforms/rigid_transform_listener', namespace=None):
        """Gets transform from ROS as a rigid transform
        
        Requires ROS rigid_transform_publisher service to be running. Assuming autolab_core is installed as a catkin package,
        this can be done with: roslaunch autolab_core rigid_transforms.launch
        
        Parameters
        ----------
        from_frame : :obj:`str`
        to_frame : :obj:`str`
        service_name : string, optional
            RigidTransformListener service to interface with. If the RigidTransformListener services are started through
            rigid_transforms.launch it will be called rigid_transform_listener
        namespace : string, optional
            Namespace to prepend to transform_listener_service. If None, current namespace is prepended.
        
        Raises
        ------
        rospy.ServiceException
            If service call to rigid_transform_listener fails
        """
        if namespace == None:
            service_name = rospy.get_namespace() + service_name
        else:
            service_name = namespace + service_name
        
        rospy.wait_for_service(service_name, timeout = 10)
        listener = rospy.ServiceProxy(service_name, RigidTransformListener)
        
        ret = listener(from_frame, to_frame)
        
        quat = np.asarray([ret.w_rot, ret.x_rot, ret.y_rot, ret.z_rot])
        trans = np.asarray([ret.x_trans, ret.y_trans, ret.z_trans])
        
        rot = RigidTransform.rotation_from_quaternion(quat)
        
        return RigidTransform(rotation=rot, translation=trans, from_frame=from_frame, to_frame=to_frame) 

def delete_from_ros(self, service_name='rigid_transforms/rigid_transform_publisher', namespace=None):
        """Removes RigidTransform referencing from_frame and to_frame from ROS publisher.
        Note that this may not be this exact transform, but may that references the same frames (order doesn't matter)
        
        Also, note that it may take quite a while for the transform to disappear from rigid_transform_publisher's cache 
        
        Requires ROS rigid_transform_publisher service to be running. Assuming autolab_core is installed as a catkin package,
        this can be done with: roslaunch autolab_core rigid_transforms.launch
        
        Parameters
        ----------
        service_name : string, optional
            RigidTransformPublisher service to interface with. If the RigidTransformPublisher services are started through
            rigid_transforms.launch it will be called rigid_transform_publisher
        namespace : string, optional
            Namespace to prepend to transform_listener_service. If None, current namespace is prepended.
        
        Raises
        ------
        rospy.ServiceException
            If service call to rigid_transform_publisher fails
        """
        if namespace == None:
            service_name = rospy.get_namespace() + service_name
        else:
            service_name = namespace + service_name
            
        rospy.wait_for_service(service_name, timeout = 10)
        publisher = rospy.ServiceProxy(service_name, RigidTransformPublisher)
        
        publisher(0, 0, 0, 0, 0, 0, 0, self.from_frame, self.to_frame, 'delete') 

def publish_to_ros(self, mode='transform', service_name='rigid_transforms/rigid_transform_publisher', namespace=None):
        """Publishes RigidTransform to ROS
        If a transform referencing the same frames already exists in the ROS publisher, it is updated instead.
        This checking is not order sensitive
        
        Requires ROS rigid_transform_publisher service to be running. Assuming autolab_core is installed as a catkin package,
        this can be done with: roslaunch autolab_core rigid_transforms.launch
        
        Parameters
        ----------
        mode : :obj:`str`
            Mode in which to publish. In {'transform', 'frame'}
            Defaults to 'transform'
        service_name : string, optional
            RigidTransformPublisher service to interface with. If the RigidTransformPublisher services are started through
            rigid_transforms.launch it will be called rigid_transform_publisher
        namespace : string, optional
            Namespace to prepend to transform_listener_service. If None, current namespace is prepended.
        
        Raises
        ------
        rospy.ServiceException
            If service call to rigid_transform_publisher fails
        """
        if namespace == None:
            service_name = rospy.get_namespace() + service_name
        else:
            service_name = namespace + service_name
        
        rospy.wait_for_service(service_name, timeout = 10)
        publisher = rospy.ServiceProxy(service_name, RigidTransformPublisher)
        
        trans = self.translation
        rot = self.quaternion
        
        publisher(trans[0], trans[1], trans[2], rot[0], rot[1], rot[2], rot[3], self.from_frame, self.to_frame, mode) 

def request_qsrs(self, req):
        """Request to compute QSRs.

        :param req: Request message.
        :type req: qsr_lib.srv.RequestQSRsRequest
        :return: ROS service response.
        :rtype: qsr_lib.srv.RequestQSRsResponse
        """
        rospy.logdebug("Requesting QSRs...")
        try:
            proxy = rospy.ServiceProxy(self.service_topic_names["request"], RequestQSRs)
            res = proxy(req)
            return res
        except rospy.ServiceException, e:
            rospy.logwarn("Service call failed: %s"%e) 

def on_button_start_calibration_pressed(self):
    self.button_start.setEnabled( False )

    try:
      rospy.wait_for_service('dvs_calibration/start', 1)
      try:
        start_calibration_service = rospy.ServiceProxy('dvs_calibration/start', Empty)
        resp = start_calibration_service()
      except rospy.ServiceException, e:
        print "Service call failed: %s"%e
    except:
      print "service not available..."
      pass 

def RequestActiveDMP(dmps):
    try:
        sad = rospy.ServiceProxy('set_active_dmp', SetActiveDMP)
        sad(dmps)
    except rospy.ServiceException, e:
        print "Service call failed: %s"%e